feat: add relay to sub scropt

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

system/leader_election_converter/CMakeLists.txt

@@ -27,4 +27,10 @@ rclcpp_components_register_node(${PROJECT_NAME}
   EXECUTOR MultiThreadedExecutor
 )
 
+install(PROGRAMS
+  script/relay_to_sub.py
+  DESTINATION lib/${PROJECT_NAME}
+  PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ
+)
+
 ament_auto_package(INSTALL_TO_SHARE config launch)

system/leader_election_converter/package.xml

@@ -15,6 +15,9 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tier4_system_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>geometry_msgs</depend>
+
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

system/leader_election_converter/script/relay_to_sub.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from autoware_adapi_v1_msgs.msg import OperationModeState
+from autoware_auto_planning_msgs.msg import Trajectory
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from tier4_system_msgs.msg import OperationModeAvailability
+
+class RemapNode(Node):
+
+    def __init__(self):
+        super().__init__('remap_node')
+
+        self.create_subscription(OperationModeAvailability, '/system/operation_mode/availability', self.operation_mode_callback, 10)
+        self.create_subscription(OperationModeState, '/system/operation_mode/state', self.operation_mode_state_callback, 10)
+        self.sub_trajectory = self.create_subscription(Trajectory, '/planning/scenario_planning/trajectory', self.trajectory_callback, 10)
+        self.sub_pose_with_covariance = self.create_subscription(PoseWithCovarianceStamped, '/localization/pose_with_covariance', self.pose_callback, 10)
+        # self.sub_initialpose3d = self.create_subscription(PoseWithCovarianceStamped, '/initialpose3d', self.initialpose_callback, 10)
+
+        self.pub_trajectory = self.create_publisher(Trajectory, '/to_sub/planning/scenario_planning/trajectory', 10)
+        self.pub_pose_with_covariance = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/localization/pose_with_covariance', 10)
+        # self.pub_initialpose3d = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/initialpose3d', 10)
+
+        self.autonomous_mode = False
+        self.operation_mode_autonomous_state = False
+        self.get_logger().info(f"Initial autonomous mode: {self.autonomous_mode}")
+
+    def operation_mode_callback(self, msg):
+        if msg.autonomous != self.autonomous_mode:
+            self.autonomous_mode = msg.autonomous
+            self.get_logger().info(f"Autonomous mode changed: {self.autonomous_mode}")
+
+    def operation_mode_state_callback(self, msg):
+        if msg.mode == 2:
+            self.operation_mode_autonomous_state == True
+        else:
+            self.operation_mode_autonomous_state == False
+
+
+    def trajectory_callback(self, msg):
+        if self.autonomous_mode or self.operation_mode_autonomous_state == False:
+            self.pub_trajectory.publish(msg)
+
+    def pose_callback(self, msg):
+        if self.autonomous_mode or self.operation_mode_autonomous_state == False:
+            self.pub_pose_with_covariance.publish(msg)
+
+    # def initialpose_callback(self, msg):
+    #     if self.autonomous_mode:
+    #         self.pub_initialpose3d.publish(msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RemapNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()