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/robot_model/robot_model.h>
00041 #include <moveit_msgs/RobotTrajectory.h>
00042 #include <sensor_msgs/JointState.h>
00043 #include <std_msgs/String.h>
00044 #include <ros/ros.h>
00045 #include <moveit/controller_manager/controller_manager.h>
00046 #include <boost/thread.hpp>
00047 #include <pluginlib/class_loader.h>
00048 #include <boost/scoped_ptr.hpp>
00049 
00050 namespace trajectory_execution_manager
00051 {
00052 
00053 // Two modes:
00054 // Managed controllers
00055 // Unmanaged controllers: given the trajectory,
00056 class TrajectoryExecutionManager
00057 {
00058 public:
00059 
00060   static const std::string EXECUTION_EVENT_TOPIC;
00061 
00063   typedef boost::function<void(const moveit_controller_manager::ExecutionStatus&)> ExecutionCompleteCallback;
00064 
00066   typedef boost::function<void(std::size_t)> PathSegmentCompleteCallback;
00067 
00069   struct TrajectoryExecutionContext
00070   {
00072     std::vector<std::string> controllers_;
00073 
00074     // The trajectory to execute, split in different parts (by joints), each set of joints corresponding to one controller
00075     std::vector<moveit_msgs::RobotTrajectory> trajectory_parts_;
00076   };
00077 
00079   TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel);
00080 
00082   TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, bool manage_controllers);
00083 
00085   ~TrajectoryExecutionManager();
00086 
00088   bool isManagingControllers() const;
00089 
00091   const moveit_controller_manager::MoveItControllerManagerPtr& getControllerManager() const;
00092 
00094   void processEvent(const std::string &event);
00095 
00098   bool ensureActiveControllersForGroup(const std::string &group);
00099 
00102   bool ensureActiveControllersForJoints(const std::vector<std::string> &joints);
00103 
00106   bool ensureActiveController(const std::string &controller);
00107 
00110   bool ensureActiveControllers(const std::vector<std::string> &controllers);
00111 
00113   bool isControllerActive(const std::string &controller);
00114 
00116   bool areControllersActive(const std::vector<std::string> &controllers);
00117 
00119   bool push(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller = "");
00120 
00122   bool push(const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller = "");
00123 
00127   bool push(const trajectory_msgs::JointTrajectory &trajectory, const std::vector<std::string> &controllers);
00128 
00132   bool push(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers);
00133 
00135   const std::vector<TrajectoryExecutionContext*>& getTrajectories() const;
00136 
00138   void execute(const ExecutionCompleteCallback &callback = ExecutionCompleteCallback(), bool auto_clear = true);
00139 
00141   void execute(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear = true);
00142 
00144   moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear = true);
00145 
00147   bool pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller = "");
00148 
00150   bool pushAndExecute(const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller = "");
00151 
00154   bool pushAndExecute(const sensor_msgs::JointState &state, const std::string &controller = "");
00155 
00159   bool pushAndExecute(const trajectory_msgs::JointTrajectory &trajectory, const std::vector<std::string> &controllers);
00160 
00164   bool pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers);
00165 
00169   bool pushAndExecute(const sensor_msgs::JointState &state, const std::vector<std::string> &controllers);
00170 
00172   moveit_controller_manager::ExecutionStatus waitForExecution();
00173 
00177   std::pair<int, int> getCurrentExpectedTrajectoryIndex() const;
00178 
00180   moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const;
00181 
00183   void stopExecution(bool auto_clear = true);
00184 
00186   void clear();
00187 
00190   void enableExecutionDurationMonitoring(bool flag);
00191 
00194   void setAllowedExecutionDurationScaling(double scaling);
00195 
00198   void setExecutionVelocityScaling(double scaling);
00199 
00200 private:
00201 
00202   struct ControllerInformation
00203   {
00204     std::string name_;
00205     std::set<std::string> joints_;
00206     std::set<std::string> overlapping_controllers_;
00207     moveit_controller_manager::MoveItControllerManager::ControllerState state_;
00208     ros::Time last_update_;
00209 
00210     bool operator<(ControllerInformation &other) const
00211     {
00212       if (joints_.size() != other.joints_.size())
00213         return joints_.size() < other.joints_.size();
00214       return name_ < other.name_;
00215     }
00216   };
00217 
00218   void initialize();
00219 
00220   void reloadControllerInformation();
00221 
00222   bool configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers);
00223 
00224   void updateControllersState(const ros::Duration &age);
00225   void updateControllerState(const std::string &controller, const ros::Duration &age);
00226   void updateControllerState(ControllerInformation &ci, const ros::Duration &age);
00227 
00228   bool distributeTrajectory(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers, std::vector<moveit_msgs::RobotTrajectory> &parts);
00229 
00230   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);
00231   bool checkControllerCombination(std::vector<std::string> &controllers, const std::set<std::string> &actuated_joints);
00232   void generateControllerCombination(std::size_t start_index, std::size_t controller_count, const std::vector<std::string> &available_controllers,
00233                                      std::vector<std::string> &selected_controllers, std::vector< std::vector<std::string> > &selected_options,
00234                                      const std::set<std::string> &actuated_joints);
00235   bool selectControllers(const std::set<std::string> &actuated_joints, const std::vector<std::string> &available_controllers, std::vector<std::string> &selected_controllers);
00236 
00237   void executeThread(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear);
00238   bool executePart(std::size_t part_index);
00239   void continuousExecutionThread();
00240 
00241 
00242   void stopExecutionInternal();
00243 
00244   void receiveEvent(const std_msgs::StringConstPtr &event);
00245 
00246   robot_model::RobotModelConstPtr robot_model_;
00247   ros::NodeHandle node_handle_;
00248   ros::NodeHandle root_node_handle_;
00249   ros::Subscriber event_topic_subscriber_;
00250 
00251   std::map<std::string, ControllerInformation> known_controllers_;
00252   bool manage_controllers_;
00253 
00254   // thread used to execute trajectories using the execute() command
00255   boost::scoped_ptr<boost::thread> execution_thread_;
00256 
00257   // thread used to execute trajectories using pushAndExecute()
00258   boost::scoped_ptr<boost::thread> continuous_execution_thread_;
00259 
00260   boost::mutex execution_state_mutex_;
00261   boost::mutex continuous_execution_mutex_;
00262 
00263   boost::condition_variable continuous_execution_condition_;
00264 
00265   // this condition is used to notify the completion of execution for given trajectories
00266   boost::condition_variable execution_complete_condition_;
00267 
00268   moveit_controller_manager::ExecutionStatus last_execution_status_;
00269   std::vector<moveit_controller_manager::MoveItControllerHandlePtr> active_handles_;
00270   int current_context_;
00271   std::vector<ros::Time> time_index_;
00272   mutable boost::mutex time_index_mutex_;
00273   bool execution_complete_;
00274 
00275   bool stop_continuous_execution_;
00276   bool run_continuous_execution_thread_;
00277   std::vector<TrajectoryExecutionContext*> trajectories_;
00278   std::deque<TrajectoryExecutionContext*> continuous_execution_queue_;
00279 
00280   boost::scoped_ptr<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> > controller_manager_loader_;
00281   moveit_controller_manager::MoveItControllerManagerPtr controller_manager_;
00282 
00283   bool verbose_;
00284 
00285   class DynamicReconfigureImpl;
00286   DynamicReconfigureImpl *reconfigure_impl_;
00287 
00288   bool execution_duration_monitoring_;
00289   double allowed_execution_duration_scaling_;
00290   double allowed_goal_duration_margin_;
00291   double execution_velocity_scaling_;
00292 };
00293 
00294 typedef boost::shared_ptr<TrajectoryExecutionManager> TrajectoryExecutionManagerPtr;
00295 typedef boost::shared_ptr<const TrajectoryExecutionManager> TrajectoryExecutionManagerConstPtr;
00296 
00297 }
00298 
00299 #endif


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:31