Write your first ROS node - LCAS/RBT1001 GitHub Wiki

On VSCode, open the file RBT1001/src/week1/week1/turtles_controller.py. This is a template that is already pre-filled with some definitions and statements to make it easier to write your first ROS node.

A python ROS should start with a main function, such as:

def main(args=None):
    rclpy.init(args=args)

    turtles_controller = TurtlesController()

    rclpy.spin(turtles_controller)

    turtles_controller.destroy_node()
    rclpy.shutdown()

where rclpy.spin() is a blocking function that starts the node and waits until it terminates.

The node itself, is a class that inherits the Node object:

class TurtlesController(Node):

    def __init__(self):
        # Defines the name of the node
        super().__init__('turtles_controller')
        ...

Once this is launched, you will be able to see the turtles_controller node in the ROS network.

from geometry_msgs.msg import Twist

class TurtlesController(Node):
    def __init__(self):
        ...
        self.t1_cmd_pub = self.create_publisher(
            Twist, 
            '/turtle1/cmd_vel', 
            10
        )

create_publisher declares that the node publishes messages of type Twist (imported from the geometry_msgs.msg module), over a topic named /turtle1/cmd_vel, and that the “queue size” is 10. Queue size is a required QoS (quality of service) setting that limits the amount of queued messages if a subscriber is not receiving them fast enough.

To publish a message, we need to create a Twist message object first and then call the publisher's publish() function. For example:

        ...
        msg = Twist()
        msg.angular.z = 0.7 * self.t1_rot_direction
        self.t1_cmd_pub.publish(msg)
        ...     

In a similar way, subscribing to receive a topic's messages can be done by calling the create_subscription method. This time, we also need to pass a callback function, called t1_pose_cb in the example below, which receives the messages.

class TurtlesController(Node):
    def __init__(self):
        ...
        self.t1_pose_sub = self.create_subscription(
            Pose,
            "/turtle1/pose",
            self.t1_pose_cb,
            10
        )
    ...
    def t1_pose_cb(self, msg):
        self.get_logger().debug('I got turtle1 pose: "%s"' % msg)
        self.t1_pose = msg

The following code creates a service called reset_controller, defines its type (Empty) and the callback function that will be called upon receiving a service request. The callback receives the request and will return a response message. In this case, both are empty messages.

      ...
      self.reset_srv = self.create_service(Empty, 'reset_controller', self.reset_cb)
      ...
      def reset_cb(self, request, response):
         ...
         return response

To call services advertised by other nodes, we need to create a service client that connects to the corresponding server. In the example below, we create a client to the /turtle1/set_pen service. Note that it is possible that the service is not reachable (for example, because it's ROS node has not been launched or because there's a connection issue), therefore, we need to make sure that it is reachable before sending a service request.

        ...
        self.t1_pen_cli = self.create_client(SetPen, '/turtle1/set_pen')
        while not self.t1_pen_cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')

        t1_pen_req = SetPen.Request()

        self.future = self.t1_pen_cli.call_async(t1_pen_req)
        rclpy.spin_until_future_complete(self, self.future)
        ...

The client call_async method is non-blocking - it calls the service and returns the response once finished. rclpy.spin_until_future_complete is needed to block execution until the service call has been fulfilled.

In this last part of the workshop, you will have to implement the code to control multiple turtles in the turtlesim. The code must:

• At the beginning, spawn 3 turtles at random <x,y> locations within the range 0 <= x/y <= 10. Each turtle should have a different pen colour.

• Prevents the turtles from moving outside the square window, and teleport them to a random location when they reach the border.

• Implement a control algorithm for turtle2 and turtle3, so that they will both try to chase turtle1.

• If any turtle collides with another turtle in their back with their head, the hit turtle will be killed and re-spawned in a random location.

• Advertise a /reset_controller service that restarts the simulation and controller, i.e., clear everything and re-spawn 3 turtles at random locations.

• Q:How do I teleport/kill/spawn a turtle? R: inspect the turtlesim node from a terminal...
• Q:How do I get the position of turtle2? R: create a new subscriber to listen to its pose.
• Q:How do I check the position of collisions (head or back)? R: use the orientation of the turtles.
• Q:I have modified my code and saved the file, but the execution remained unchanged? R: you need to run rebuild everytime you make a modification to a ROS file.