|
| 1 | +#!/usr/bin/env python3 |
| 2 | + |
| 3 | +from autoware_adapi_v1_msgs.msg import OperationModeState |
| 4 | +from autoware_planning_msgs.msg import Trajectory |
| 5 | +from geometry_msgs.msg import PoseWithCovarianceStamped |
| 6 | +import rclpy |
| 7 | +from rclpy.node import Node |
| 8 | +from tier4_system_msgs.msg import OperationModeAvailability |
| 9 | + |
| 10 | + |
| 11 | +class RemapNode(Node): |
| 12 | + def __init__(self): |
| 13 | + super().__init__("remap_node") |
| 14 | + |
| 15 | + self.create_subscription( |
| 16 | + OperationModeAvailability, |
| 17 | + "/system/operation_mode/availability", |
| 18 | + self.operation_mode_callback, |
| 19 | + 10, |
| 20 | + ) |
| 21 | + self.create_subscription( |
| 22 | + OperationModeState, |
| 23 | + "/system/operation_mode/state", |
| 24 | + self.operation_mode_state_callback, |
| 25 | + 10, |
| 26 | + ) |
| 27 | + self.sub_trajectory = self.create_subscription( |
| 28 | + Trajectory, "/planning/scenario_planning/trajectory", self.trajectory_callback, 10 |
| 29 | + ) |
| 30 | + self.sub_pose_with_covariance = self.create_subscription( |
| 31 | + PoseWithCovarianceStamped, "/localization/pose_with_covariance", self.pose_callback, 10 |
| 32 | + ) |
| 33 | + # self.sub_initialpose3d = self.create_subscription(PoseWithCovarianceStamped, '/initialpose3d', self.initialpose_callback, 10) |
| 34 | + |
| 35 | + self.pub_trajectory = self.create_publisher( |
| 36 | + Trajectory, "/to_sub/planning/scenario_planning/trajectory", 10 |
| 37 | + ) |
| 38 | + self.pub_pose_with_covariance = self.create_publisher( |
| 39 | + PoseWithCovarianceStamped, "/to_sub/localization/pose_with_covariance", 10 |
| 40 | + ) |
| 41 | + # self.pub_initialpose3d = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/initialpose3d', 10) |
| 42 | + |
| 43 | + self.autonomous_mode = False |
| 44 | + self.operation_mode_autonomous_state = False |
| 45 | + self.get_logger().info(f"Initial autonomous mode: {self.autonomous_mode}") |
| 46 | + self.tmp_operation_mode_autonomous_state = False |
| 47 | + |
| 48 | + def operation_mode_callback(self, msg): |
| 49 | + if msg.autonomous != self.autonomous_mode: |
| 50 | + self.autonomous_mode = msg.autonomous |
| 51 | + self.get_logger().info(f"Autonomous mode changed: {self.autonomous_mode}") |
| 52 | + |
| 53 | + def operation_mode_state_callback(self, msg): |
| 54 | + self.tmp_operation_mode_autonomous_state = self.operation_mode_autonomous_state |
| 55 | + if msg.mode == 2: |
| 56 | + self.operation_mode_autonomous_state = True |
| 57 | + if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state: |
| 58 | + self.get_logger().info( |
| 59 | + f"Operation mode changed: {self.operation_mode_autonomous_state}" |
| 60 | + ) |
| 61 | + else: |
| 62 | + self.operation_mode_autonomous_state = False |
| 63 | + if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state: |
| 64 | + self.get_logger().info( |
| 65 | + f"Operation mode changed: {self.operation_mode_autonomous_state}" |
| 66 | + ) |
| 67 | + |
| 68 | + def trajectory_callback(self, msg): |
| 69 | + if not self.autonomous_mode or self.operation_mode_autonomous_state: |
| 70 | + self.pub_trajectory.publish(msg) |
| 71 | + |
| 72 | + def pose_callback(self, msg): |
| 73 | + if not self.autonomous_mode or self.operation_mode_autonomous_state: |
| 74 | + self.pub_pose_with_covariance.publish(msg) |
| 75 | + |
| 76 | + # def initialpose_callback(self, msg): |
| 77 | + # if self.autonomous_mode: |
| 78 | + # self.pub_initialpose3d.publish(msg) |
| 79 | + |
| 80 | + |
| 81 | +def main(args=None): |
| 82 | + rclpy.init(args=args) |
| 83 | + node = RemapNode() |
| 84 | + rclpy.spin(node) |
| 85 | + node.destroy_node() |
| 86 | + rclpy.shutdown() |
| 87 | + |
| 88 | + |
| 89 | +if __name__ == "__main__": |
| 90 | + main() |
0 commit comments