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 #ifndef _TRAJECTORY_CONTROLLER_HANDLER_H_
00038 #define _TRAJECTORY_CONTROLLER_HANDLER_H_
00039
00040 #include <ros/ros.h>
00041 #include <boost/function.hpp>
00042 #include <trajectory_msgs/JointTrajectory.h>
00043 #include <trajectory_execution_monitor/trajectory_recorder.h>
00044
00045 namespace trajectory_execution_monitor
00046 {
00047
00048 namespace TrajectoryControllerStates
00049 {
00051 enum TrajectoryControllerState
00052 {
00053 IDLE = 0,
00054 PAUSED,
00055 EXECUTING,
00056 OVERSHOOTING,
00057 SUCCESS,
00058 OVERSHOOT_TIMEOUT,
00059 EXECUTION_FAILURE,
00060 EXECUTION_TIMEOUT,
00061 CANCELLED
00062 };
00063 }
00064 typedef TrajectoryControllerStates::TrajectoryControllerState TrajectoryControllerState;
00065
00067 typedef boost::function<void(TrajectoryControllerState)> TrajectoryFinishedCallbackFunction;
00068
00071 class TrajectoryControllerHandler {
00072
00073 public:
00074
00075 TrajectoryControllerHandler(const std::string& group_name,
00076 const std::string& controller_name) :
00077 group_name_(group_name),
00078 controller_name_(controller_name),
00079 monitor_overshoot_(false),
00080 controller_state_(TrajectoryControllerStates::IDLE),
00081 timeout_(ros::Duration(100))
00082 {
00083 group_controller_combo_name_ = combineGroupAndControllerNames(group_name,controller_name);
00084 };
00085
00086 virtual ~TrajectoryControllerHandler() {
00087
00088 }
00089
00090 static std::string combineGroupAndControllerNames(const std::string& group_name,
00091 const std::string& controller_name) {
00092 return(group_name+"_"+controller_name);
00093 }
00094
00097 bool enableOvershoot(
00098 double max_overshoot_velocity_epsilon,
00099 ros::Duration min_overshoot_time,
00100 ros::Duration max_overshoot_time);
00101
00103 void disableOvershoot();
00104
00106 void setMaximumExecutionTime( ros::Duration max_execution_time ) {
00107 timeout_ = max_execution_time;
00108 }
00109
00113 virtual bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory,
00114 boost::shared_ptr<TrajectoryRecorder>& recorder,
00115 const TrajectoryFinishedCallbackFunction& traj_callback) = 0;
00116
00118 virtual void cancelExecution() = 0;
00119
00121 void timeout(const ros::TimerEvent& event);
00122
00123 const trajectory_msgs::JointTrajectory& getLastGoalTrajectory() const {
00124 return goal_trajectory_;
00125 }
00126
00127 const trajectory_msgs::JointTrajectory& getLastRecordedTrajectory() const {
00128 return recorded_trajectory_;
00129 }
00130
00131 const trajectory_msgs::JointTrajectory& getLastOvershootTrajectory() const {
00132 return overshoot_trajectory_;
00133 }
00134
00135 const std::string& getGroupName() const {
00136 return group_name_;
00137 }
00138
00139 const std::string& getControllerName() const {
00140 return controller_name_;
00141 }
00142
00143 const std::string& getGroupControllerComboName() const {
00144 return group_controller_combo_name_;
00145 }
00146
00147 protected:
00148
00149 bool addNewStateToRecordedTrajectory(const ros::Time& time,
00150 const std::map<std::string, double>& joint_positions,
00151 const std::map<std::string, double>& joint_velocities);
00152 bool _addNewStateToTrajectory(const ros::Time& time,
00153 const std::map<std::string, double>& joint_positions,
00154 const std::map<std::string, double>& joint_velocities,
00155 trajectory_msgs::JointTrajectory& trajectory);
00156
00159 void done();
00162 void doneDelayed();
00163
00164 void initializeRecordedTrajectory(const trajectory_msgs::JointTrajectory& goal_trajectory);
00165 void initializeOvershootTrajectory();
00166
00167 unsigned int findClosestIndex( ros::Duration time_from_start );
00168
00169 std::string group_name_;
00170 std::string controller_name_;
00171 std::string group_controller_combo_name_;
00172
00173 trajectory_msgs::JointTrajectory recorded_trajectory_;
00174 trajectory_msgs::JointTrajectory overshoot_trajectory_;
00175 trajectory_msgs::JointTrajectory goal_trajectory_;
00176
00177 bool monitor_overshoot_;
00178 double max_overshoot_velocity_epsilon_;
00179 ros::Duration min_overshoot_time_;
00180 ros::Duration max_overshoot_time_;
00181
00182 boost::shared_ptr<trajectory_execution_monitor::TrajectoryRecorder> recorder_;
00183 trajectory_execution_monitor::TrajectoryFinishedCallbackFunction trajectory_finished_callback_;
00184 TrajectoryControllerState controller_state_;
00185
00186
00187 ros::Duration timeout_;
00188 ros::NodeHandle nh_;
00189 ros::Timer timer_;
00190
00191
00192
00193 };
00194
00195 }
00196
00197 #endif