mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Print warning if no controllers are configured for trajectory execution (#514)
This commit is contained in:
parent
739375298a
commit
0c4b4fcaa8
@ -174,6 +174,15 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
||||
exec_traj.trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
|
||||
exec_traj.trajectory->setRobotTrajectoryMsg(state, sub_traj.trajectory);
|
||||
|
||||
// Check that sub trajectories that contain a valid trajectory have controllers configured.
|
||||
if (!sub_traj.trajectory.joint_trajectory.points.empty() && sub_traj.execution_info.controller_names.empty()) {
|
||||
RCLCPP_WARN(LOGGER,
|
||||
"The trajectory of stage '%i' from task '%s' does not have any controllers specified for "
|
||||
"trajectory execution. This might lead to unexpected controller selection.",
|
||||
sub_traj.info.stage_id, solution.task_id.c_str());
|
||||
}
|
||||
exec_traj.controller_name = sub_traj.execution_info.controller_names;
|
||||
|
||||
/* TODO add action feedback and markers */
|
||||
exec_traj.effect_on_success = [this,
|
||||
&scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
|
||||
|
||||
Loading…
Reference in New Issue
Block a user