Fix joint limits crash

[AimanHaidar]

review:

Description

When I put max velocity of joint to number with zero fraction part (100.0) the setup_assistance put it in the joint_limits.yaml
as 100 then

j5:
    has_velocity_limits: true
    max_velocity: 100.0
    has_acceleration_limits: true
    max_acceleration: 10.0
  j6:
    has_velocity_limits: true
    max_velocity: 100
    has_acceleration_limits: true
    max_acceleration: 10

if I use it (for example with moveit_servo) I get:

[manual_control-5] terminate called after throwing an instance of 'rclcpp::exceptions::InvalidParameterTypeException'
[manual_control-5]   what():  parameter 'robot_description_planning.joint_limits.j6.max_velocity' has invalid type: Wrong parameter type, parameter {robot_description_planning.joint_limits.j6.max_velocity} is of type {double}, setting it to {integer} is not allowed.
. 
.
.
[ERROR] [manual_control-5]: process has died [pid 53938, exit code -6, cmd '/home/aymanhadair/ROS2/ProjectController/install/manual_control/lib/manual_control/manual_control --ros-args --params-file /tmp/launch_params_2bxo_2iu --params-file /tmp/launch_params_gpgmeah0 --params-file /tmp/launch_params_6cgfm0fw --params-file /tmp/launch_params_r_lfeaqi --params-file /tmp/launch_params_fijlhhaz --params-file /tmp/launch_params__key9xja --params-file /tmp/launch_params_zj2opjrh'].

I solve it just by adding ".0" if the value is pure integer:

// To Solve Putting integer Values like 100 as 100.0 in YAML
double val = std::min(fabs(b.max_velocity_), fabs(b.min_velocity_));

// Check if val is a full integer (within floating-point tolerance)
bool is_integer = std::fabs(val - std::round(val)) < 1e-9;

// Format value as string
std::ostringstream oss;
if (is_integer)
  oss << std::fixed << std::setprecision(1) << val;  // e.g. 100.0
else
  oss << val;  // e.g. 99.75

emitter << YAML::Value << oss.str();

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[mosfet80]

Nice!! This close #3546 and #3553

[EzraBrooks]

Happy to approve if you add a test demonstrating the fixed behavior so it doesn't regress in the future.

[AimanHaidar]

@EzraBrooks

make urdf with one joint and set the upper limit as (upper="10.0") or any pure integers:

<robot name="simple_two_link_robot">

  <!-- BASE LINK -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.05"/>
      </geometry>
      <material name="gray">
        <color rgba="0.6 0.6 0.6 1.0"/>
      </material>
    </visual>
  </link>

  <!-- MOVING LINK -->
  <link name="link1">
    <visual>
      <!-- the visual is a thin rectangular arm extending along +X -->
      <origin xyz="0 0 0.125" rpy="0 0 0"/>
      <geometry>
        <box size="0.5 0.05 0.05"/>
      </geometry>
      <material name="blue">
        <color rgba="0.2 0.4 0.8 1.0"/>
      </material>
    </visual>
  </link>

  <!-- REVOLUTE JOINT: base_link -> link1 -->
  <joint name="joint1" type="revolute">
    <!-- joint origin: located at top of base (adjust as needed) -->
    <origin xyz="0 0 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="link1"/>
    <!-- axis of rotation (Z axis) -->
    <axis xyz="0 0 1"/>
    <!-- optional joint limits -->
    <limit lower="0.0" upper="10.0" effort="50.0" velocity="10.0"/>
    <!-- safety/continuous not set; if you want full rotation use type="continuous" -->
  </joint>

</robot>

then set it with moveit setup assistant you
will see that the joint_limits.yaml file saved max_velocity as:

max_velocity: 10

then if you tried to use with servo, your program will crash to test that:

1- go to moveit_resources/panda_moveit_config/config and open hard_joint_limits.yaml (because it is the used with the servo demo)
2- change any of the limits to integer without commas:

max_velocity: 2 # instead of 2.1750

3- run the servo demo:

ros2 launch moveit_servo demo_twist.launch.py

the program will not work and you will get:

[demo_twist-5]   what():  parameter 'robot_description_planning.joint_limits.panda_joint1.max_jerk' has invalid type: Wrong parameter type, parameter {robot_description_planning.joint_limits.panda_joint1.max_jerk} is of type {double}, setting it to {integer} is not allowed.
[demo_twist-5] Stack trace (most recent call last):
[demo_twist-5] #18   Object "/usr/lib/x86_64-linux-gnu/ld-linux-x86-64.so.2", at 0xffffffffffffffff, in 
[demo_twist-5] #17   Object "/home/aymanhadair/ros2_ws/install/moveit_servo/lib/moveit_servo/demo_twist", at 0x56b554fc45c4, in _start
[demo_twist-5] #16   Source "../csu/libc-start.c", line 360, in __libc_start_main_impl [0x760b1602a28a]
[demo_twist-5] #15   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __libc_start_call_main [0x760b1602a1c9]
[demo_twist-5] #14   Object "/home/aymanhadair/ros2_ws/install/moveit_servo/lib/moveit_servo/demo_twist", at 0x56b554fc34e9, in main
[demo_twist-5] #13   Object "/home/aymanhadair/ros2_ws/install/moveit_servo/lib/libmoveit_servo_lib_cpp.so.2.14.1", at 0x760b16f0c7c2, in moveit_servo::createPlanningSceneMonitor(std::shared_ptr<rclcpp::Node> const&, servo::Params const&)

.
.
.
.
[demo_twist-5] #12   Object "/home/aymanhadair/ros2_ws/install/moveit_ros_planning/lib/libmoveit_planning_scene_monitor.so.2.14.1", at 0x760b16d77841, in planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[demo_twist-5] #11   Object "/home/aymanhadair/ros2_ws/install/moveit_ros_planning/lib/libmoveit_planning_scene_monitor.so.2.14.1", at 0x760b16d7779a, in planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(std::shared_ptr<rclcpp::Node> const&, std::shared_ptr<planning_scene::PlanningScene> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[demo_twist-5] #10   Object "/home/aymanhadair/ros2_ws/install/moveit_ros_planning/lib/libmoveit_robot_model_loader.so.2.14.1", at 0x760b166cf0a4, in robot_model_loader::RobotModelLoader::RobotModelLoader(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)
[demo_twist-5] #9    Object "/home/aymanhadair/ros2_ws/install/moveit_ros_planning/lib/libmoveit_robot_model_loader.so.2.14.1", at 0x760b166cdf3a, in robot_model_loader::RobotModelLoader::configure(robot_model_loader::RobotModelLoader::Options const&)
[demo_twist-5] #8    Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x760b1694f11f, in rclcpp::node_interfaces::NodeParameters::declare_parameter(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rclcpp::ParameterType, rcl_interfaces::msg::ParameterDescriptor_<std::allocator<void> > const&, bool)
[demo_twist-5] #7    Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x760b168d2153, in 
[demo_twist-5] #6    Source "../../../../src/libstdc++-v3/libsupc++/eh_throw.cc", line 98, in __cxa_throw [0x760b164bb390]
[demo_twist-5] #5    Source "../../../../src/libstdc++-v3/libsupc++/eh_terminate.cc", line 58, in terminate [0x760b164a5a54]
[demo_twist-5] #4    Source "../../../../src/libstdc++-v3/libsupc++/eh_terminate.cc", line 48, in __terminate [0x760b164bb0d9]
[demo_twist-5] #3    Source "../../../../src/libstdc++-v3/libsupc++/vterminate.cc", line 95, in __verbose_terminate_handler [0x760b164a5ff4]
[demo_twist-5] #2    Source "./stdlib/abort.c", line 79, in abort [0x760b160288fe]
[demo_twist-5] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x760b1604527d]
[demo_twist-5] #0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
[demo_twist-5]     | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
[demo_twist-5]       Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x760b1609eb2c]
[demo_twist-5] Aborted (Signal sent by tkill() 77568 1000)
[ERROR] [demo_twist-5]: process has died [pid 77568, exit code -6, cmd '/home/aymanhadair/ros2_ws/install/moveit_servo/lib/moveit_servo/demo_twist --ros-args --params-file /tmp/launch_params_m9x2c8jn --params-file /tmp/launch_params_8n19z_h4 --params-file /tmp/launch_params_vwzkje7k --params-file /tmp/launch_params_zzey6rab --params-file /tmp/launch_params_3zocy2kg --params-file /tmp/launch_params_2q3py77n --params-file /tmp/launch_params_h6kss1t4'].

[EzraBrooks]

I meant an automated test 😄

[AimanHaidar]

😄
I tried to make the test as short as possible.

the first part is how the assistant saves yaml and the second what will happen if we use this format🙂

[EzraBrooks]

yikes, I just realized there aren't any automated tests in this package at all