Merge branch 'main' into unify-logging

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -771,6 +771,7 @@ void ModelBasedPlanningContext::postSolve()
 
 void ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res)
 {
+  res.planner_id = request_.planner_id;
   res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts);
   if (res.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
   {
@@ -800,12 +801,11 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& re
 
 void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res)
 {
-  moveit_msgs::msg::MoveItErrorCodes moveit_result =
-      solve(request_.allowed_planning_time, request_.num_planning_attempts);
-  if (moveit_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+  res.planner_id = request_.planner_id;
+  res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts);
+  if (res.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
   {
     RCLCPP_INFO(getLogger(), "Unable to solve the planning problem");
-    res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
     return;
   }
 
@@ -843,7 +843,6 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResp
 
   RCLCPP_DEBUG(getLogger(), "%s: Returning successful solution with %lu states", getName().c_str(),
                getOMPLSimpleSetup()->getSolutionPath().getStateCount());
-  res.error_code.val = moveit_result.val;
 }
 
 const moveit_msgs::msg::MoveItErrorCodes ModelBasedPlanningContext::solve(double timeout, unsigned int count)

moveit_py/moveit/planning.pyi

@@ -68,6 +68,9 @@ class PlanningSceneMonitor:
 
 class TrajectoryExecutionManager:
     def __init__(self, *args, **kwargs) -> None: ...
+    def allowed_execution_duration_scaling(self, *args, **kwargs) -> Any: ...
+    def allowed_goal_duration_margin(self, *args, **kwargs) -> Any: ...
+    def allowed_start_tolerance(self, *args, **kwargs) -> Any: ...
     def are_controllers_active(self, *args, **kwargs) -> Any: ...
     def clear(self, *args, **kwargs) -> Any: ...
     def enable_execution_duration_monitoring(self, *args, **kwargs) -> Any: ...
@@ -77,6 +80,8 @@ class TrajectoryExecutionManager:
     def ensure_active_controllers_for_joints(self, *args, **kwargs) -> Any: ...
     def execute(self, *args, **kwargs) -> Any: ...
     def execute_and_wait(self, *args, **kwargs) -> Any: ...
+    def execution_duration_monitoring(self, *args, **kwargs) -> Any: ...
+    def execution_velocity_scaling(self, *args, **kwargs) -> Any: ...
     def get_last_execution_status(self, *args, **kwargs) -> Any: ...
     def is_controller_active(self, *args, **kwargs) -> Any: ...
     def is_managing_controllers(self, *args, **kwargs) -> Any: ...
@@ -89,3 +94,4 @@ class TrajectoryExecutionManager:
     def set_wait_for_trajectory_completion(self, *args, **kwargs) -> Any: ...
     def stop_execution(self, *args, **kwargs) -> Any: ...
     def wait_for_execution(self, *args, **kwargs) -> Any: ...
+    def wait_for_trajectory_completion(self, *args, **kwargs) -> Any: ...

moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp

@@ -214,19 +214,37 @@ void initTrajectoryExecutionManager(py::module& m)
            If a controller takes longer than expected, the trajectory is canceled.
            )")
 
+      .def("execution_duration_monitoring",
+           &trajectory_execution_manager::TrajectoryExecutionManager::executionDurationMonitoring,
+           R"(
+           Get the current status of the monitoring of trajectory execution duration.
+           )")
+
       .def("set_allowed_execution_duration_scaling",
            &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedExecutionDurationScaling,
            py::arg("scaling"),
            R"(
            When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution.
            )")
 
+      .def("allowed_execution_duration_scaling",
+           &trajectory_execution_manager::TrajectoryExecutionManager::allowedExecutionDurationScaling,
+           R"(
+           Get the current scaling of the duration of a trajectory to get the allowed duration of execution.
+           )")
+
       .def("set_allowed_goal_duration_margin",
            &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedGoalDurationMargin, py::arg("margin"),
            R"(
            When determining the expected duration of a trajectory, this additional margin s applied after scalign to allow more than the expected execution time before triggering trajectory cancel.
            )")
 
+      .def("allowed_goal_duration_margin",
+           &trajectory_execution_manager::TrajectoryExecutionManager::allowedGoalDurationMargin,
+           R"(
+           Get the current margin of the duration of a trajectory to get the allowed duration of execution.
+           )")
+
       .def("set_execution_velocity_scaling",
            &trajectory_execution_manager::TrajectoryExecutionManager::setExecutionVelocityScaling, py::arg("scaling"),
            R"(
@@ -235,16 +253,33 @@ void initTrajectoryExecutionManager(py::module& m)
            By default, this is 1.0
            )")
 
+      .def("execution_velocity_scaling",
+           &trajectory_execution_manager::TrajectoryExecutionManager::executionVelocityScaling,
+           R"(
+           Get the current scaling of the execution velocities.
+           )")
+
       .def("set_allowed_start_tolerance",
            &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedStartTolerance, py::arg("tolerance"),
            R"(
            Set joint-value tolerance for validating trajectory's start point against current robot state.
            )")
 
+      .def("allowed_start_tolerance", &trajectory_execution_manager::TrajectoryExecutionManager::allowedStartTolerance,
+           R"(
+           Get the current joint-value for validating trajectory's start point against current robot state.
+           )")
+
       .def("set_wait_for_trajectory_completion",
            &trajectory_execution_manager::TrajectoryExecutionManager::setWaitForTrajectoryCompletion, py::arg("flag"),
            R"(
            Enable or disable waiting for trajectory completion.
+           )")
+
+      .def("wait_for_trajectory_completion",
+           &trajectory_execution_manager::TrajectoryExecutionManager::waitForTrajectoryCompletion,
+           R"(
+           Get the current state of waiting for the trajectory being completed.
            )");
 
   // ToDo(MatthijsBurgh)

moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp

@@ -65,7 +65,13 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla
   const planning_pipeline::PlanningPipelinePtr pipeline = it->second;
   if (!pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response))
   {
-    motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE;
+    if ((motion_plan_response.error_code.val == moveit::core::MoveItErrorCode::SUCCESS) ||
+        (motion_plan_response.error_code.val == moveit::core::MoveItErrorCode::UNDEFINED))
+    {
+      RCLCPP_ERROR(getLogger(), "Planning pipeline '%s' failed to plan, but did not set an error code",
+                   motion_plan_request.pipeline_id.c_str());
+      motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE;
+    }
   }
   return motion_plan_response;
 }

moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp

@@ -85,8 +85,18 @@ class CheckStartStateCollision : public planning_interface::PlanningRequestAdapt
     }
     else
     {
+      collision_detection::CollisionResult::ContactMap contacts;
+      planning_scene->getCollidingPairs(contacts);
+
+      std::string contact_information = std::to_string(contacts.size()) + " contact(s) detected : ";
+
+      for (const auto& [contact_pair, contact_info] : contacts)
+      {
+        contact_information.append(contact_pair.first + " - " + contact_pair.second + ", ");
+      }
+
       status.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION;
-      status.message = std::string("Start state in collision.");
+      status.message = std::string(contact_information);
     }
     status.source = getDescription();
     return status;

moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h

@@ -193,24 +193,42 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
   /// longer than expected, the trajectory is canceled
   void enableExecutionDurationMonitoring(bool flag);
 
+  /// Get the current status of the monitoring of trajectory execution duration.
+  bool executionDurationMonitoring() const;
+
   /// When determining the expected duration of a trajectory, this multiplicative factor is applied
   /// to get the allowed duration of execution
   void setAllowedExecutionDurationScaling(double scaling);
 
+  /// Get the current scaling of the duration of a trajectory to get the allowed duration of execution.
+  double allowedExecutionDurationScaling() const;
+
   /// 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
   void setAllowedGoalDurationMargin(double margin);
 
+  /// Get the current margin of the duration of a trajectory to get the allowed duration of execution.
+  double allowedGoalDurationMargin() const;
+
   /// Before sending a trajectory to a controller, scale the velocities by the factor specified.
   /// By default, this is 1.0
   void setExecutionVelocityScaling(double scaling);
 
+  /// Get the current scaling of the execution velocities.
+  double executionVelocityScaling() const;
+
   /// Set joint-value tolerance for validating trajectory's start point against current robot state
   void setAllowedStartTolerance(double tolerance);
 
+  /// Get the current joint-value for validating trajectory's start point against current robot state.
+  double allowedStartTolerance() const;
+
   /// Enable or disable waiting for trajectory completion
   void setWaitForTrajectoryCompletion(bool flag);
 
+  /// Get the current state of waiting for the trajectory being completed.
+  bool waitForTrajectoryCompletion() const;
+
   rclcpp::Node::SharedPtr getControllerManagerNode()
   {
     return controller_mgr_node_;

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -258,31 +258,61 @@ void TrajectoryExecutionManager::enableExecutionDurationMonitoring(bool flag)
   execution_duration_monitoring_ = flag;
 }
 
+bool TrajectoryExecutionManager::executionDurationMonitoring() const
+{
+  return execution_duration_monitoring_;
+}
+
 void TrajectoryExecutionManager::setAllowedExecutionDurationScaling(double scaling)
 {
   allowed_execution_duration_scaling_ = scaling;
 }
 
+double TrajectoryExecutionManager::allowedExecutionDurationScaling() const
+{
+  return allowed_execution_duration_scaling_;
+}
+
 void TrajectoryExecutionManager::setAllowedGoalDurationMargin(double margin)
 {
   allowed_goal_duration_margin_ = margin;
 }
 
+double TrajectoryExecutionManager::allowedGoalDurationMargin() const
+{
+  return allowed_goal_duration_margin_;
+}
+
 void TrajectoryExecutionManager::setExecutionVelocityScaling(double scaling)
 {
   execution_velocity_scaling_ = scaling;
 }
 
+double TrajectoryExecutionManager::executionVelocityScaling() const
+{
+  return execution_velocity_scaling_;
+}
+
 void TrajectoryExecutionManager::setAllowedStartTolerance(double tolerance)
 {
   allowed_start_tolerance_ = tolerance;
 }
 
+double TrajectoryExecutionManager::allowedStartTolerance() const
+{
+  return allowed_start_tolerance_;
+}
+
 void TrajectoryExecutionManager::setWaitForTrajectoryCompletion(bool flag)
 {
   wait_for_trajectory_completion_ = flag;
 }
 
+bool TrajectoryExecutionManager::waitForTrajectoryCompletion() const
+{
+  return wait_for_trajectory_completion_;
+}
+
 bool TrajectoryExecutionManager::isManagingControllers() const
 {
   return manage_controllers_;