Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00054
00055
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
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
00255 boost::scoped_ptr<boost::thread> execution_thread_;
00256
00257
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
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