Repository: moveit/moveit2 (C++)

Issue:
CheckStartStateBounds does not support REVOLUTE and NOT Continuous type joints
### Description
CheckStartStateBounds does not support REVOLUTE and NOT Continuous type joints

### Your environment
* ROS Distro: Jazzy
* OS Version: e.g. Ubuntu 24.04
* Binary build 2.10.0

### Steps to reproduce
- Set the initial posture of the actual robot to a position beyond the urdf limit.
- Press “plan” button in rviz to plan the trajectory.

### Expected behaviour
CheckStartStateBounds is not SUCCESS. And fixed to joint limit in case fix_start_state is true.

### Actual behaviour
CheckStartStateBounds is SUCCESS.

### Details

In https://github.com/moveit/moveit2/pull/2429, CheckStartStateBounds is refactored.
The logic has also been changed so that the check is skipped for non-infinite rotation joints, which are used in most arm joints.

https://github.com/moveit/moveit2/blob/a7fe0df4c565db15710581bdcba1d33d70aa0ddb/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp#L105-L120



Discussion: This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.
Looking at the code - yup, this is definitely an issue - the switch statement logic will basically never hit the default case
Does this just involve moving the `break;` in that block up one indentation level?
The original logic was:

- for each joint model:
  - if REVOLUTE & continuous, call enforceBounds() [OK]
  - else
     - if PLANAR - normalizeRotation [OK]
     - else FLOATING - normalizeRotation [OK]
- read parameters
- for each joint model:
   - apply the satisfiesBounds logic

I think we actually just need to pull the "default" case outside the switch statement and do it for all joints? 



Fix (patch):
diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt
index 5ad3751114..54cb62da1a 100644
--- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt
+++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt
@@ -14,3 +14,14 @@ set_target_properties(moveit_default_planning_request_adapter_plugins
                       PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 ament_target_dependencies(moveit_default_planning_request_adapter_plugins
                           moveit_core rclcpp pluginlib)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+
+  ament_add_gmock(test_check_start_state_bounds
+                  test/test_check_start_state_bounds.cpp)
+  target_link_libraries(test_check_start_state_bounds moveit_planning_pipeline
+                        moveit_default_planning_request_adapter_plugins)
+
+endif()
diff --git a/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml b/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml
index 9af11adad9..387cf86470 100644
--- a/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml
+++ b/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml
@@ -1,8 +1,8 @@
-# This file contains the parameters for all of MoveIt's default PlanRequestAdapters
+# This file contains the parameters for all of MoveIt's default PlanningRequestAdapters
 default_request_adapter_parameters:
   fix_start_state: {
     type: bool,
-    description: "CheckStartStateBounds: If set true and the start state is out of bounds, the MotionPlanRequests start will be updated to a start state with enforced bounds for revolute or continuous joints.",
+    description: "CheckStartStateBounds: If true and the start state has continuous, planar, or floating joints, the start state of the MotionPlanRequest will be updated to normalize the rotation values of those joints.",
     default_value: false,
   }
   default_workspace_bounds: {
diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp
index 315084ebef..4b22eee85a 100644
--- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp
+++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp
@@ -94,14 +94,15 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
     // Read parameters
     const auto params = param_listener_->get_params();
 
-    bool changed_req = false;
+    bool should_fix_state = false;
+    bool is_out_of_bounds = false;
     for (const moveit::core::JointModel* jmodel : jmodels)
     {
       // Check if we have a revolute, continuous joint. If we do, then we only need to make sure
       // it is within the model's declared bounds (usually -Pi, Pi), since the values wrap around.
       // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
       // how many times the joint was wrapped. Because of this, we remember the offsets for continuous
-      // joints, and we un-do them when the plan comes from the planner
+      // joints, and we undo them when the plan comes from the planner.
       switch (jmodel->getType())
       {
         case moveit::core::JointModel::REVOLUTE:
@@ -113,7 +114,7 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
             double after = start_state.getJointPositions(jmodel)[0];
             if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
             {
-              changed_req = true;
+              should_fix_state |= true;
             }
           }
           break;
@@ -126,7 +127,7 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
           if (static_cast<const moveit::core::PlanarJointModel*>(jmodel)->normalizeRotation(copy))
           {
             start_state.setJointPositions(jmodel, copy);
-            changed_req = true;
+            should_fix_state |= true;
           }
           break;
         }
@@ -138,58 +139,60 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
           if (static_cast<const moveit::core::FloatingJointModel*>(jmodel)->normalizeRotation(copy))
           {
             start_state.setJointPositions(jmodel, copy);
-            changed_req = true;
+            should_fix_state |= true;
           }
           break;
         }
         default:
         {
-          if (!start_state.satisfiesBounds(jmodel))
-          {
-            std::stringstream joint_values;
-            std::stringstream joint_bounds_low;
-            std::stringstream joint_bounds_hi;
-            const double* p = start_state.getJointPositions(jmodel);
-            for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k)
-            {
-              joint_values << p[k] << ' ';
-            }
-            const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds();
-            for (const moveit::core::VariableBounds& variable_bounds : b)
-            {
-              joint_bounds_low << variable_bounds.min_position_ << ' ';
-              joint_bounds_hi << variable_bounds.max_position_ << ' ';
-            }
-            RCLCPP_ERROR(logger_,
-                         "Joint '%s' from the starting state is outside bounds by: [%s] should be in "
-                         "the range [%s], [%s].",
-                         jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(),
-                         joint_bounds_hi.str().c_str());
-          }
+          break;  // do nothing
         }
       }
-    }
 
-    // If we made any changes, consider using them them
-    if (params.fix_start_state && changed_req)
-    {
-      RCLCPP_WARN(logger_, "Changing start state.");
-      moveit::core::robotStateToRobotStateMsg(start_state, req.start_state);
-      return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""),
-                                           getDescription());
+      // Check the joint against its bounds.
+      if (!start_state.satisfiesBounds(jmodel))
+      {
+        is_out_of_bounds |= true;
+
+        std::stringstream joint_values;
+        std::stringstream joint_bounds_low;
+        std::stringstream joint_bounds_hi;
+        const double* p = start_state.getJointPositions(jmodel);
+        for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k)
+        {
+          joint_values << p[k] << ' ';
+        }
+        const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds();
+        for (const moveit::core::VariableBounds& variable_bounds : b)
+        {
+          joint_bounds_low << variable_bounds.min_position_ << ' ';
+          joint_bounds_hi << variable_bounds.max_position_ << ' ';
+        }
+        RCLCPP_ERROR(logger_,
+                     "Joint '%s' from the starting state is outside bounds by: [%s] should be in "
+                     "the range [%s], [%s].",
+                     jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(),
+                     joint_bounds_hi.str().c_str());
+      }
     }
 
+    // Package up the adapter result, changing the state if applicable.
     auto status = moveit::core::MoveItErrorCode();
-    if (!changed_req)
-    {
-      status.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
-    }
-    else
+    status.source = getDescription();
+    status.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
+
+    if (is_out_of_bounds || (!params.fix_start_state && should_fix_state))
     {
       status.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_INVALID;
       status.message = std::string("Start state out of bounds.");
     }
-    status.source = getDescription();
+    else if (params.fix_start_state && should_fix_state)
+    {
+      constexpr auto msg_string = "Normalized start state.";
+      status.message = msg_string;
+      RCLCPP_WARN(logger_, msg_string);
+      moveit::core::robotStateToRobotStateMsg(start_state, req.start_state);
+    }
     return status;
   }