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 <trajectory_execution_monitor/trajectory_controller_handler.h>
00038 #include <trajectory_execution_monitor/trajectory_stats.h>
00039
00040 using namespace trajectory_execution_monitor;
00041
00042 unsigned int TrajectoryControllerHandler::findClosestIndex( ros::Duration time_from_start )
00043 {
00044 for(unsigned int i=0; i<overshoot_trajectory_.points.size(); i++)
00045 {
00046 trajectory_msgs::JointTrajectoryPoint& p = overshoot_trajectory_.points[i];
00047
00048 if( p.time_from_start > time_from_start )
00049 return i;
00050 }
00051
00052 return overshoot_trajectory_.points.size()-1;
00053 }
00054
00055 bool TrajectoryControllerHandler::addNewStateToRecordedTrajectory(const ros::Time& time,
00056 const std::map<std::string, double>& joint_positions,
00057 const std::map<std::string, double>& joint_velocities)
00058 {
00059 if( controller_state_ == TrajectoryControllerStates::OVERSHOOTING )
00060 {
00061 ros::Duration dur = time - overshoot_trajectory_.header.stamp;
00062 if( dur > min_overshoot_time_ )
00063 {
00064
00065 unsigned int closest_index = findClosestIndex( dur-min_overshoot_time_ );
00066 double max_vel = TrajectoryStats::getMaxAngularVelocity(overshoot_trajectory_, closest_index);
00067
00068 if( max_vel <= max_overshoot_velocity_epsilon_ )
00069 {
00070 controller_state_ = TrajectoryControllerStates::SUCCESS;
00071 doneDelayed();
00072 return false;
00073 }
00074 }
00075 }
00076
00077 if( controller_state_ == TrajectoryControllerStates::EXECUTING )
00078 {
00079 return _addNewStateToTrajectory(time, joint_positions, joint_velocities, recorded_trajectory_);
00080 }
00081 else if( controller_state_ == TrajectoryControllerStates::OVERSHOOTING )
00082 {
00083 return _addNewStateToTrajectory(time, joint_positions, joint_velocities, overshoot_trajectory_);
00084 }
00085 return false;
00086 }
00087
00088 bool TrajectoryControllerHandler::_addNewStateToTrajectory(const ros::Time& time,
00089 const std::map<std::string, double>& joint_positions,
00090 const std::map<std::string, double>& joint_velocities,
00091 trajectory_msgs::JointTrajectory& trajectory)
00092 {
00093 ros::Time start_time;
00094
00095 trajectory_msgs::JointTrajectoryPoint p;
00096 for(unsigned int i = 0; i < trajectory.joint_names.size(); i++) {
00097 const std::string& jn = trajectory.joint_names[i];
00098 if(joint_positions.find(jn) == joint_positions.end()) {
00099 return false;
00100 }
00101 p.positions.push_back(joint_positions.at(jn));
00102 if(joint_velocities.find(jn) == joint_velocities.end()) {
00103 p.velocities.push_back(joint_velocities.at(jn));
00104 }
00105 p.time_from_start = time-trajectory.header.stamp;
00106 }
00107 trajectory.points.push_back(p);
00108
00109 return true;
00110 }
00111
00112 bool TrajectoryControllerHandler::enableOvershoot(
00113 double max_overshoot_velocity_epsilon,
00114 ros::Duration min_overshoot_time,
00115 ros::Duration max_overshoot_time )
00116 {
00117 monitor_overshoot_ = true;
00118 max_overshoot_velocity_epsilon_ = max_overshoot_velocity_epsilon;
00119 min_overshoot_time_ = min_overshoot_time;
00120 max_overshoot_time_ = max_overshoot_time;
00121 return true;
00122 }
00123
00124 void TrajectoryControllerHandler::disableOvershoot()
00125 {
00126 monitor_overshoot_ = false;
00127 }
00128
00129
00130 void TrajectoryControllerHandler::timeout(const ros::TimerEvent& event)
00131 {
00132 if( controller_state_ == TrajectoryControllerStates::OVERSHOOTING )
00133 {
00134 ROS_ERROR("overshoot exceeded %f seconds", max_overshoot_time_.toSec());
00135 controller_state_ = TrajectoryControllerStates::OVERSHOOT_TIMEOUT;
00136 done();
00137 }
00138 else if( controller_state_ == TrajectoryControllerStates::EXECUTING )
00139 {
00140 ROS_ERROR("Execution exceeded %f seconds", timeout_.toSec() );
00141 controller_state_ = TrajectoryControllerStates::EXECUTION_TIMEOUT;
00142 done();
00143 }
00144 }
00145
00146 void TrajectoryControllerHandler::initializeRecordedTrajectory(const trajectory_msgs::JointTrajectory& goal_trajectory)
00147 {
00148 goal_trajectory_ = goal_trajectory;
00149
00150 recorded_trajectory_.header.stamp = ros::Time::now();
00151 recorded_trajectory_.joint_names = goal_trajectory.joint_names;
00152 recorded_trajectory_.points.clear();
00153
00154 controller_state_ = TrajectoryControllerStates::EXECUTING;
00155
00156 timer_ = nh_.createTimer(timeout_, &TrajectoryControllerHandler::timeout, this, true, true);
00157 }
00158
00159 void TrajectoryControllerHandler::initializeOvershootTrajectory()
00160 {
00161 overshoot_trajectory_.header.stamp = ros::Time::now();
00162 overshoot_trajectory_.joint_names = goal_trajectory_.joint_names;
00163 overshoot_trajectory_.points.clear();
00164
00165 controller_state_ = TrajectoryControllerStates::OVERSHOOTING;
00166
00167 timer_.stop();
00168 timer_ = nh_.createTimer(max_overshoot_time_, &TrajectoryControllerHandler::timeout, this, true, true);
00169 }
00170
00171 void TrajectoryControllerHandler::done()
00172 {
00173 timer_.stop();
00174 const TrajectoryControllerState state = controller_state_;
00175 controller_state_ = TrajectoryControllerStates::IDLE;
00176 recorder_->deregisterCallback(group_controller_combo_name_);
00177 trajectory_finished_callback_( state );
00178 }
00179
00180 void TrajectoryControllerHandler::doneDelayed()
00181 {
00182 timer_.stop();
00183 const TrajectoryControllerState state = controller_state_;
00184 controller_state_ = TrajectoryControllerStates::IDLE;
00185 recorder_->delayedDeregisterCallback(group_controller_combo_name_);
00186 trajectory_finished_callback_( state );
00187 }