Moveit_servo not working / crashing

[fmauch]

Affected ROS2 Driver version(s)

c4d8d6138e09d3be3737f9b047a459c259075da2

Used ROS distribution.

Rolling

Which combination of platform is the ROS driver running on.

Other Linux system

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR E-series robot, URSim in docker

Robot SW / URSim version(s)

latest

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

I was not able to get the MoveIt! servo_node running

Issue details

When reviewing #854 I also wanted to try interfacing with the servo_node. However, doing so always resulted in a crash.

Steps to Reproduce

1. Start the driver with ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101
2. Start moveit using ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e
3. Start the external control program on the TP
4. Switch controllers ros2 control switch_controllers --activate forward_position_controller --deactivate scaled_joint_trajectory_controller
5. Verify controllers with ros2 control list_controllers

   forward_position_controller[position_controllers/JointGroupPositionController] active
   joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
   io_and_status_controller[ur_controllers/GPIOController] active
   force_torque_sensor_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active
   speed_scaling_state_broadcaster[ur_controllers/SpeedScalingStateBroadcaster] active
   scaled_joint_trajectory_controller[ur_controllers/ScaledJointTrajectoryController] inactive

6. Configure servo with ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "command_type: 1"
7. Send a command

   ros2 topic pub /servo_node/delta_twist_cmds geometry_msgs/msg/TwistStamped "header:
     stamp:
       sec: 0
       nanosec: 0
     frame_id: 'base_link'
   twist:
     linear:
       x: 0.0
       y: 0.0
       z: 0.1
     angular:
       x: 0.0
       y: 0.0
       z: 0.0"

Expected Behavior

I'm not that familiar with moveit_servo but I would expect the arm to move up 10 cm

Actual Behavior

Crash:

[servo_node-3] Stack trace (most recent call last) in thread 514086:
[servo_node-3] #6    Object "", at 0xffffffffffffffff, in
[servo_node-3] #5    Source "../sysdeps/unix/sysv/linux/x86_64/clone3.S", line 81, in __clone3 [0x7f3f906eca3f]
[servo_node-3] #4    Source "./nptl/pthread_create.c", line 442, in start_thread [0x7f3f9065aac2]
[servo_node-3] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3f908ea252, in
[servo_node-3] #2    Object "/opt/ros/rolling/lib/libmoveit_servo_lib_ros.so.2.8.0", at 0x7f3f8c6f17f1, in moveit_servo::ServoNode::servoLoop()
[servo_node-3] #1    Object "/opt/ros/rolling/lib/libmoveit_servo_lib_ros.so.2.8.0", at 0x7f3f8c6efc94, in moveit_servo::ServoNode::processTwistCommand()
[servo_node-3] #0    Object "/opt/ros/rolling/lib/libmoveit_servo_lib_cpp.so.2.8.0", at 0x7f3f8c5fc0f4, in moveit_servo::Servo::smoothHalt(moveit_servo::KinematicState)
[servo_node-3] Segmentation fault (Address not mapped to object [(nil)])
[ERROR] [servo_node-3]: process has died [pid 514008, exit code -11, cmd '/opt/ros/rolling/lib/moveit_servo/servo_node --ros-args --params-file /tmp/launch_params_u352z3f_ --params-file /tmp/launch_params_gpurb6uz --params-file /tmp/launch_params_isuklilv'].

I am not sure whether this results from a broken MoveIt on my current rolling setup or this is the result of wrong parameters from our side, though. To verify one would probably have to compile moveit from src and debug the crash.

Workaround Suggestion

-

Relevant log output

No response

Accept Public visibility

• [X] I agree to make this context public

[eladpar]

Maybe check my comment at #630 I would check where the robot is at, maybe the command is to far for the robot (happend to me)