search

Interbotix / interbotix_ros_manipulators

ROS Packages for Interbotix Arms
BSD 3-Clause "New" or "Revised" License
120 stars 82 forks source link

[Bug]: xsarm_moveit.launch.py does not get the planning scene and the robot pose #203

Open qcharmoillau opened 3 months ago

qcharmoillau commented 3 months ago

What happened?

I aim at planning motion with a physical Interbotix Widow-X250s robotic arm, and a Raspberry Pi 5 as controller, using xsarm_moveit.launch.py.Unfortunately, the Planning Scene cannot be loaded in the RViz station. Moreover, the actual pose of the arm is not shown in the station.

Robot Model

wx250s

Operating System

Ubuntu 22.04

ROS Distro

ROS 2 Rolling

Steps To Reproduce

I set the working directory, I source setup.bash, then I launch xsarm_moveit.launch.py as indicated in the constructor's tutorial. Unfortunately, the Planning Scene cannot be loaded (as seen in the screenshot below) and the actual pose doesn't match the one shown in the station. image

In the terminal, the log output reveals several problems, particularly in defining the Move Group node and the Controller Manager (below are shown only errors and warnings)

However, the joint states are visible to ROS, I checked it using ros2 topic echo /wx250s/joint_states in the terminal. It returns consistent data.

The planning scene also seems to get data, I checked it using ros2 topic echo /monitored_planning_scene in the terminal. It returns this content : topic monitored_planning_scene terminal feedback.txt

Relevant log output

...
[ros2_control_node-3] [WARN] [1719998304.136143470] [wx250s.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
...
[move_group-1] [WARN] [1719998304.412076777] [move_group]: Falling back to using the the move_group node namespace (deprecated behavior).
...
[move_group-1] [WARN] [1719998304.681879453] [move_group.moveit.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-1] [ERROR] [1719998304.682057141] [move_group.moveit.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates
...
[xs_sdk-6] [WARN] Writing startup register values to EEPROM. This only needs to be done once on a robot if using a default motor config file, or after a motor config file has been modified. Can set `write_eeprom_on_startup` to false from now on.
...
[ros2_control_node-3] [WARN] [1719998305.831259985] [wx250s.controller_manager]: Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT scheduling. See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details.
...
[ERROR] [move_group-1]: process has died [pid 2362, exit code -6, cmd '/opt/ros/rolling/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_9lqzprnd --params-file /tmp/launch_params_wd4_om_4 --params-file /tmp/launch_params_pzl7ywkr --params-file /home/pi/interbotix_ws/install/interbotix_xsarm_moveit/share/interbotix_xsarm_moveit/config/kinematics.yaml --params-file /tmp/launch_params_mii5ilpp --params-file /tmp/launch_params_36u4nvtl --params-file /tmp/launch_params_0rye57qp --params-file /tmp/launch_params_9ptj3j7j --params-file /tmp/launch_params_yw1yqwn5 -r wx250s/get_planning_scene:=/wx250s/get_planning_scene -r /arm_controller/follow_joint_trajectory:=/wx250s/arm_controller/follow_joint_trajectory -r /gripper_controller/follow_joint_trajectory:=/wx250s/gripper_controller/follow_joint_trajectory'].

Additional Info

Potential reasons I suppose :

lukeschmitt-tr commented 3 months ago

Please provide the entire contents of your terminal. I would also note that ROS 2 Rolling is not a supported distribution and we do not test it regularly. You may face issues or unexpected behavior when on that branch.

ThmX commented 3 months ago

Is there a timeline for when Jazzy will be supported? Everything works rather well with Rolling except for the MoveIt integration.

qcharmoillau commented 3 months ago

Please provide the entire contents of your terminal. I would also note that ROS 2 Rolling is not a supported distribution and we do not test it regularly. You may face issues or unexpected behavior when on that branch.

Hello @lukeschmitt-tr , thank you for your quick reply. Below is the entire content:

pi@raspberrypi:~/interbotix_ws$ ros2 launch interbotix_xsarm_moveit xsarm_moveit.launch.py robot_model:=wx250s hardware_type:=actual use_sim_time:=true
[INFO] [launch]: All log files can be found below /home/pi/.ros/log/2024-07-15-12-51-05-688420-raspberrypi-2016
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [move_group-1]: process started with pid [2023]
[INFO] [rviz2-2]: process started with pid [2025]
[INFO] [ros2_control_node-3]: process started with pid [2027]
[INFO] [spawner-4]: process started with pid [2029]
[INFO] [spawner-5]: process started with pid [2031]
[INFO] [xs_sdk-6]: process started with pid [2033]
[INFO] [robot_state_publisher-7]: process started with pid [2035]
[INFO] [robot_state_publisher-8]: process started with pid [2037]
[xs_sdk-6] [INFO] Using Interbotix X-Series Driver Version: 'v0.3.3'.
[xs_sdk-6] [INFO] Using logging level 'INFO'.
[xs_sdk-6] [INFO] Loaded mode configs from '/home/pi/interbotix_ws/install/interbotix_xsarm_moveit/share/interbotix_xsarm_moveit/config/modes.yaml'.
[xs_sdk-6] [INFO] Loaded motor configs from '/home/pi/interbotix_ws/install/interbotix_xsarm_control/share/interbotix_xsarm_control/config/wx250s.yaml'.
[xs_sdk-6] [INFO] Pinging all motors specified in the motor_config file. (Attempt 1/3)
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  8, Model: 'XL430-W250', Joint Name: 'wrist_rotate'.
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  7, Model: 'XM430-W350', Joint Name: 'wrist_angle'.
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  5, Model: 'XM430-W350', Joint Name: 'elbow_shadow'.
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  4, Model: 'XM430-W350', Joint Name: 'elbow'.
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  6, Model: 'XM430-W350', Joint Name: 'forearm_roll'.
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  2, Model: 'XM430-W350', Joint Name: 'shoulder'.
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  9, Model: 'XL430-W250', Joint Name: 'gripper'.
[move_group-1] [WARN] [1721040670.658504145] [move_group]: Falling back to using the the move_group node namespace (deprecated behavior).
[move_group-1] [INFO] [1721040670.661919200] [move_group.moveit.RDFLoader]: Loaded robot model in 0 seconds
[move_group-1] [INFO] [1721040670.662037239] [move_group.moveit.robot_model]: Loading robot model 'wx250s'...
[move_group-1] [INFO] [1721040670.662062868] [move_group.moveit.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  3, Model: 'XM430-W350', Joint Name: 'shoulder_shadow'.
[robot_state_publisher-8] [INFO] [1721040670.705650521] [robot_state_publisher]: Robot initialized
[xs_sdk-6] [INFO]   Found DYNAMIXEL ID:  1, Model: 'XM430-W350', Joint Name: 'waist'.
[ros2_control_node-3] [WARN] [1721040670.993268181] [wx250s.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-3] [INFO] [1721040670.993671722] [resource_manager]: Loading hardware 'XSHardwareInterface' 
[move_group-1] [INFO] [1721040671.111675193] [move_group.moveit.kdl_kinematics_plugin]: Joint weights for group 'interbotix_arm': 1 1 1 1 1 1
[ros2_control_node-3] [INFO] [1721040671.167734313] [resource_manager]: Initialize hardware 'XSHardwareInterface' 
[xs_sdk-6] [WARN] Writing startup register values to EEPROM. This only needs to be done once on a robot if using a default motor config file, or after a motor config file has been modified. Can set `write_eeprom_on_startup` to false from now on.
[move_group-1] [INFO] [1721040671.624735663] [move_group.moveit.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-1] [INFO] [1721040671.625069222] [move_group.moveit.moveit_cpp]: Listening to '/wx250s/joint_states' for joint states
[move_group-1] [INFO] [1721040671.629028154] [move_group.moveit.current_state_monitor]: Listening to joint states on topic '/wx250s/joint_states'
[move_group-1] [INFO] [1721040671.630848784] [move_group.moveit.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-1] [INFO] [1721040671.630946100] [move_group.moveit.planning_scene_monitor]: Starting planning scene monitor
[move_group-1] [INFO] [1721040671.632370652] [move_group.moveit.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-1] [INFO] [1721040671.632439579] [move_group.moveit.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-1] [INFO] [1721040671.636377936] [move_group.moveit.planning_scene_monitor]: Listening to 'collision_object'
[move_group-1] [INFO] [1721040671.637854322] [move_group.moveit.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-1] [WARN] [1721040671.639002724] [move_group.moveit.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-1] [ERROR] [1721040671.639103651] [move_group.moveit.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates
[move_group-1] terminate called after throwing an instance of 'rclcpp::ParameterTypeException'
[move_group-1]   what():  expected [string_array] got [string]
[move_group-1] Stack trace (most recent call last):
[move_group-1] #17   Object "/usr/lib/aarch64-linux-gnu/ld-linux-aarch64.so.1", at 0xffffffffffffffff, in 
[move_group-1] #16   Object "/opt/ros/rolling/lib/moveit_ros_move_group/move_group", at 0x5555d5101caf, in _start
[xs_sdk-6] [INFO] The operating mode for the 'arm' group was changed to 'position' with profile type 'velocity'.
[xs_sdk-6] [INFO] The operating mode for the 'gripper' joint was changed to 'linear_position' with profile type 'velocity'.
[xs_sdk-6] [INFO] Interbotix X-Series Driver is up!
[move_group-1] #15   Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7fff197b74cb]
[move_group-1] #14   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __aarch64_ldadd4_acq [0x7fff197b73fb]
[move_group-1] #13   Object "/opt/ros/rolling/lib/moveit_ros_move_group/move_group", at 0x5555d51007ff, in main
[move_group-1] #12   Object "/opt/ros/rolling/lib/libmoveit_cpp.so.2.9.0", at 0x7fff1a0deb1b, in moveit_cpp::MoveItCpp::MoveItCpp(std::shared_ptr<rclcpp::Node> const&, moveit_cpp::MoveItCpp::Options const&)
[move_group-1] #11   Object "/opt/ros/rolling/lib/libmoveit_cpp.so.2.9.0", at 0x7fff1a0dbab3, in moveit_cpp::MoveItCpp::loadPlanningPipelines(moveit_cpp::MoveItCpp::PlanningPipelineOptions const&)
[move_group-1] #10   Object "/opt/ros/rolling/lib/libmoveit_planning_pipeline_interfaces.so.2.9.0", at 0x7fff194b330b, in moveit::planning_pipeline_interfaces::createPlanningPipelineMap(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::shared_ptr<moveit::core::RobotModel const> const&, std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[move_group-1] #9    Object "/opt/ros/rolling/lib/libmoveit_planning_pipeline.so.2.9.0", at 0x7fff193762ab, in planning_pipeline::PlanningPipeline::PlanningPipeline(std::shared_ptr<moveit::core::RobotModel const> const&, std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[move_group-1] #8    Object "/opt/ros/rolling/lib/libmoveit_planning_pipeline.so.2.9.0", at 0x7fff19372727, in 
[move_group-1] #7    Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7fff19d5d1c3, in rclcpp::Parameter::as_string_array[abi:cxx11]() const
[move_group-1] #6    Object "/usr/lib/aarch64-linux-gnu/libstdc++.so.6.0.30", at 0x7fff19a12e03, in __cxa_throw
[move_group-1] #5    Object "/usr/lib/aarch64-linux-gnu/libstdc++.so.6.0.30", at 0x7fff19a12b1f, in std::terminate()
[move_group-1] #4    Object "/usr/lib/aarch64-linux-gnu/libstdc++.so.6.0.30", at 0x7fff19a12abb, in 
[move_group-1] #3    Object "/usr/lib/aarch64-linux-gnu/libstdc++.so.6.0.30", at 0x7fff19a162db, in __gnu_cxx::__verbose_terminate_handler()
[move_group-1] #2    Source "./stdlib/abort.c", line 79, in abort [0x7fff197b712f]
[move_group-1] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7fff197ca67b]
[move_group-1] #0    Source "./nptl/pthread_kill.c", line 44, in __pthread_kill_implementation [0x7fff1980f200]
[move_group-1] Aborted (Signal sent by tkill() 2023 1000)
[ros2_control_node-3] [INFO] [1721040672.068920154] [wx250s.xs_hardware_interface]: Joint states received.
[ros2_control_node-3] [INFO] [1721040672.069940998] [resource_manager]: Successful initialization of hardware 'XSHardwareInterface'
[ros2_control_node-3] [INFO] [1721040672.070634931] [resource_manager]: 'configure' hardware 'XSHardwareInterface' 
[ros2_control_node-3] [INFO] [1721040672.072336209] [resource_manager]: Successful 'configure' of hardware 'XSHardwareInterface'
[ros2_control_node-3] [INFO] [1721040672.072374635] [resource_manager]: 'activate' hardware 'XSHardwareInterface' 
[ros2_control_node-3] [INFO] [1721040672.072389432] [resource_manager]: Successful 'activate' of hardware 'XSHardwareInterface'
[ros2_control_node-3] [INFO] [1721040672.120508541] [wx250s.controller_manager]: update rate is 10 Hz
[ros2_control_node-3] [WARN] [1721040672.120889841] [wx250s.controller_manager]: Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT scheduling. See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details.
[xs_sdk-6] [INFO] [1721040672.138688977] [interbotix_xs_sdk.xs_sdk]: InterbotixRobotXS is up!
[ros2_control_node-3] [INFO] [1721040672.746909639] [wx250s.controller_manager]: Loading controller 'arm_controller'
[spawner-4] [INFO] [1721040673.222313023] [wx250s.arm_controller_spawner]: Loaded arm_controller
[ros2_control_node-3] [INFO] [1721040673.224385083] [wx250s.controller_manager]: Configuring controller 'arm_controller'
[ros2_control_node-3] [INFO] [1721040673.224704512] [wx250s.arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-3] [INFO] [1721040673.224748809] [wx250s.arm_controller]: Command interfaces are [position] and state interfaces are [position].
[ros2_control_node-3] [INFO] [1721040673.224766420] [wx250s.arm_controller]: Using 'splines' interpolation method.
[ros2_control_node-3] [INFO] [1721040673.401135722] [wx250s.arm_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-4] [INFO] [1721040673.722753308] [wx250s.arm_controller_spawner]: Configured and activated arm_controller
[ros2_control_node-3] [INFO] [1721040673.727736491] [wx250s.controller_manager]: Loading controller 'gripper_controller'
[spawner-5] [INFO] [1721040673.823646392] [wx250s.gripper_controller_spawner]: Loaded gripper_controller
[ros2_control_node-3] [INFO] [1721040673.827484026] [wx250s.controller_manager]: Configuring controller 'gripper_controller'
[ros2_control_node-3] [INFO] [1721040673.827659213] [wx250s.gripper_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-3] [INFO] [1721040673.827987568] [wx250s.gripper_controller]: Command interfaces are [position] and state interfaces are [position].
[ros2_control_node-3] [INFO] [1721040673.828023013] [wx250s.gripper_controller]: Using 'splines' interpolation method.
[ros2_control_node-3] [INFO] [1721040673.833413775] [wx250s.gripper_controller]: Action status changes will be monitored at 20.00 Hz.
[INFO] [spawner-4]: process has finished cleanly [pid 2029]
[spawner-5] [INFO] [1721040674.122584401] [wx250s.gripper_controller_spawner]: Configured and activated gripper_controller
[INFO] [spawner-5]: process has finished cleanly [pid 2031]
[ERROR] [move_group-1]: process has died [pid 2023, exit code -6, cmd '/opt/ros/rolling/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_ccsblgr1 --params-file /tmp/launch_params_b74ybehz --params-file /tmp/launch_params_a5o2h6xz --params-file /home/pi/interbotix_ws/install/interbotix_xsarm_moveit/share/interbotix_xsarm_moveit/config/kinematics.yaml --params-file /tmp/launch_params_3bhmy_1i --params-file /tmp/launch_params_8wouqxq0 --params-file /tmp/launch_params_nro9oaha --params-file /tmp/launch_params_eagc0g_1 --params-file /tmp/launch_params_6k0b1xh5 -r wx250s/get_planning_scene:=/wx250s/get_planning_scene -r /arm_controller/follow_joint_trajectory:=/wx250s/arm_controller/follow_joint_trajectory -r /gripper_controller/follow_joint_trajectory:=/wx250s/gripper_controller/follow_joint_trajectory'].
lukeschmitt-tr commented 3 months ago 
[move_group-1] terminate called after throwing an instance of 'rclcpp::ParameterTypeException'
[move_group-1]   what():  expected [string_array] got [string]
[move_group-1] Stack trace (most recent call last):
[move_group-1] #17   Object "/usr/lib/aarch64-linux-gnu/ld-linux-aarch64.so.1", at 0xffffffffffffffff, in 
[move_group-1] #16   Object "/opt/ros/rolling/lib/moveit_ros_move_group/move_group", at 0x5555d5101caf, in _start
[move_group-1] #15   Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7fff197b74cb]
[move_group-1] #14   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __aarch64_ldadd4_acq [0x7fff197b73fb]
[move_group-1] #13   Object "/opt/ros/rolling/lib/moveit_ros_move_group/move_group", at 0x5555d51007ff, in main
[move_group-1] #12   Object "/opt/ros/rolling/lib/libmoveit_cpp.so.2.9.0", at 0x7fff1a0deb1b, in moveit_cpp::MoveItCpp::MoveItCpp(std::shared_ptr<rclcpp::Node> const&, moveit_cpp::MoveItCpp::Options const&)
[move_group-1] #11   Object "/opt/ros/rolling/lib/libmoveit_cpp.so.2.9.0", at 0x7fff1a0dbab3, in moveit_cpp::MoveItCpp::loadPlanningPipelines(moveit_cpp::MoveItCpp::PlanningPipelineOptions const&)
[move_group-1] #10   Object "/opt/ros/rolling/lib/libmoveit_planning_pipeline_interfaces.so.2.9.0", at 0x7fff194b330b, in moveit::planning_pipeline_interfaces::createPlanningPipelineMap(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::shared_ptr<moveit::core::RobotModel const> const&, std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[move_group-1] #9    Object "/opt/ros/rolling/lib/libmoveit_planning_pipeline.so.2.9.0", at 0x7fff193762ab, in planning_pipeline::PlanningPipeline::PlanningPipeline(std::shared_ptr<moveit::core::RobotModel const> const&, std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[move_group-1] #8    Object "/opt/ros/rolling/lib/libmoveit_planning_pipeline.so.2.9.0", at 0x7fff19372727, in 
[move_group-1] #7    Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7fff19d5d1c3, in rclcpp::Parameter::as_string_array[abi:cxx11]() const
[move_group-1] #6    Object "/usr/lib/aarch64-linux-gnu/libstdc++.so.6.0.30", at 0x7fff19a12e03, in __cxa_throw
[move_group-1] #5    Object "/usr/lib/aarch64-linux-gnu/libstdc++.so.6.0.30", at 0x7fff19a12b1f, in std::terminate()
[move_group-1] #4    Object "/usr/lib/aarch64-linux-gnu/libstdc++.so.6.0.30", at 0x7fff19a12abb, in 
[move_group-1] #3    Object "/usr/lib/aarch64-linux-gnu/libstdc++.so.6.0.30", at 0x7fff19a162db, in __gnu_cxx::__verbose_terminate_handler()
[move_group-1] #2    Source "./stdlib/abort.c", line 79, in abort [0x7fff197b712f]
[move_group-1] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7fff197ca67b]
[move_group-1] #0    Source "./nptl/pthread_kill.c", line 44, in __pthread_kill_implementation [0x7fff1980f200]
[move_group-1] Aborted (Signal sent by tkill() 2023 1000)

Taking a look at the stack trace, we can see that move group node is throwing an exception. The type for some parameter has likely changed between distributions, specifically something used in the PlanningPipeline constructor. It is likely one of these parameters: https://github.com/moveit/moveit2/blob/2.9.0/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml#L2-L19

We currently do not have the bandwidth to do further investigation, but this is where I would start looking.