[ROS2] Implement MoveIt Servo

[amalnanavati]

MoveIt Servo promises to be an easy way to enable users to teleoperate the robot (while respecting collisions), and might make it easier to do cartesian motions of the robot. This issue exists to track progress on implementing MoveIt Servo for ADA. The corresponding PRs are ada_feeding#118, ada_ros2#23, pr_ros_controllers#28, ada_ros2#24, ada_ros2#25.

Anticipated steps are below. Note that this can only be tested on real, since we will be using velocity control which doesn't exist in sim.

• [x] Implement a keyboard teleop node in ada_feeding to send twist commands to MoveIt Servo, and 0 velocity commands when a key is not being pressed.
• [x] Get MoveIt Servo working reliably using that node with a JointGroupVelocityController.
• [x] Verify that MoveIt2 can switch between the two controllers depending on what motions are commanded.
• [x] Implement a force gated version of the JointGroupVelocityController.
• [x] Create a ROS node to handle controller switching and starting/stopping MoveIt Servo.
• [x] Thoroughly test it, including ensuring we have desired behavior near singularities

[amalnanavati]

Moving offline discussion here:

MoveIt's Controller Managers don't support controller switching for MoveIt Servo. Therefore, in order to enable MoveItServo, we must first:

• Switch Controllers: ros2 service call /controller_manager/switch_controller controller_manager_msgs/srv/SwitchController "{activate_controllers: [\"jaco_arm_servo_controller\"], deactivate_controllers: [\"jaco_arm_controller\"], start_controllers: [], stop_controllers: [], strictness: 0, start_asap: false, activate_asap: false, timeout: {sec: 0, nanosec: 0}}"
  • NOTE: I haven't yet looked at all the parameters to determine whether the above call is actually the right one.
• Start Servo: ros2 service call /servo_node/start_servo std_srvs/srv/Trigger "{}"

We should create a ROS node that provides a single service to handle both.

[amalnanavati]

The above 5 PRs close this issue. I'll branch off a new issue for the ++ improvements to MoveIt Servo.