trajectory_execution_manager.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_
00038 #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_
00039 
00040 #include <moveit/macros/class_forward.h>
00041 #include <moveit/robot_model/robot_model.h>
00042 #include <moveit/planning_scene_monitor/current_state_monitor.h>
00043 #include <moveit_msgs/RobotTrajectory.h>
00044 #include <sensor_msgs/JointState.h>
00045 #include <std_msgs/String.h>
00046 #include <ros/ros.h>
00047 #include <moveit/controller_manager/controller_manager.h>
00048 #include <moveit/macros/deprecation.h>
00049 #include <boost/thread.hpp>
00050 #include <pluginlib/class_loader.h>
00051 #include <boost/scoped_ptr.hpp>
00052 
00053 namespace trajectory_execution_manager
00054 {
00055 MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager);
00056 
00057 // Two modes:
00058 // Managed controllers
00059 // Unmanaged controllers: given the trajectory,
00060 class TrajectoryExecutionManager
00061 {
00062 public:
00063   static const std::string EXECUTION_EVENT_TOPIC;
00064 
00067   typedef boost::function<void(const moveit_controller_manager::ExecutionStatus&)> ExecutionCompleteCallback;
00068 
00071   typedef boost::function<void(std::size_t)> PathSegmentCompleteCallback;
00072 
00074   struct TrajectoryExecutionContext
00075   {
00077     std::vector<std::string> controllers_;
00078 
00079     // The trajectory to execute, split in different parts (by joints), each set of joints corresponding to one
00080     // controller
00081     std::vector<moveit_msgs::RobotTrajectory> trajectory_parts_;
00082   };
00083 
00085   MOVEIT_DEPRECATED TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel);  // switch to following
00086                                                                                                 // constructor!
00087   TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
00088                              const planning_scene_monitor::CurrentStateMonitorPtr& csm);
00089 
00091   MOVEIT_DEPRECATED TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
00092                                                bool manage_controllers);  // switch to following constructor!
00093   TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
00094                              const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers);
00095 
00097   ~TrajectoryExecutionManager();
00098 
00100   bool isManagingControllers() const;
00101 
00103   const moveit_controller_manager::MoveItControllerManagerPtr& getControllerManager() const;
00104 
00106   void processEvent(const std::string& event);
00107 
00112   bool ensureActiveControllersForGroup(const std::string& group);
00113 
00118   bool ensureActiveControllersForJoints(const std::vector<std::string>& joints);
00119 
00123   bool ensureActiveController(const std::string& controller);
00124 
00128   bool ensureActiveControllers(const std::vector<std::string>& controllers);
00129 
00131   bool isControllerActive(const std::string& controller);
00132 
00134   bool areControllersActive(const std::vector<std::string>& controllers);
00135 
00138   bool push(const moveit_msgs::RobotTrajectory& trajectory, const std::string& controller = "");
00139 
00142   bool push(const trajectory_msgs::JointTrajectory& trajectory, const std::string& controller = "");
00143 
00149   bool push(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<std::string>& controllers);
00150 
00156   bool push(const moveit_msgs::RobotTrajectory& trajectory, const std::vector<std::string>& controllers);
00157 
00159   const std::vector<TrajectoryExecutionContext*>& getTrajectories() const;
00160 
00162   void execute(const ExecutionCompleteCallback& callback = ExecutionCompleteCallback(), bool auto_clear = true);
00163 
00166   void execute(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback,
00167                bool auto_clear = true);
00168 
00171   moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear = true);
00172 
00175   bool pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory, const std::string& controller = "");
00176 
00179   bool pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory, const std::string& controller = "");
00180 
00184   bool pushAndExecute(const sensor_msgs::JointState& state, const std::string& controller = "");
00185 
00191   bool pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<std::string>& controllers);
00192 
00198   bool pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory, const std::vector<std::string>& controllers);
00199 
00205   bool pushAndExecute(const sensor_msgs::JointState& state, const std::vector<std::string>& controllers);
00206 
00209   moveit_controller_manager::ExecutionStatus waitForExecution();
00210 
00217   std::pair<int, int> getCurrentExpectedTrajectoryIndex() const;
00218 
00220   moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const;
00221 
00223   void stopExecution(bool auto_clear = true);
00224 
00226   void clear();
00227 
00230   void enableExecutionDurationMonitoring(bool flag);
00231 
00234   void setAllowedExecutionDurationScaling(double scaling);
00235 
00238   void setAllowedGoalDurationMargin(double margin);
00239 
00242   void setExecutionVelocityScaling(double scaling);
00243 
00245   void setAllowedStartTolerance(double tolerance);
00246 
00247 private:
00248   struct ControllerInformation
00249   {
00250     std::string name_;
00251     std::set<std::string> joints_;
00252     std::set<std::string> overlapping_controllers_;
00253     moveit_controller_manager::MoveItControllerManager::ControllerState state_;
00254     ros::Time last_update_;
00255 
00256     bool operator<(ControllerInformation& other) const
00257     {
00258       if (joints_.size() != other.joints_.size())
00259         return joints_.size() < other.joints_.size();
00260       return name_ < other.name_;
00261     }
00262   };
00263 
00264   void initialize();
00265 
00266   void reloadControllerInformation();
00267 
00269   bool validate(const TrajectoryExecutionContext& context) const;
00270   bool configure(TrajectoryExecutionContext& context, const moveit_msgs::RobotTrajectory& trajectory,
00271                  const std::vector<std::string>& controllers);
00272 
00273   void updateControllersState(const ros::Duration& age);
00274   void updateControllerState(const std::string& controller, const ros::Duration& age);
00275   void updateControllerState(ControllerInformation& ci, const ros::Duration& age);
00276 
00277   bool distributeTrajectory(const moveit_msgs::RobotTrajectory& trajectory, const std::vector<std::string>& controllers,
00278                             std::vector<moveit_msgs::RobotTrajectory>& parts);
00279 
00280   bool findControllers(const std::set<std::string>& actuated_joints, std::size_t controller_count,
00281                        const std::vector<std::string>& available_controllers,
00282                        std::vector<std::string>& selected_controllers);
00283   bool checkControllerCombination(std::vector<std::string>& controllers, const std::set<std::string>& actuated_joints);
00284   void generateControllerCombination(std::size_t start_index, std::size_t controller_count,
00285                                      const std::vector<std::string>& available_controllers,
00286                                      std::vector<std::string>& selected_controllers,
00287                                      std::vector<std::vector<std::string> >& selected_options,
00288                                      const std::set<std::string>& actuated_joints);
00289   bool selectControllers(const std::set<std::string>& actuated_joints,
00290                          const std::vector<std::string>& available_controllers,
00291                          std::vector<std::string>& selected_controllers);
00292 
00293   void executeThread(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback,
00294                      bool auto_clear);
00295   bool executePart(std::size_t part_index);
00296   bool waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time = 1.0);
00297   void continuousExecutionThread();
00298 
00299   void stopExecutionInternal();
00300 
00301   void receiveEvent(const std_msgs::StringConstPtr& event);
00302 
00303   robot_model::RobotModelConstPtr robot_model_;
00304   planning_scene_monitor::CurrentStateMonitorPtr csm_;
00305   ros::NodeHandle node_handle_;
00306   ros::NodeHandle root_node_handle_;
00307   ros::Subscriber event_topic_subscriber_;
00308 
00309   std::map<std::string, ControllerInformation> known_controllers_;
00310   bool manage_controllers_;
00311 
00312   // thread used to execute trajectories using the execute() command
00313   boost::scoped_ptr<boost::thread> execution_thread_;
00314 
00315   // thread used to execute trajectories using pushAndExecute()
00316   boost::scoped_ptr<boost::thread> continuous_execution_thread_;
00317 
00318   boost::mutex execution_state_mutex_;
00319   boost::mutex continuous_execution_mutex_;
00320 
00321   boost::condition_variable continuous_execution_condition_;
00322 
00323   // this condition is used to notify the completion of execution for given trajectories
00324   boost::condition_variable execution_complete_condition_;
00325 
00326   moveit_controller_manager::ExecutionStatus last_execution_status_;
00327   std::vector<moveit_controller_manager::MoveItControllerHandlePtr> active_handles_;
00328   int current_context_;
00329   std::vector<ros::Time> time_index_;
00330   mutable boost::mutex time_index_mutex_;
00331   bool execution_complete_;
00332 
00333   bool stop_continuous_execution_;
00334   bool run_continuous_execution_thread_;
00335   std::vector<TrajectoryExecutionContext*> trajectories_;
00336   std::deque<TrajectoryExecutionContext*> continuous_execution_queue_;
00337 
00338   boost::scoped_ptr<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> >
00339       controller_manager_loader_;
00340   moveit_controller_manager::MoveItControllerManagerPtr controller_manager_;
00341 
00342   bool verbose_;
00343 
00344   class DynamicReconfigureImpl;
00345   DynamicReconfigureImpl* reconfigure_impl_;
00346 
00347   bool execution_duration_monitoring_;
00348   double allowed_execution_duration_scaling_;
00349   double allowed_goal_duration_margin_;
00350   double allowed_start_tolerance_;  // joint tolerance for validate(): radians for revolute joints
00351   double execution_velocity_scaling_;
00352 };
00353 }
00354 
00355 #endif


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Apr 23 2018 10:25:14