|
| 1 | +#!/usr/bin/env python3 |
| 2 | +""" |
| 3 | +Relay node that publishes Joy message to enable autonomy mode when goal_pose is received. |
| 4 | +Mimics the behavior of the goalpoint_rviz_plugin for Foxglove compatibility. |
| 5 | +""" |
| 6 | + |
| 7 | +from geometry_msgs.msg import PointStamped, PoseStamped |
| 8 | +import rclpy |
| 9 | +from rclpy.node import Node |
| 10 | +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy |
| 11 | +from sensor_msgs.msg import Joy |
| 12 | + |
| 13 | + |
| 14 | +class GoalAutonomyRelay(Node): |
| 15 | + def __init__(self): |
| 16 | + super().__init__("goal_autonomy_relay") |
| 17 | + |
| 18 | + # QoS for goal topics (match foxglove_bridge) |
| 19 | + goal_qos = QoSProfile( |
| 20 | + reliability=ReliabilityPolicy.RELIABLE, |
| 21 | + history=HistoryPolicy.KEEP_LAST, |
| 22 | + durability=DurabilityPolicy.VOLATILE, |
| 23 | + depth=5, |
| 24 | + ) |
| 25 | + |
| 26 | + # Subscribe to goal_pose (PoseStamped from Foxglove) |
| 27 | + self.pose_sub = self.create_subscription( |
| 28 | + PoseStamped, "/goal_pose", self.goal_pose_callback, goal_qos |
| 29 | + ) |
| 30 | + |
| 31 | + # Subscribe to way_point (PointStamped from Foxglove) |
| 32 | + self.point_sub = self.create_subscription( |
| 33 | + PointStamped, "/way_point", self.way_point_callback, goal_qos |
| 34 | + ) |
| 35 | + |
| 36 | + # Publisher for Joy message to enable autonomy |
| 37 | + self.joy_pub = self.create_publisher(Joy, "/joy", 5) |
| 38 | + |
| 39 | + self.get_logger().info( |
| 40 | + "Goal autonomy relay started - will publish Joy to enable autonomy when goals are received" |
| 41 | + ) |
| 42 | + |
| 43 | + def publish_autonomy_joy(self): |
| 44 | + """Publish Joy message that enables autonomy mode (mimics goalpoint_rviz_plugin)""" |
| 45 | + joy = Joy() |
| 46 | + joy.header.stamp = self.get_clock().now().to_msg() |
| 47 | + joy.header.frame_id = "goal_autonomy_relay" |
| 48 | + |
| 49 | + # axes[2] = -1.0 enables autonomy mode in pathFollower |
| 50 | + # axes[4] = 1.0 sets forward speed |
| 51 | + # axes[5] = 1.0 enables obstacle checking |
| 52 | + joy.axes = [0.0, 0.0, -1.0, 0.0, 1.0, 1.0, 0.0, 0.0] |
| 53 | + |
| 54 | + # buttons[7] = 1 (same as RViz plugin) |
| 55 | + joy.buttons = [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0] |
| 56 | + |
| 57 | + self.joy_pub.publish(joy) |
| 58 | + self.get_logger().info("Published Joy message to enable autonomy mode") |
| 59 | + |
| 60 | + def goal_pose_callback(self, msg: PoseStamped): |
| 61 | + self.get_logger().info( |
| 62 | + f"Received goal_pose at ({msg.pose.position.x:.2f}, {msg.pose.position.y:.2f})" |
| 63 | + ) |
| 64 | + self.publish_autonomy_joy() |
| 65 | + |
| 66 | + def way_point_callback(self, msg: PointStamped): |
| 67 | + self.get_logger().info(f"Received way_point at ({msg.point.x:.2f}, {msg.point.y:.2f})") |
| 68 | + self.publish_autonomy_joy() |
| 69 | + |
| 70 | + |
| 71 | +def main(args=None): |
| 72 | + rclpy.init(args=args) |
| 73 | + node = GoalAutonomyRelay() |
| 74 | + try: |
| 75 | + rclpy.spin(node) |
| 76 | + except KeyboardInterrupt: |
| 77 | + pass |
| 78 | + finally: |
| 79 | + node.destroy_node() |
| 80 | + rclpy.shutdown() |
| 81 | + |
| 82 | + |
| 83 | +if __name__ == "__main__": |
| 84 | + main() |
0 commit comments