From 0c4b4fcaa89992f8b0a6f19a46342bd51352c5c9 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 8 Dec 2023 13:18:06 -0700 Subject: [PATCH] Print warning if no controllers are configured for trajectory execution (#514) --- capabilities/src/execute_task_solution_capability.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 8dc1ea2e..27bdee5a 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -174,6 +174,15 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr exec_traj.trajectory = std::make_shared(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),