41 #include <boost/algorithm/string/join.hpp> 43 #include <dynamic_reconfigure/server.h> 44 #include <moveit_ros_planning/PlanExecutionDynamicReconfigureConfig.h> 54 : owner_(owner), dynamic_reconfigure_server_(
ros::NodeHandle(
"~/plan_execution"))
56 dynamic_reconfigure_server_.setCallback(
57 boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback,
this, _1, _2));
63 owner_->setMaxReplanAttempts(config.max_replan_attempts);
64 owner_->setTrajectoryStateRecordingFrequency(config.record_trajectory_state_frequency);
74 const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution)
76 , planning_scene_monitor_(planning_scene_monitor)
77 , trajectory_execution_manager_(trajectory_execution)
107 if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
109 else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME)
110 return "Invalid group name";
111 else if (error_code.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED)
112 return "Planning failed.";
113 else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN)
114 return "Invalid motion plan";
115 else if (error_code.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
116 return "Unable to aquire sensor data";
117 else if (error_code.val == moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
118 return "Motion plan invalidated by environment change";
119 else if (error_code.val == moveit_msgs::MoveItErrorCodes::CONTROL_FAILED)
120 return "Controller failed during execution";
121 else if (error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT)
122 return "Timeout reached";
123 else if (error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
125 else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS)
126 return "Invalid goal constraints";
127 else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME)
128 return "Invalid object name";
129 else if (error_code.val == moveit_msgs::MoveItErrorCodes::FAILURE)
130 return "Catastrophic failure";
131 return "Unknown event";
142 const moveit_msgs::PlanningScene& scene_diff,
const Options& opt)
166 unsigned int max_replan_attempts =
168 unsigned int replan_attempts = 0;
169 bool previously_solved =
false;
176 ROS_INFO_NAMED(
"plan_execution",
"Planning attempt %u of at most %u", replan_attempts, max_replan_attempts);
196 if (plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED ||
197 plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN ||
198 plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
200 if (plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA &&
211 previously_solved =
true;
215 if (plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
228 if (plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
232 if (plan.
error_code_.val != moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
239 ROS_INFO_NAMED(
"plan_execution",
"Waiting for a %lf seconds before attempting a new plan ...",
251 plan.
error_code_.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
257 if (plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
258 ROS_DEBUG_NAMED(
"plan_execution",
"PlanExecution finished successfully.");
272 const std::pair<int, int>& path_segment)
274 if (path_segment.first >= 0 &&
288 for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
299 ROS_INFO_NAMED(
"plan_execution",
"Trajectory component '%s' is invalid",
324 moveit_msgs::MoveItErrorCodes result;
332 result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
338 result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
353 bool unwound =
false;
354 for (std::size_t j = 0; j < i; ++j)
381 moveit_msgs::RobotTrajectory msg;
388 result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
426 ROS_INFO_NAMED(
"plan_execution",
"Stopping execution due to preempt request");
431 ROS_INFO_NAMED(
"plan_execution",
"Stopping execution because the path to execute became invalid" 432 "(probably the environment changed)");
437 ROS_WARN_NAMED(
"plan_execution",
"Stopping execution due to unknown reason." 438 "Possibly the node is about to shut down.");
448 result.val = moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE;
453 result.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
459 result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
462 result.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
464 result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
489 ROS_WARN_NAMED(
"plan_execution",
"Length of provided motion plan is zero.");
499 ROS_ERROR_NAMED(
"plan_execution",
"Execution of path-completion side-effect failed. Preempting.");
506 if (index < plan->plan_components_.size() && plan->
plan_components_[index].trajectory_ &&
#define ROS_INFO_NAMED(name,...)
#define ROS_ERROR_STREAM_NAMED(name, args)
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
The geometry of the scene was updated. This includes receiving new octomaps, collision objects...
#define ROS_WARN_NAMED(name,...)
bool path_became_invalid_
const std::string & getGroupName() const
unsigned int replan_attempts_
moveit_msgs::MoveItErrorCodes error_code_
An error code reflecting what went wrong (if anything)
void planAndExecuteHelper(ExecutableMotionPlan &plan, const Options &opt)
boost::function< bool(ExecutableMotionPlan &plan_to_update, const std::pair< int, int > &trajectory_index)> repair_plan_callback_
planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
#define ROS_DEBUG_NAMED(name,...)
ros::NodeHandle node_handle_
planning_scene::PlanningSceneConstPtr planning_scene_
void planAndExecute(ExecutableMotionPlan &plan, const Options &opt)
boost::function< void()> done_callback_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
ExecutableMotionPlanComputationFn plan_callback_
Callback for computing motion plans. This callback must always be specified.
boost::function< void()> before_plan_callback_
PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor, const trajectory_execution_manager::TrajectoryExecutionManagerPtr &trajectory_execution)
double replan_delay_
The amount of time to wait in between replanning attempts (in seconds)
bool isRemainingPathValid(const ExecutableMotionPlan &plan)
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
bool replan_
Flag indicating whether replanning is allowed.
const robot_state::RobotState & getWayPoint(std::size_t index) const
std::size_t getWayPointCount() const
This namespace includes functionality specific to the execution and monitoring of motion plans...
Monitors the joint_states topic and tf to record the trajectory of the robot.
#define ROS_ERROR_NAMED(name,...)
The maintained set of fixed transforms in the monitored scene was updated.
A generic representation on what a computed motion plan looks like.
void successfulTrajectorySegmentExecution(const ExecutableMotionPlan *plan, std::size_t index)
unsigned int default_max_replan_attempts_
boost::function< void()> before_execution_callback_
DynamicReconfigureImpl * reconfigure_impl_
std::string getErrorCodeString(const moveit_msgs::MoveItErrorCodes &error_code)
void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus &status)
std::vector< ExecutableTrajectory > plan_components_
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
moveit_msgs::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan &plan)
Execute and monitor a previously created plan.