Hello World (ROS2 Python)

Hello World example using ROS2 in Python to communicate with the CreOS SDK ROS2 node.

This example demonstrates how to use ROS2 in Python to communicate with the CreOS SDK ROS2 node. Below you can find the full source code. You can also find a more detailed explanation of the code.

Source code

main.py

 
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import NavSatFix, NavSatStatus
from creos_sdk_msgs.srv import EnterLowPowerMode
 
class HelloWorldNode(Node):
    def __init__(self):
        super().__init__('hello_world_node')
 
        self.publisher = self.create_publisher(Twist, 'cmd_vel', QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE))
        self.publish_timer = self.create_timer(1, self.publish_callback)
 
        self.subscription = self.create_subscription(NavSatFix, 'gnss', self.gps_callback, QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE))
 
        self.client = self.create_client(EnterLowPowerMode, 'enter_low_power_mode')
 
        powermode_request = EnterLowPowerMode.Request()
 
        powermode_future = self.client.call_async(powermode_request)
 
        powermode_future.add_done_callback(self.powermode_callback)
 
    def publish_callback(self):
        msg = Twist()
        msg.linear.x = 0.5
        msg.angular.z = 0.1
        self.publisher.publish(msg)
 
    def gps_callback(self, msg: NavSatFix):
        status = 'UNKNOWN'
        match msg.status.status:
            case NavSatStatus.STATUS_NO_FIX:
                status = 'NO_FIX'
            case NavSatStatus.STATUS_FIX:
                status = 'FIX'
            case NavSatStatus.STATUS_SBAS_FIX:
                status = 'SBAS_FIX'
            case NavSatStatus.STATUS_GBAS_FIX:
                status = 'GBAS_FIX'
            case _:
                status = 'UNKNOWN'
        self.get_logger().info("Received GNSS message with status: %s" % status)
 
    def powermode_callback(self, future):
        try:
            response = future.result()
            self.get_logger().info("Received response from EnterLowPowerMode service: %s" % response.success)
        except Exception as e:
            self.get_logger().error("Service call failed %r" % (e,))
 
def main():
    rclpy.init()
    node = HelloWorldNode()
    rclpy.spin(node)
 
 
if __name__ == '__main__':
    main()
 

Explanation

At the top of the main.py file, we import the necessary modules for the example. Specifically, we import some rclpy modules to interact with ROS2, and the necessary messages packages to communicate with the CreOS SDK ROS2 node. These also include the EnterLowPowerMode service type from the creos_sdk_msgs package.

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import NavSatFix, NavSatStatus
from creos_sdk_msgs.srv import EnterLowPowerMode

For this example, we create a HelloWorldNode class that extends from rclpy.node.Node. Most of our implementation will be inside this class' constructor.

class HelloWorldNode(Node):
    def __init__(self):
        super().__init__('hello_world_node')

We create a publisher that publishes geometry_msgs.msg.Twist messages to the cmd_vel topic. This message contains the linear and angular velocity of the robot. We also create a timer that triggers every second. Note that we match the Quality of Service settings with the settings used by the CreOS SDK ROS2 node, as specified in the Input Topics section.

        self.publisher = self.create_publisher(Twist, 'cmd_vel', QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE))
        self.publish_timer = self.create_timer(1, self.publish_callback)

In the callback of the timer, we create a geometry_msgs.msg.Twist message and publish it.

    def publish_callback(self):
        msg = Twist()
        msg.linear.x = 0.5
        msg.angular.z = 0.1
        self.publisher.publish(msg)

Next, we create a subscriber that subscribes to the gnss topic. Note that we also match the Quality of Service settings here.

        self.subscription = self.create_subscription(NavSatFix, 'gnss', self.gps_callback, QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE))

In the callback of the subscriber, we extract the NavSatStatus from the received message, and print the status in a human-readable format.

    def gps_callback(self, msg: NavSatFix):
        status = 'UNKNOWN'
        match msg.status.status:
            case NavSatStatus.STATUS_NO_FIX:
                status = 'NO_FIX'
            case NavSatStatus.STATUS_FIX:
                status = 'FIX'
            case NavSatStatus.STATUS_SBAS_FIX:
                status = 'SBAS_FIX'
            case NavSatStatus.STATUS_GBAS_FIX:
                status = 'GBAS_FIX'
            case _:
                status = 'UNKNOWN'
        self.get_logger().info("Received GNSS message with status: %s" % status)

We also create a service client that calls the enter_low_power_mode service. We immediately use this client to send an asynchronous request to the service, and add a done callback to returned future.

        self.client = self.create_client(EnterLowPowerMode, 'enter_low_power_mode')
 
        powermode_request = EnterLowPowerMode.Request()
 
        powermode_future = self.client.call_async(powermode_request)
 
        powermode_future.add_done_callback(self.powermode_callback)

In the done callback, we print whether the transition to low-power mode was successful. If any exception has happened during the request, we print the failure to the console as well.

    def powermode_callback(self, future):
        try:
            response = future.result()
            self.get_logger().info("Received response from EnterLowPowerMode service: %s" % response.success)
        except Exception as e:
            self.get_logger().error("Service call failed %r" % (e,))

In the main function, we simply create a new instance of our HelloWorldNode class and spin the node.

def main():
    rclpy.init()
    node = HelloWorldNode()
    rclpy.spin(node)
 
 
if __name__ == '__main__':
    main()