37 #ifndef MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_ 38 #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_ 43 #include <moveit_msgs/RobotTrajectory.h> 44 #include <sensor_msgs/JointState.h> 45 #include <std_msgs/String.h> 48 #include <boost/thread.hpp> 86 const planning_scene_monitor::CurrentStateMonitorPtr& csm);
90 const planning_scene_monitor::CurrentStateMonitorPtr& csm,
bool manage_controllers);
134 bool push(
const moveit_msgs::RobotTrajectory& trajectory,
const std::string& controller =
"");
138 bool push(
const trajectory_msgs::JointTrajectory& trajectory,
const std::string& controller =
"");
145 bool push(
const trajectory_msgs::JointTrajectory& trajectory,
const std::vector<std::string>& controllers);
152 bool push(
const moveit_msgs::RobotTrajectory& trajectory,
const std::vector<std::string>& controllers);
155 const std::vector<TrajectoryExecutionContext*>&
getTrajectories()
const;
162 void execute(
const ExecutionCompleteCallback& callback,
const PathSegmentCompleteCallback& part_callback,
163 bool auto_clear =
true);
171 bool pushAndExecute(
const moveit_msgs::RobotTrajectory& trajectory,
const std::string& controller =
"");
175 bool pushAndExecute(
const trajectory_msgs::JointTrajectory& trajectory,
const std::string& controller =
"");
180 bool pushAndExecute(
const sensor_msgs::JointState& state,
const std::string& controller =
"");
187 bool pushAndExecute(
const trajectory_msgs::JointTrajectory& trajectory,
const std::vector<std::string>& controllers);
194 bool pushAndExecute(
const moveit_msgs::RobotTrajectory& trajectory,
const std::vector<std::string>& controllers);
201 bool pushAndExecute(
const sensor_msgs::JointState& state,
const std::vector<std::string>& controllers);
257 if (joints_.size() != other.
joints_.size())
258 return joints_.size() < other.
joints_.size();
259 return name_ < other.
name_;
270 const std::vector<std::string>& controllers);
276 bool distributeTrajectory(
const moveit_msgs::RobotTrajectory& trajectory,
const std::vector<std::string>& controllers,
277 std::vector<moveit_msgs::RobotTrajectory>& parts);
279 bool findControllers(
const std::set<std::string>& actuated_joints, std::size_t controller_count,
280 const std::vector<std::string>& available_controllers,
281 std::vector<std::string>& selected_controllers);
284 const std::vector<std::string>& available_controllers,
285 std::vector<std::string>& selected_controllers,
287 const std::set<std::string>& actuated_joints);
289 const std::vector<std::string>& available_controllers,
290 std::vector<std::string>& selected_controllers);
292 void executeThread(
const ExecutionCompleteCallback& callback,
const PathSegmentCompleteCallback& part_callback,
300 void receiveEvent(
const std_msgs::StringConstPtr& event);
305 const std::string
name_ =
"trajectory_execution_manager";
308 planning_scene_monitor::CurrentStateMonitorPtr
csm_;
342 std::unique_ptr<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> >
std::map< std::string, double > controller_allowed_execution_duration_scaling_
std::map< std::string, double > controller_allowed_goal_duration_margin_
moveit_controller_manager::ExecutionStatus last_execution_status_
std::deque< TrajectoryExecutionContext * > continuous_execution_queue_
TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, const planning_scene_monitor::CurrentStateMonitorPtr &csm)
Load the controller manager plugin, start listening for events on a topic.
std::unique_ptr< pluginlib::ClassLoader< moveit_controller_manager::MoveItControllerManager > > controller_manager_loader_
moveit_controller_manager::ExecutionStatus waitForExecution()
ros::NodeHandle root_node_handle_
std::vector< ros::Time > time_index_
void setExecutionVelocityScaling(double scaling)
bool distributeTrajectory(const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers, std::vector< moveit_msgs::RobotTrajectory > &parts)
bool ensureActiveControllersForJoints(const std::vector< std::string > &joints)
Make sure the active controllers are such that trajectories that actuate joints in the specified set ...
robot_model::RobotModelConstPtr robot_model_
bool ensureActiveController(const std::string &controller)
Make sure a particular controller is active.
double allowed_goal_duration_margin_
boost::condition_variable execution_complete_condition_
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)
bool run_continuous_execution_thread_
ros::NodeHandle node_handle_
bool waitForRobotToStop(const TrajectoryExecutionContext &context, double wait_time=1.0)
bool validate(const TrajectoryExecutionContext &context) const
Validate first point of trajectory matches current robot state.
std::unique_ptr< boost::thread > execution_thread_
ros::Subscriber event_topic_subscriber_
void receiveEvent(const std_msgs::StringConstPtr &event)
bool pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller="")
MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager)
bool isManagingControllers() const
If this function returns true, then this instance of the manager is allowed to load/unload/switch con...
bool ensureActiveControllers(const std::vector< std::string > &controllers)
Make sure a particular set of controllers are active.
std::map< std::string, ControllerInformation > known_controllers_
const moveit_controller_manager::MoveItControllerManagerPtr & getControllerManager() const
Get the instance of the controller manager used (this is the plugin instance loaded) ...
bool execution_duration_monitoring_
double execution_velocity_scaling_
static const std::string EXECUTION_EVENT_TOPIC
DynamicReconfigureImpl * reconfigure_impl_
boost::mutex continuous_execution_mutex_
boost::function< void(const moveit_controller_manager::ExecutionStatus &)> ExecutionCompleteCallback
boost::function< void(std::size_t)> PathSegmentCompleteCallback
std::vector< TrajectoryExecutionContext * > trajectories_
void setWaitForTrajectoryCompletion(bool flag)
Enable or disable waiting for trajectory completion.
bool checkControllerCombination(std::vector< std::string > &controllers, const std::set< std::string > &actuated_joints)
std::vector< std::string > controllers_
The controllers to use for executing the different trajectory parts;.
moveit_controller_manager::MoveItControllerManagerPtr controller_manager_
std::unique_ptr< boost::thread > continuous_execution_thread_
std::vector< std::vector< std::string > > selected_options
double allowed_start_tolerance_
bool ensureActiveControllersForGroup(const std::string &group)
Make sure the active controllers are such that trajectories that actuate joints in the specified grou...
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
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)
bool wait_for_trajectory_completion_
void setAllowedExecutionDurationScaling(double scaling)
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 w...
boost::mutex time_index_mutex_
void stopExecutionInternal()
void processEvent(const std::string &event)
Execute a named event (e.g., 'stop')
void setAllowedGoalDurationMargin(double margin)
void continuousExecutionThread()
std::vector< moveit_msgs::RobotTrajectory > trajectory_parts_
bool executePart(std::size_t part_index)
void executeThread(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
bool configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers)
~TrajectoryExecutionManager()
Destructor. Cancels all running trajectories (if any)
void reloadControllerInformation()
void clear()
Clear the trajectories to execute.
moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear=true)
bool areControllersActive(const std::vector< std::string > &controllers)
Check if a set of controllers are active.
std::vector< moveit_controller_manager::MoveItControllerHandlePtr > active_handles_
moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const
Return the controller status for the last attempted execution.
boost::condition_variable continuous_execution_condition_
void stopExecution(bool auto_clear=true)
Stop whatever executions are active, if any.
const std::vector< TrajectoryExecutionContext * > & getTrajectories() const
Get the trajectories to be executed.
void setAllowedStartTolerance(double tolerance)
Set joint-value tolerance for validating trajectory's start point against current robot state...
bool stop_continuous_execution_
void loadControllerParams()
std::pair< int, int > getCurrentExpectedTrajectoryIndex() const
boost::mutex execution_state_mutex_
void updateControllersState(const ros::Duration &age)
bool isControllerActive(const std::string &controller)
Check if a controller is active.
bool selectControllers(const std::set< std::string > &actuated_joints, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers)
planning_scene_monitor::CurrentStateMonitorPtr csm_
double allowed_execution_duration_scaling_
void enableExecutionDurationMonitoring(bool flag)
Data structure that represents information necessary to execute a trajectory.
void updateControllerState(const std::string &controller, const ros::Duration &age)
bool push(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller="")