#include <trajectory_execution_manager.h>
Classes | |
struct | ControllerInformation |
class | DynamicReconfigureImpl |
struct | TrajectoryExecutionContext |
Data structure that represents information necessary to execute a trajectory. More... | |
Public Types | |
typedef boost::function< void(const moveit_controller_manager::ExecutionStatus &)> | ExecutionCompleteCallback |
using | PathSegmentCompleteCallback = boost::function< void(std::size_t)> |
Public Member Functions | |
bool | areControllersActive (const std::vector< std::string > &controllers) |
Check if a set of controllers are active. More... | |
void | clear () |
Clear the trajectories to execute. More... | |
void | enableExecutionDurationMonitoring (bool flag) |
bool | ensureActiveController (const std::string &controller) |
Make sure a particular controller is active. More... | |
bool | ensureActiveControllers (const std::vector< std::string > &controllers) |
Make sure a particular set of controllers are active. More... | |
bool | ensureActiveControllersForGroup (const std::string &group) |
Make sure the active controllers are such that trajectories that actuate joints in the specified group can be executed. More... | |
bool | ensureActiveControllersForJoints (const std::vector< std::string > &joints) |
Make sure the active controllers are such that trajectories that actuate joints in the specified set can be executed. More... | |
void | execute (const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear=true) |
void | execute (const ExecutionCompleteCallback &callback=ExecutionCompleteCallback(), bool auto_clear=true) |
Start the execution of pushed trajectories; this does not wait for completion, but calls a callback when done. More... | |
moveit_controller_manager::ExecutionStatus | executeAndWait (bool auto_clear=true) |
const moveit_controller_manager::MoveItControllerManagerPtr & | getControllerManager () const |
Get the instance of the controller manager used (this is the plugin instance loaded) More... | |
std::pair< int, int > | getCurrentExpectedTrajectoryIndex () const |
moveit_controller_manager::ExecutionStatus | getLastExecutionStatus () const |
Return the controller status for the last attempted execution. More... | |
const std::vector< TrajectoryExecutionContext * > & | getTrajectories () const |
Get the trajectories to be executed. More... | |
bool | isControllerActive (const std::string &controller) |
Check if a controller is active. More... | |
bool | isManagingControllers () const |
If this function returns true, then this instance of the manager is allowed to load/unload/switch controllers. More... | |
void | processEvent (const std::string &event) |
Execute a named event (e.g., 'stop') More... | |
bool | push (const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller="") |
bool | push (const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers) |
bool | push (const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller="") |
bool | push (const trajectory_msgs::JointTrajectory &trajectory, const std::vector< std::string > &controllers) |
void | setAllowedExecutionDurationScaling (double scaling) |
void | setAllowedGoalDurationMargin (double margin) |
void | setAllowedStartTolerance (double tolerance) |
Set joint-value tolerance for validating trajectory's start point against current robot state. More... | |
void | setExecutionVelocityScaling (double scaling) |
void | setWaitForTrajectoryCompletion (bool flag) |
Enable or disable waiting for trajectory completion. More... | |
void | stopExecution (bool auto_clear=true) |
Stop whatever executions are active, if any. More... | |
TrajectoryExecutionManager (const moveit::core::RobotModelConstPtr &robot_model, const planning_scene_monitor::CurrentStateMonitorPtr &csm) | |
Load the controller manager plugin, start listening for events on a topic. More... | |
TrajectoryExecutionManager (const moveit::core::RobotModelConstPtr &robot_model, const planning_scene_monitor::CurrentStateMonitorPtr &csm, bool manage_controllers) | |
Load the controller manager plugin, start listening for events on a topic. More... | |
moveit_controller_manager::ExecutionStatus | waitForExecution () |
~TrajectoryExecutionManager () | |
Destructor. Cancels all running trajectories (if any) More... | |
Static Public Attributes | |
static const std::string | EXECUTION_EVENT_TOPIC = "trajectory_execution_event" |
Private Member Functions | |
bool | checkControllerCombination (std::vector< std::string > &controllers, const std::set< std::string > &actuated_joints) |
bool | configure (TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers) |
bool | distributeTrajectory (const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers, std::vector< moveit_msgs::RobotTrajectory > &parts) |
bool | executePart (std::size_t part_index) |
void | executeThread (const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear) |
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) |
void | generateControllerCombination (std::size_t start_index, std::size_t controller_count, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers, std::vector< std::vector< std::string > > &selected_options, const std::set< std::string > &actuated_joints) |
void | initialize () |
void | loadControllerParams () |
void | receiveEvent (const std_msgs::StringConstPtr &event) |
void | reloadControllerInformation () |
bool | selectControllers (const std::set< std::string > &actuated_joints, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers) |
void | stopExecutionInternal () |
void | updateControllersState (const ros::Duration &age) |
void | updateControllerState (const std::string &controller, const ros::Duration &age) |
void | updateControllerState (ControllerInformation &ci, const ros::Duration &age) |
bool | validate (const TrajectoryExecutionContext &context) const |
Validate first point of trajectory matches current robot state. More... | |
bool | waitForRobotToStop (const TrajectoryExecutionContext &context, double wait_time=1.0) |
Definition at line 91 of file trajectory_execution_manager.h.
typedef boost::function<void(const moveit_controller_manager::ExecutionStatus&)> trajectory_execution_manager::TrajectoryExecutionManager::ExecutionCompleteCallback |
Definition of the function signature that is called when the execution of all the pushed trajectories completes. The status of the overall execution is passed as argument
Definition at line 98 of file trajectory_execution_manager.h.
using trajectory_execution_manager::TrajectoryExecutionManager::PathSegmentCompleteCallback = boost::function<void(std::size_t)> |
Definition of the function signature that is called when the execution of a pushed trajectory completes successfully.
Definition at line 102 of file trajectory_execution_manager.h.
trajectory_execution_manager::TrajectoryExecutionManager::TrajectoryExecutionManager | ( | const moveit::core::RobotModelConstPtr & | robot_model, |
const planning_scene_monitor::CurrentStateMonitorPtr & | csm | ||
) |
Load the controller manager plugin, start listening for events on a topic.
Definition at line 85 of file trajectory_execution_manager.cpp.
trajectory_execution_manager::TrajectoryExecutionManager::TrajectoryExecutionManager | ( | const moveit::core::RobotModelConstPtr & | robot_model, |
const planning_scene_monitor::CurrentStateMonitorPtr & | csm, | ||
bool | manage_controllers | ||
) |
Load the controller manager plugin, start listening for events on a topic.
Definition at line 95 of file trajectory_execution_manager.cpp.
trajectory_execution_manager::TrajectoryExecutionManager::~TrajectoryExecutionManager | ( | ) |
Destructor. Cancels all running trajectories (if any)
Definition at line 103 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::areControllersActive | ( | const std::vector< std::string > & | controllers | ) |
Check if a set of controllers are active.
Definition at line 544 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 364 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::clear | ( | ) |
Clear the trajectories to execute.
Definition at line 1037 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 816 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 582 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::enableExecutionDurationMonitoring | ( | bool | flag | ) |
Enable or disable the monitoring of trajectory execution duration. If a controller takes longer than expected, the trajectory is canceled
Definition at line 185 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveController | ( | const std::string & | controller | ) |
Make sure a particular controller is active.
Definition at line 1433 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllers | ( | const std::vector< std::string > & | controllers | ) |
Make sure a particular set of controllers are active.
Definition at line 1438 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllersForGroup | ( | const std::string & | group | ) |
Make sure the active controllers are such that trajectories that actuate joints in the specified group can be executed.
Definition at line 1399 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllersForJoints | ( | const std::vector< std::string > & | joints | ) |
Make sure the active controllers are such that trajectories that actuate joints in the specified set can be executed.
Definition at line 1408 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::execute | ( | const ExecutionCompleteCallback & | callback, |
const PathSegmentCompleteCallback & | part_callback, | ||
bool | auto_clear = true |
||
) |
Start the execution of pushed trajectories; this does not wait for completion, but calls a callback when done. A callback is also called for every trajectory part that completes successfully.
Definition at line 996 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::execute | ( | const ExecutionCompleteCallback & | callback = ExecutionCompleteCallback() , |
bool | auto_clear = true |
||
) |
Start the execution of pushed trajectories; this does not wait for completion, but calls a callback when done.
Definition at line 991 of file trajectory_execution_manager.cpp.
moveit_controller_manager::ExecutionStatus trajectory_execution_manager::TrajectoryExecutionManager::executeAndWait | ( | bool | auto_clear = true | ) |
This is a blocking call for the execution of the passed in trajectories. This just calls execute() and waitForExecution()
Definition at line 926 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 1102 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 1049 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 455 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 390 of file trajectory_execution_manager.cpp.
const moveit_controller_manager::MoveItControllerManagerPtr & trajectory_execution_manager::TrajectoryExecutionManager::getControllerManager | ( | ) | const |
Get the instance of the controller manager used (this is the plugin instance loaded)
Definition at line 220 of file trajectory_execution_manager.cpp.
std::pair< int, int > trajectory_execution_manager::TrajectoryExecutionManager::getCurrentExpectedTrajectoryIndex | ( | ) | const |
Get the state that the robot is expected to be at, given current time, after execute() has been called. The return value is a pair of two index values: first = the index of the trajectory to be executed (in the order push() was called), second = the index of the point within that trajectory. Values of -1 are returned when there is no trajectory being executed, or if the trajectory was passed using pushAndExecute().
Definition at line 1375 of file trajectory_execution_manager.cpp.
moveit_controller_manager::ExecutionStatus trajectory_execution_manager::TrajectoryExecutionManager::getLastExecutionStatus | ( | ) | const |
Return the controller status for the last attempted execution.
Definition at line 1394 of file trajectory_execution_manager.cpp.
const std::vector< TrajectoryExecutionManager::TrajectoryExecutionContext * > & trajectory_execution_manager::TrajectoryExecutionManager::getTrajectories | ( | ) | const |
Get the trajectories to be executed.
Definition at line 1389 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 109 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::isControllerActive | ( | const std::string & | controller | ) |
Check if a controller is active.
Definition at line 539 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::isManagingControllers | ( | ) | const |
If this function returns true, then this instance of the manager is allowed to load/unload/switch controllers.
Definition at line 215 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 1536 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::processEvent | ( | const std::string & | event | ) |
Execute a named event (e.g., 'stop')
Definition at line 225 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::push | ( | const moveit_msgs::RobotTrajectory & | trajectory, |
const std::string & | controller = "" |
||
) |
Add a trajectory for future execution. Optionally specify a controller to use for the trajectory. If no controller is specified, a default is used.
Definition at line 239 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::push | ( | const moveit_msgs::RobotTrajectory & | trajectory, |
const std::vector< std::string > & | controllers | ||
) |
Add a trajectory for future execution. Optionally specify a set of controllers to consider using for the trajectory. Multiple controllers can be used simultaneously to execute the different parts of the trajectory. If multiple controllers can be used, preference is given to the already loaded ones. If no controller is specified, a default is used.
Definition at line 263 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::push | ( | const trajectory_msgs::JointTrajectory & | trajectory, |
const std::string & | controller = "" |
||
) |
Add a trajectory for future execution. Optionally specify a controller to use for the trajectory. If no controller is specified, a default is used.
Definition at line 247 of file trajectory_execution_manager.cpp.
bool trajectory_execution_manager::TrajectoryExecutionManager::push | ( | const trajectory_msgs::JointTrajectory & | trajectory, |
const std::vector< std::string > & | controllers | ||
) |
Add a trajectory for future execution. Optionally specify a set of controllers to consider using for the trajectory. Multiple controllers can be used simultaneously to execute the different parts of the trajectory. If multiple controllers can be used, preference is given to the already loaded ones. If no controller is specified, a default is used.
Definition at line 255 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 233 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 298 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 556 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::setAllowedExecutionDurationScaling | ( | double | scaling | ) |
When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution
Definition at line 190 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::setAllowedGoalDurationMargin | ( | double | margin | ) |
When determining the expected duration of a trajectory, this multiplicative factor is applied to allow more than the expected execution time before triggering trajectory cancel
Definition at line 195 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::setAllowedStartTolerance | ( | double | tolerance | ) |
Set joint-value tolerance for validating trajectory's start point against current robot state.
Definition at line 205 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::setExecutionVelocityScaling | ( | double | scaling | ) |
Before sending a trajectory to a controller, scale the velocities by the factor specified. By default, this is 1.0
Definition at line 200 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::setWaitForTrajectoryCompletion | ( | bool | flag | ) |
Enable or disable waiting for trajectory completion.
Definition at line 210 of file trajectory_execution_manager.cpp.
void trajectory_execution_manager::TrajectoryExecutionManager::stopExecution | ( | bool | auto_clear = true | ) |
Stop whatever executions are active, if any.
Definition at line 946 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 932 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 358 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 333 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 342 of file trajectory_execution_manager.cpp.
|
private |
Validate first point of trajectory matches current robot state.
Definition at line 718 of file trajectory_execution_manager.cpp.
moveit_controller_manager::ExecutionStatus trajectory_execution_manager::TrajectoryExecutionManager::waitForExecution | ( | ) |
Wait until the execution is complete. This only works for executions started by execute(). If you call this after pushAndExecute(), it will immediately stop execution.
Definition at line 1023 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 1316 of file trajectory_execution_manager.cpp.
|
private |
Definition at line 319 of file trajectory_execution_manager.h.
|
private |
Definition at line 337 of file trajectory_execution_manager.h.
|
private |
Definition at line 338 of file trajectory_execution_manager.h.
|
private |
Definition at line 344 of file trajectory_execution_manager.h.
|
private |
Definition at line 341 of file trajectory_execution_manager.h.
|
private |
Definition at line 342 of file trajectory_execution_manager.h.
|
private |
Definition at line 328 of file trajectory_execution_manager.h.
|
private |
Definition at line 327 of file trajectory_execution_manager.h.
|
private |
Definition at line 301 of file trajectory_execution_manager.h.
|
private |
Definition at line 320 of file trajectory_execution_manager.h.
|
private |
Definition at line 304 of file trajectory_execution_manager.h.
|
private |
Definition at line 323 of file trajectory_execution_manager.h.
|
private |
Definition at line 316 of file trajectory_execution_manager.h.
|
private |
Definition at line 335 of file trajectory_execution_manager.h.
|
static |
Definition at line 94 of file trajectory_execution_manager.h.
|
private |
Definition at line 312 of file trajectory_execution_manager.h.
|
private |
Definition at line 310 of file trajectory_execution_manager.h.
|
private |
Definition at line 313 of file trajectory_execution_manager.h.
|
private |
Definition at line 345 of file trajectory_execution_manager.h.
|
private |
Definition at line 306 of file trajectory_execution_manager.h.
|
private |
Definition at line 318 of file trajectory_execution_manager.h.
|
private |
Definition at line 307 of file trajectory_execution_manager.h.
|
private |
Definition at line 302 of file trajectory_execution_manager.h.
|
private |
Definition at line 332 of file trajectory_execution_manager.h.
|
private |
Definition at line 300 of file trajectory_execution_manager.h.
|
private |
Definition at line 303 of file trajectory_execution_manager.h.
|
private |
Definition at line 321 of file trajectory_execution_manager.h.
|
mutableprivate |
Definition at line 322 of file trajectory_execution_manager.h.
|
private |
Definition at line 325 of file trajectory_execution_manager.h.
|
private |
Definition at line 330 of file trajectory_execution_manager.h.
|
private |
Definition at line 346 of file trajectory_execution_manager.h.