[Knowledge sharing] Multi-robot navigation - more fun - goal splitter

[peterborkuti]

Hi,

Instead of manually setting in rviz2 the goal position topic, I wrote a “splitter”. It listens to the goal_topic and pass the goal to one of the robots.
It was easy to write, but if you do not want to spend time with it:

import rclpy
from rclpy.node import Node

from geometry_msgs.msg import PoseStamped
from std_msgs.msg import String


class GoalPoseSplitter(Node):
    def __init__(self):
        super().__init__('goal_pose_splitter')
        self.splitter = 0
        self.tb03 = [
            self.create_publisher(PoseStamped, 'tb3_0/goal_pose', 10),
            self.create_publisher(PoseStamped, 'tb3_1/goal_pose', 10)
        ]

        self.subscription = self.create_subscription(
            PoseStamped,
            'goal_pose',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg: PoseStamped):
        self.get_logger().info('Publishing goal to robot %d -> %s' % (self.splitter, str(msg.pose)))
        self.tb03[self.splitter].publish(msg)
        self.splitter = 1 - self.splitter

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

    minimal_publisher = GoalPoseSplitter()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()