Sets up the controller for execution, handles the responses, and times-out the controller if necessary. More...
#include <trajectory_controller_handler.h>
Public Member Functions | |
virtual void | cancelExecution ()=0 |
cancel the execution of the trajectory. This should be implemented by derived classes. | |
void | disableOvershoot () |
Disable overshoot monitoring. Overshoot monitoring is disabled by default. | |
bool | enableOvershoot (double max_overshoot_velocity_epsilon, ros::Duration min_overshoot_time, ros::Duration max_overshoot_time) |
This function is called if the trajectory should monitor overshoot (after the trajectory is executed). Returns true if the subclass can monitor overshoot. | |
virtual bool | executeTrajectory (const trajectory_msgs::JointTrajectory &trajectory, boost::shared_ptr< TrajectoryRecorder > &recorder, const TrajectoryFinishedCallbackFunction &traj_callback)=0 |
Execute the trajectory while recording using recorder. Call the callback function when finished. To enable overshoot monitoring, call enableOvershoot function. Overshoot is not monitored by default. | |
const std::string & | getControllerName () const |
const std::string & | getGroupControllerComboName () const |
const std::string & | getGroupName () const |
const trajectory_msgs::JointTrajectory & | getLastGoalTrajectory () const |
const trajectory_msgs::JointTrajectory & | getLastOvershootTrajectory () const |
const trajectory_msgs::JointTrajectory & | getLastRecordedTrajectory () const |
void | setMaximumExecutionTime (ros::Duration max_execution_time) |
Sets a maximum exection time, otherwise the default max execution time will be used. | |
void | timeout (const ros::TimerEvent &event) |
This function gets called when our maximum execution time is exceeded. | |
TrajectoryControllerHandler (const std::string &group_name, const std::string &controller_name) | |
virtual | ~TrajectoryControllerHandler () |
Static Public Member Functions | |
static std::string | combineGroupAndControllerNames (const std::string &group_name, const std::string &controller_name) |
Protected Member Functions | |
bool | _addNewStateToTrajectory (const ros::Time &time, const std::map< std::string, double > &joint_positions, const std::map< std::string, double > &joint_velocities, trajectory_msgs::JointTrajectory &trajectory) |
bool | addNewStateToRecordedTrajectory (const ros::Time &time, const std::map< std::string, double > &joint_positions, const std::map< std::string, double > &joint_velocities) |
void | done () |
Deregisters from the recorder, and executes callback to the monitor. _success should be set before calling this function. | |
void | doneDelayed () |
Sets a flag to deregister when returning execution to the recorder (after its callback). This function should be called from within the TrajectoryFinishedCallbackFunction. | |
unsigned int | findClosestIndex (ros::Duration time_from_start) |
void | initializeOvershootTrajectory () |
void | initializeRecordedTrajectory (const trajectory_msgs::JointTrajectory &goal_trajectory) |
Protected Attributes | |
std::string | controller_name_ |
TrajectoryControllerState | controller_state_ |
trajectory_msgs::JointTrajectory | goal_trajectory_ |
std::string | group_controller_combo_name_ |
std::string | group_name_ |
ros::Duration | max_overshoot_time_ |
double | max_overshoot_velocity_epsilon_ |
ros::Duration | min_overshoot_time_ |
bool | monitor_overshoot_ |
ros::NodeHandle | nh_ |
trajectory_msgs::JointTrajectory | overshoot_trajectory_ |
trajectory_msgs::JointTrajectory | recorded_trajectory_ |
boost::shared_ptr < trajectory_execution_monitor::TrajectoryRecorder > | recorder_ |
ros::Duration | timeout_ |
ros::Timer | timer_ |
trajectory_execution_monitor::TrajectoryFinishedCallbackFunction | trajectory_finished_callback_ |
Sets up the controller for execution, handles the responses, and times-out the controller if necessary.
Definition at line 71 of file trajectory_controller_handler.h.
trajectory_execution_monitor::TrajectoryControllerHandler::TrajectoryControllerHandler | ( | const std::string & | group_name, |
const std::string & | controller_name | ||
) | [inline] |
Definition at line 75 of file trajectory_controller_handler.h.
virtual trajectory_execution_monitor::TrajectoryControllerHandler::~TrajectoryControllerHandler | ( | ) | [inline, virtual] |
Definition at line 86 of file trajectory_controller_handler.h.
bool TrajectoryControllerHandler::_addNewStateToTrajectory | ( | const ros::Time & | time, |
const std::map< std::string, double > & | joint_positions, | ||
const std::map< std::string, double > & | joint_velocities, | ||
trajectory_msgs::JointTrajectory & | trajectory | ||
) | [protected] |
Definition at line 88 of file trajectory_controller_handler.cpp.
bool TrajectoryControllerHandler::addNewStateToRecordedTrajectory | ( | const ros::Time & | time, |
const std::map< std::string, double > & | joint_positions, | ||
const std::map< std::string, double > & | joint_velocities | ||
) | [protected] |
Definition at line 55 of file trajectory_controller_handler.cpp.
virtual void trajectory_execution_monitor::TrajectoryControllerHandler::cancelExecution | ( | ) | [pure virtual] |
cancel the execution of the trajectory. This should be implemented by derived classes.
Implemented in Pr2GripperTrajectoryControllerHandler, and FollowJointTrajectoryControllerHandler.
static std::string trajectory_execution_monitor::TrajectoryControllerHandler::combineGroupAndControllerNames | ( | const std::string & | group_name, |
const std::string & | controller_name | ||
) | [inline, static] |
Definition at line 90 of file trajectory_controller_handler.h.
Disable overshoot monitoring. Overshoot monitoring is disabled by default.
Definition at line 124 of file trajectory_controller_handler.cpp.
void TrajectoryControllerHandler::done | ( | ) | [protected] |
Deregisters from the recorder, and executes callback to the monitor. _success should be set before calling this function.
Definition at line 171 of file trajectory_controller_handler.cpp.
void TrajectoryControllerHandler::doneDelayed | ( | ) | [protected] |
Sets a flag to deregister when returning execution to the recorder (after its callback). This function should be called from within the TrajectoryFinishedCallbackFunction.
Definition at line 180 of file trajectory_controller_handler.cpp.
bool TrajectoryControllerHandler::enableOvershoot | ( | double | max_overshoot_velocity_epsilon, |
ros::Duration | min_overshoot_time, | ||
ros::Duration | max_overshoot_time | ||
) |
This function is called if the trajectory should monitor overshoot (after the trajectory is executed). Returns true if the subclass can monitor overshoot.
Reimplemented in Pr2GripperTrajectoryControllerHandler.
Definition at line 112 of file trajectory_controller_handler.cpp.
virtual bool trajectory_execution_monitor::TrajectoryControllerHandler::executeTrajectory | ( | const trajectory_msgs::JointTrajectory & | trajectory, |
boost::shared_ptr< TrajectoryRecorder > & | recorder, | ||
const TrajectoryFinishedCallbackFunction & | traj_callback | ||
) | [pure virtual] |
Execute the trajectory while recording using recorder. Call the callback function when finished. To enable overshoot monitoring, call enableOvershoot function. Overshoot is not monitored by default.
Implemented in Pr2GripperTrajectoryControllerHandler, and FollowJointTrajectoryControllerHandler.
unsigned int TrajectoryControllerHandler::findClosestIndex | ( | ros::Duration | time_from_start | ) | [protected] |
Definition at line 42 of file trajectory_controller_handler.cpp.
const std::string& trajectory_execution_monitor::TrajectoryControllerHandler::getControllerName | ( | ) | const [inline] |
Definition at line 139 of file trajectory_controller_handler.h.
const std::string& trajectory_execution_monitor::TrajectoryControllerHandler::getGroupControllerComboName | ( | ) | const [inline] |
Definition at line 143 of file trajectory_controller_handler.h.
const std::string& trajectory_execution_monitor::TrajectoryControllerHandler::getGroupName | ( | ) | const [inline] |
Definition at line 135 of file trajectory_controller_handler.h.
const trajectory_msgs::JointTrajectory& trajectory_execution_monitor::TrajectoryControllerHandler::getLastGoalTrajectory | ( | ) | const [inline] |
Definition at line 123 of file trajectory_controller_handler.h.
const trajectory_msgs::JointTrajectory& trajectory_execution_monitor::TrajectoryControllerHandler::getLastOvershootTrajectory | ( | ) | const [inline] |
Definition at line 131 of file trajectory_controller_handler.h.
const trajectory_msgs::JointTrajectory& trajectory_execution_monitor::TrajectoryControllerHandler::getLastRecordedTrajectory | ( | ) | const [inline] |
Definition at line 127 of file trajectory_controller_handler.h.
void TrajectoryControllerHandler::initializeOvershootTrajectory | ( | ) | [protected] |
Definition at line 159 of file trajectory_controller_handler.cpp.
void TrajectoryControllerHandler::initializeRecordedTrajectory | ( | const trajectory_msgs::JointTrajectory & | goal_trajectory | ) | [protected] |
Definition at line 146 of file trajectory_controller_handler.cpp.
void trajectory_execution_monitor::TrajectoryControllerHandler::setMaximumExecutionTime | ( | ros::Duration | max_execution_time | ) | [inline] |
Sets a maximum exection time, otherwise the default max execution time will be used.
Definition at line 106 of file trajectory_controller_handler.h.
void TrajectoryControllerHandler::timeout | ( | const ros::TimerEvent & | event | ) |
This function gets called when our maximum execution time is exceeded.
Definition at line 130 of file trajectory_controller_handler.cpp.
std::string trajectory_execution_monitor::TrajectoryControllerHandler::controller_name_ [protected] |
Definition at line 170 of file trajectory_controller_handler.h.
TrajectoryControllerState trajectory_execution_monitor::TrajectoryControllerHandler::controller_state_ [protected] |
Definition at line 184 of file trajectory_controller_handler.h.
trajectory_msgs::JointTrajectory trajectory_execution_monitor::TrajectoryControllerHandler::goal_trajectory_ [protected] |
Definition at line 175 of file trajectory_controller_handler.h.
std::string trajectory_execution_monitor::TrajectoryControllerHandler::group_controller_combo_name_ [protected] |
Definition at line 171 of file trajectory_controller_handler.h.
std::string trajectory_execution_monitor::TrajectoryControllerHandler::group_name_ [protected] |
Definition at line 169 of file trajectory_controller_handler.h.
ros::Duration trajectory_execution_monitor::TrajectoryControllerHandler::max_overshoot_time_ [protected] |
Definition at line 180 of file trajectory_controller_handler.h.
double trajectory_execution_monitor::TrajectoryControllerHandler::max_overshoot_velocity_epsilon_ [protected] |
Definition at line 178 of file trajectory_controller_handler.h.
ros::Duration trajectory_execution_monitor::TrajectoryControllerHandler::min_overshoot_time_ [protected] |
Definition at line 179 of file trajectory_controller_handler.h.
Definition at line 177 of file trajectory_controller_handler.h.
Definition at line 188 of file trajectory_controller_handler.h.
trajectory_msgs::JointTrajectory trajectory_execution_monitor::TrajectoryControllerHandler::overshoot_trajectory_ [protected] |
Definition at line 174 of file trajectory_controller_handler.h.
trajectory_msgs::JointTrajectory trajectory_execution_monitor::TrajectoryControllerHandler::recorded_trajectory_ [protected] |
Definition at line 173 of file trajectory_controller_handler.h.
boost::shared_ptr<trajectory_execution_monitor::TrajectoryRecorder> trajectory_execution_monitor::TrajectoryControllerHandler::recorder_ [protected] |
Definition at line 182 of file trajectory_controller_handler.h.
Definition at line 187 of file trajectory_controller_handler.h.
Definition at line 189 of file trajectory_controller_handler.h.
trajectory_execution_monitor::TrajectoryFinishedCallbackFunction trajectory_execution_monitor::TrajectoryControllerHandler::trajectory_finished_callback_ [protected] |
Definition at line 183 of file trajectory_controller_handler.h.