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

[fastcat]

Nice!! This close #3546 and #3553

There may be multiple crash reasons folks are experiencing. Based on the testing and debugging I noted on #3541, I don't think this will fix the crashes for folks where downgrading rviz avoids the crash. Notably I tried putting all the .0 in my configs and it didn't fix the issue, and also the crash reason noted in the PR description here is quite different from that listed in many of the linked issues.