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
00037 #include <ros/ros.h>
00038 #include <boost/function.hpp>
00039 #include <trajectory_msgs/JointTrajectory.h>
00040 #include <trajectory_execution_monitor/trajectory_recorder.h>
00041 #include <trajectory_execution_monitor/trajectory_controller_handler.h>
00042 #include <planning_environment/models/collision_models.h>
00043
00044 namespace trajectory_execution_monitor
00045 {
00046
00048 struct TrajectoryExecutionRequest {
00049
00050 std::string group_name_;
00051 std::string controller_name_;
00052 std::string recorder_name_;
00053
00054 bool monitor_overshoot_;
00055 double max_overshoot_velocity_epsilon_;
00056 ros::Duration min_overshoot_time_;
00057 ros::Duration max_overshoot_time_;
00058
00059 bool failure_ok_;
00060 bool test_for_close_enough_;
00061 double max_joint_distance_;
00062 double failure_time_factor_;
00063
00064 trajectory_msgs::JointTrajectory trajectory_;
00065 boost::function<void(const std::string& group_name)> callback_function_;
00066
00067 TrajectoryExecutionRequest();
00068 };
00069
00071 enum TrajectoryExecutionResult {
00072 NOT_ATTEMPTED = 0,
00073 SUCCEEDED,
00074 NO_RECORDER,
00075 NO_HANDLER,
00076 ALREADY_AT_GOAL,
00077 HANDLER_FAILED_ENTIRELY,
00078 HANDLER_REPORTS_FAILURE,
00079 HANDLER_REPORTS_FAILURE_BUT_OK,
00080 HANDLER_REPORTS_FAILURE_BUT_CLOSE_ENOUGH
00081 };
00082
00084 struct TrajectoryExecutionData {
00085
00087 TrajectoryExecutionResult result_;
00088
00090 trajectory_msgs::JointTrajectory recorded_trajectory_;
00093 trajectory_msgs::JointTrajectory overshoot_trajectory_;
00094
00096 ros::Duration time_;
00098 ros::Duration overshoot_time_;
00101 double angular_distance_;
00102 };
00103
00105 struct TrajectoryExecutionDataVector : public std::vector<TrajectoryExecutionData>
00106 {
00107
00108 void reset() {
00109 clear();
00110 last_attempted_trajectory_index_ = 0;
00111 }
00112
00113 unsigned int last_attempted_trajectory_index_;
00114 };
00115
00117 class TrajectoryExecutionMonitor
00118 {
00119
00120 public:
00121
00122 TrajectoryExecutionMonitor() : cm_("robot_description"){};
00123
00125 void addTrajectoryRecorder(boost::shared_ptr<TrajectoryRecorder>& trajectory_recorder);
00126
00128 void addTrajectoryControllerHandler(boost::shared_ptr<TrajectoryControllerHandler>& trajectory_controller_handler);
00129
00132 void executeTrajectories(const std::vector<TrajectoryExecutionRequest>& to_execute,
00133 const boost::function<bool(TrajectoryExecutionDataVector)>& done_callback);
00134
00135 protected:
00136
00137 bool sendTrajectory(const TrajectoryExecutionRequest& ter);
00138
00140 void trajectoryFinishedCallbackFunction( TrajectoryControllerState controller_state );
00141
00143 bool closeEnough(const TrajectoryExecutionRequest& ter,
00144 const TrajectoryExecutionData& ted);
00145
00147 void compareLastRecordedToStart(const TrajectoryExecutionRequest& last_ter,
00148 const TrajectoryExecutionRequest& next_ter,
00149 const TrajectoryExecutionData& ted);
00150
00151
00152 boost::function<bool(TrajectoryExecutionDataVector)> result_callback_;
00153 TrajectoryExecutionDataVector execution_result_vector_;
00154 const std::vector<TrajectoryExecutionRequest>* execution_data_;
00155 unsigned int current_trajectory_index_;
00156
00157 boost::shared_ptr<TrajectoryControllerHandler> last_requested_handler_;
00158 std::map<std::string, boost::shared_ptr<TrajectoryRecorder> > trajectory_recorder_map_;
00159 std::map<std::string, boost::shared_ptr<TrajectoryControllerHandler> > trajectory_controller_handler_map_;
00160
00161 planning_environment::CollisionModels cm_;
00162
00163 };
00164
00165 }