Add launch_servo argument

[JenniferBuehler]

Adds the option to launch the moveit servo to the launch file.

Important

Depends on this PR.

Test plan

Note

If the PR is not merged yet: check out the branch for the related ur_moveit_config chages:

cd <your-ros2-ws>/src
git clone [email protected]:JenniferBuehler/Universal_Robots_ROS2_Driver.git -b fix/sim-time-to-servo-node

and re-build & source the setup.sh.
Use branch fix/sim-time-to-servo-node-jazzy instead if you're on jazzy - it has the same change.

Launch the simulation with the launch_servo:=true argument:

ros2 launch ur_simulation_gz ur_sim_moveit.launch.py ur_type:=ur5e launch_servo:=true

If the robot starts in some straight configuration (e.g. pointing to the sky), move it away from the singularity-close pose with the RViz interactive marker and Plan & Execute first (the moveit servo is finicky with those).

Add the forward position controller manually (doesn't get added by default yet) and switch to it

ros2 control load_controller --set-state inactive forward_position_controller

ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller --activate forward_position_controller --strict

For publishing pose commands, you have to set the mode to 2 (pose publish mode)

ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "
command_type: 2
"

Now, it would be nice to just publish a pose like this

ros2 topic pub /servo_node/pose_target_cmds geometry_msgs/msg/PoseStamped "
header:
  frame_id: base
pose:
  position:
    x: -0.620
    y: -0.109
    z: 0.5 
  orientation:
    x: 0.001
    y: 0.707
    z: -0.707
    w: 0.001
"

However, sadly this won't work because of the timestamp not being set. So put together a simple node to set it, e.g.

#!/usr/bin/env python3
import time
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped


class PosePublisher(Node):
    def __init__(self):
        super().__init__("pose_5s_pub")
        self.pub = self.create_publisher(
            PoseStamped,
            "/servo_node/pose_target_cmds",
            10,
        )
        self.start_time = self.get_clock().now()
        self.timer = self.create_timer(0.01, self.tick)

    def tick(self):
        now = self.get_clock().now()
        SECONDS = 10
        if (now - self.start_time).nanoseconds > SECONDS*1e9:
            self.get_logger().info("Done publishing pose")
            rclpy.shutdown()
            return

        msg = PoseStamped()
        msg.header.stamp = now.to_msg()
        msg.header.frame_id = "base"

        # target pose
        msg.pose.position.x = -0.6
        msg.pose.position.y = -0.2
        msg.pose.position.z = 0.6 
        msg.pose.orientation.x = 0.001
        msg.pose.orientation.y = 0.707
        msg.pose.orientation.z = -0.707
        msg.pose.orientation.w = 0.001

        self.pub.publish(msg)


def main():
    rclpy.init()
    rclpy.spin(PosePublisher())


if __name__ == "__main__":
    main()

Run it, and the robot will move.