moveit2
The MoveIt Motion Planning Framework for ROS 2.
locked_robot_state.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * Copyright (c) 2014, SRI International
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan, Acorn Pooley */
37 
38 #pragma once
39 
42 
43 #include <functional>
44 #include <thread>
45 
46 namespace robot_interaction
47 {
48 MOVEIT_CLASS_FORWARD(LockedRobotState); // Defines LockedRobotStatePtr, ConstPtr, WeakPtr... etc
49 
51 //
52 // Only allow one thread to modify the RobotState at a time.
53 //
54 // Allow any thread access to the RobotState when it is not being modified. If
55 // a (const) reference to the robot state is held when the RobotState needs to
56 // be modified, a copy is made and the copy is modified. Any externally held
57 // references will be out of date but still valid.
58 //
59 // The RobotState can only be modified by passing a callback function which
60 // does the modification.
62 {
63 public:
65  LockedRobotState(const moveit::core::RobotModelPtr& model);
66 
67  virtual ~LockedRobotState();
68 
70  //
71  // This state may go stale, meaning the maintained state has been updated,
72  // but it will never be modified while the caller is holding a reference to
73  // it.
74  //
75  // The transforms in the returned state will always be up to date.
76  moveit::core::RobotStateConstPtr getState() const;
77 
79  void setState(const moveit::core::RobotState& state);
80 
81  // This is a function that can modify the maintained state.
82  typedef std::function<void(moveit::core::RobotState*)> ModifyStateFunction;
83 
84  // Modify the state.
85  //
86  // This modifies the state by calling \e modify on it.
87  // The \e modify function is passed a reference to the state which it can
88  // modify. No threads will be given access to the state while the \e modify
89  // function is running.
90  void modifyState(const ModifyStateFunction& modify);
91 
92 protected:
93  // This is called when the internally maintained state has changed.
94  // This is called with state_lock_ unlocked.
95  // Default definition does nothing. Override to get notification of state
96  // change.
97  // TODO: is this needed?
98  virtual void robotStateChanged();
99 
100 protected:
101  // this locks all accesses to the state_ member.
102  // The lock can also be used by subclasses to lock additional fields.
103  mutable std::mutex state_lock_;
104 
105 private:
106  // The state maintained by this class.
107  // When a modify function is being called this is nullptr.
108  // PROTECTED BY state_lock_
109  moveit::core::RobotStatePtr state_;
110 };
111 } // namespace robot_interaction
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
Maintain a RobotState in a multithreaded environment.
moveit::core::RobotStateConstPtr getState() const
get read-only access to the state.
LockedRobotState(const moveit::core::RobotState &state)
void modifyState(const ModifyStateFunction &modify)
std::function< void(moveit::core::RobotState *)> ModifyStateFunction
void setState(const moveit::core::RobotState &state)
Set the state to the new value.
MOVEIT_CLASS_FORWARD(InteractionHandler)