trajectory_execution_manager.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_
38 #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_
39 
43 #include <moveit_msgs/RobotTrajectory.h>
44 #include <sensor_msgs/JointState.h>
45 #include <std_msgs/String.h>
46 #include <ros/ros.h>
48 #include <boost/thread.hpp>
50 
51 #include <memory>
52 
54 {
56 
57 // Two modes:
58 // Managed controllers
59 // Unmanaged controllers: given the trajectory,
61 {
62 public:
63  static const std::string EXECUTION_EVENT_TOPIC;
64 
67  typedef boost::function<void(const moveit_controller_manager::ExecutionStatus&)> ExecutionCompleteCallback;
68 
71  typedef boost::function<void(std::size_t)> PathSegmentCompleteCallback;
72 
75  {
77  std::vector<std::string> controllers_;
78 
79  // The trajectory to execute, split in different parts (by joints), each set of joints corresponding to one
80  // controller
81  std::vector<moveit_msgs::RobotTrajectory> trajectory_parts_;
82  };
83 
85  TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
86  const planning_scene_monitor::CurrentStateMonitorPtr& csm);
87 
89  TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
90  const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers);
91 
94 
96  bool isManagingControllers() const;
97 
99  const moveit_controller_manager::MoveItControllerManagerPtr& getControllerManager() const;
100 
102  void processEvent(const std::string& event);
103 
108  bool ensureActiveControllersForGroup(const std::string& group);
109 
114  bool ensureActiveControllersForJoints(const std::vector<std::string>& joints);
115 
119  bool ensureActiveController(const std::string& controller);
120 
124  bool ensureActiveControllers(const std::vector<std::string>& controllers);
125 
127  bool isControllerActive(const std::string& controller);
128 
130  bool areControllersActive(const std::vector<std::string>& controllers);
131 
134  bool push(const moveit_msgs::RobotTrajectory& trajectory, const std::string& controller = "");
135 
138  bool push(const trajectory_msgs::JointTrajectory& trajectory, const std::string& controller = "");
139 
145  bool push(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<std::string>& controllers);
146 
152  bool push(const moveit_msgs::RobotTrajectory& trajectory, const std::vector<std::string>& controllers);
153 
155  const std::vector<TrajectoryExecutionContext*>& getTrajectories() const;
156 
158  void execute(const ExecutionCompleteCallback& callback = ExecutionCompleteCallback(), bool auto_clear = true);
159 
162  void execute(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback,
163  bool auto_clear = true);
164 
168 
171  bool pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory, const std::string& controller = "");
172 
175  bool pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory, const std::string& controller = "");
176 
180  bool pushAndExecute(const sensor_msgs::JointState& state, const std::string& controller = "");
181 
187  bool pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<std::string>& controllers);
188 
194  bool pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory, const std::vector<std::string>& controllers);
195 
201  bool pushAndExecute(const sensor_msgs::JointState& state, const std::vector<std::string>& controllers);
202 
206 
213  std::pair<int, int> getCurrentExpectedTrajectoryIndex() const;
214 
217 
219  void stopExecution(bool auto_clear = true);
220 
222  void clear();
223 
226  void enableExecutionDurationMonitoring(bool flag);
227 
230  void setAllowedExecutionDurationScaling(double scaling);
231 
234  void setAllowedGoalDurationMargin(double margin);
235 
238  void setExecutionVelocityScaling(double scaling);
239 
241  void setAllowedStartTolerance(double tolerance);
242 
243 private:
245  {
246  std::string name_;
247  std::set<std::string> joints_;
248  std::set<std::string> overlapping_controllers_;
251 
252  bool operator<(ControllerInformation& other) const
253  {
254  if (joints_.size() != other.joints_.size())
255  return joints_.size() < other.joints_.size();
256  return name_ < other.name_;
257  }
258  };
259 
260  void initialize();
261 
263 
265  bool validate(const TrajectoryExecutionContext& context) const;
266  bool configure(TrajectoryExecutionContext& context, const moveit_msgs::RobotTrajectory& trajectory,
267  const std::vector<std::string>& controllers);
268 
269  void updateControllersState(const ros::Duration& age);
270  void updateControllerState(const std::string& controller, const ros::Duration& age);
272 
273  bool distributeTrajectory(const moveit_msgs::RobotTrajectory& trajectory, const std::vector<std::string>& controllers,
274  std::vector<moveit_msgs::RobotTrajectory>& parts);
275 
276  bool findControllers(const std::set<std::string>& actuated_joints, std::size_t controller_count,
277  const std::vector<std::string>& available_controllers,
278  std::vector<std::string>& selected_controllers);
279  bool checkControllerCombination(std::vector<std::string>& controllers, const std::set<std::string>& actuated_joints);
280  void generateControllerCombination(std::size_t start_index, std::size_t controller_count,
281  const std::vector<std::string>& available_controllers,
282  std::vector<std::string>& selected_controllers,
283  std::vector<std::vector<std::string> >& selected_options,
284  const std::set<std::string>& actuated_joints);
285  bool selectControllers(const std::set<std::string>& actuated_joints,
286  const std::vector<std::string>& available_controllers,
287  std::vector<std::string>& selected_controllers);
288 
289  void executeThread(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback,
290  bool auto_clear);
291  bool executePart(std::size_t part_index);
292  bool waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time = 1.0);
294 
295  void stopExecutionInternal();
296 
297  void receiveEvent(const std_msgs::StringConstPtr& event);
298 
299  void loadControllerParams();
300 
301  // Name of this class for logging
302  const std::string name_ = "trajectory_execution_manager";
303 
304  robot_model::RobotModelConstPtr robot_model_;
305  planning_scene_monitor::CurrentStateMonitorPtr csm_;
309 
310  std::map<std::string, ControllerInformation> known_controllers_;
312 
313  // thread used to execute trajectories using the execute() command
314  std::unique_ptr<boost::thread> execution_thread_;
315 
316  // thread used to execute trajectories using pushAndExecute()
317  std::unique_ptr<boost::thread> continuous_execution_thread_;
318 
321 
323 
324  // this condition is used to notify the completion of execution for given trajectories
326 
328  std::vector<moveit_controller_manager::MoveItControllerHandlePtr> active_handles_;
330  std::vector<ros::Time> time_index_; // used to find current expected trajectory location
331  mutable boost::mutex time_index_mutex_;
333 
336  std::vector<TrajectoryExecutionContext*> trajectories_;
337  std::deque<TrajectoryExecutionContext*> continuous_execution_queue_;
338 
339  std::unique_ptr<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> >
341  moveit_controller_manager::MoveItControllerManagerPtr controller_manager_;
342 
343  bool verbose_;
344 
347 
349  // 'global' values
352  // controller-specific values
353  // override the 'global' values
354  std::map<std::string, double> controller_allowed_execution_duration_scaling_;
355  std::map<std::string, double> controller_allowed_goal_duration_margin_;
356 
357  double allowed_start_tolerance_; // joint tolerance for validate(): radians for revolute joints
359 };
360 }
361 
362 #endif
moveit_controller_manager::ExecutionStatus last_execution_status_
std::deque< TrajectoryExecutionContext * > continuous_execution_queue_
TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, const planning_scene_monitor::CurrentStateMonitorPtr &csm)
Load the controller manager plugin, start listening for events on a topic.
std::unique_ptr< pluginlib::ClassLoader< moveit_controller_manager::MoveItControllerManager > > controller_manager_loader_
bool distributeTrajectory(const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers, std::vector< moveit_msgs::RobotTrajectory > &parts)
bool ensureActiveControllersForJoints(const std::vector< std::string > &joints)
Make sure the active controllers are such that trajectories that actuate joints in the specified set ...
bool ensureActiveController(const std::string &controller)
Make sure a particular controller is active.
void generateControllerCombination(std::size_t start_index, std::size_t controller_count, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers, std::vector< std::vector< std::string > > &selected_options, const std::set< std::string > &actuated_joints)
bool waitForRobotToStop(const TrajectoryExecutionContext &context, double wait_time=1.0)
bool validate(const TrajectoryExecutionContext &context) const
Validate first point of trajectory matches current robot state.
bool pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller="")
MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager)
bool isManagingControllers() const
If this function returns true, then this instance of the manager is allowed to load/unload/switch con...
bool ensureActiveControllers(const std::vector< std::string > &controllers)
Make sure a particular set of controllers are active.
std::map< std::string, ControllerInformation > known_controllers_
const moveit_controller_manager::MoveItControllerManagerPtr & getControllerManager() const
Get the instance of the controller manager used (this is the plugin instance loaded) ...
boost::function< void(const moveit_controller_manager::ExecutionStatus &)> ExecutionCompleteCallback
bool checkControllerCombination(std::vector< std::string > &controllers, const std::set< std::string > &actuated_joints)
moveit_controller_manager::MoveItControllerManager::ControllerState state_
std::vector< std::string > controllers_
The controllers to use for executing the different trajectory parts;.
moveit_controller_manager::MoveItControllerManagerPtr controller_manager_
std::vector< std::vector< std::string > > selected_options
bool ensureActiveControllersForGroup(const std::string &group)
Make sure the active controllers are such that trajectories that actuate joints in the specified grou...
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
bool findControllers(const std::set< std::string > &actuated_joints, std::size_t controller_count, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers)
void execute(const ExecutionCompleteCallback &callback=ExecutionCompleteCallback(), bool auto_clear=true)
Start the execution of pushed trajectories; this does not wait for completion, but calls a callback w...
void processEvent(const std::string &event)
Execute a named event (e.g., &#39;stop&#39;)
void executeThread(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
bool configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers)
~TrajectoryExecutionManager()
Destructor. Cancels all running trajectories (if any)
moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear=true)
bool areControllersActive(const std::vector< std::string > &controllers)
Check if a set of controllers are active.
std::vector< moveit_controller_manager::MoveItControllerHandlePtr > active_handles_
moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const
Return the controller status for the last attempted execution.
void stopExecution(bool auto_clear=true)
Stop whatever executions are active, if any.
const std::vector< TrajectoryExecutionContext * > & getTrajectories() const
Get the trajectories to be executed.
void setAllowedStartTolerance(double tolerance)
Set joint-value tolerance for validating trajectory&#39;s start point against current robot state...
bool isControllerActive(const std::string &controller)
Check if a controller is active.
bool selectControllers(const std::set< std::string > &actuated_joints, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers)
Data structure that represents information necessary to execute a trajectory.
void updateControllerState(const std::string &controller, const ros::Duration &age)
bool push(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller="")


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 18 2018 02:49:00