feat(dummy_operation_mode_publisher): add an adapi topic (#1510)

* feat(dummy_operation_mode_publisher): add an adapi topic

Signed-off-by: Makoto Kurihara <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Makoto Kurihara <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml

@@ -3,5 +3,6 @@
 
   <node pkg="dummy_operation_mode_publisher" exec="dummy_operation_mode_publisher_node" name="dummy_operation_mode_publisher" output="screen">
     <remap from="~/output/operation_mode_state" to="/system/operation_mode/state"/>
+    <remap from="~/output/operation_mode_state_adapi" to="/api/operation_mode/state"/>
   </node>
 </launch>

dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp

@@ -27,6 +27,9 @@ DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptio
   // Publisher
   pub_operation_mode_state_ = create_publisher<autoware_adapi_v1_msgs::msg::OperationModeState>(
     "~/output/operation_mode_state", 10);
+  pub_operation_mode_state_adapi_ =
+    create_publisher<autoware_adapi_v1_msgs::msg::OperationModeState>(
+      "~/output/operation_mode_state_adapi", 10);
 
   // Service
 
@@ -55,6 +58,7 @@ void DummyOperationModePublisher::onTimer()
   msg.is_remote_mode_available = true;
 
   pub_operation_mode_state_->publish(msg);
+  pub_operation_mode_state_adapi_->publish(msg);
 }
 
 }  // namespace dummy_operation_mode_publisher

dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp

@@ -37,6 +37,8 @@ class DummyOperationModePublisher : public rclcpp::Node
   // Publisher
   rclcpp::Publisher<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
     pub_operation_mode_state_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
+    pub_operation_mode_state_adapi_;
 
   // Service
 

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -336,7 +336,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
   RCLCPP_INFO(get_logger(), "external velocity limit pointer exist!!!");
   // sender
   external_velocity_limit_.sender = external_velocity_limit_ptr_->sender;
-  RCLCPP_INFO(get_logger(), "external velocity limit sender: %s", external_velocity_limit_.sender.c_str());
+  RCLCPP_INFO(
+    get_logger(), "external velocity limit sender: %s", external_velocity_limit_.sender.c_str());
 
   // on the first time, apply directly
   if (prev_output_.empty() || !current_closest_point_from_prev_output_) {
@@ -378,7 +379,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
         external_velocity_limit_ptr_->use_constraints ? cstr.max_jerk : smoother_->getMaxJerk();
       const auto j_min =
         external_velocity_limit_ptr_->use_constraints ? cstr.min_jerk : smoother_->getMinJerk();
-      RCLCPP_INFO(get_logger(), "constraints: a_min = %f, j_max = %f, j_min = %f", a_min, j_max, j_min);
+      RCLCPP_INFO(
+        get_logger(), "constraints: a_min = %f, j_max = %f, j_min = %f", a_min, j_max, j_min);
 
       // If the closest acceleration is positive, velocity will increase
       // until the acceleration becomes zero
@@ -388,7 +390,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
       } else {
         max_velocity_with_deceleration_ = v0;
       }
-      RCLCPP_INFO(get_logger(), "max_velocity_with_deceleration = %f", max_velocity_with_deceleration_);
+      RCLCPP_INFO(
+        get_logger(), "max_velocity_with_deceleration = %f", max_velocity_with_deceleration_);
 
       if (external_velocity_limit_ptr_->max_velocity < max_velocity_with_deceleration_) {
         // TODO(mkuri) If v0 < external_velocity_limit_ptr_->max_velocity <
@@ -409,11 +412,13 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
           RCLCPP_WARN(get_logger(), "Stop distance calculation failed!");
         }
         external_velocity_limit_.dist = stop_dist + margin;
-        RCLCPP_INFO(get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
+        RCLCPP_INFO(
+          get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
       } else {
         max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity;
         external_velocity_limit_.dist = 0.0;
-        RCLCPP_INFO(get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
+        RCLCPP_INFO(
+          get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
       }
     }
   }
@@ -910,7 +915,8 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t
   // apply external velocity limit from the inserted point
   trajectory_utils::applyMaximumVelocityLimit(
     *inserted_index, traj.size(), external_velocity_limit_.velocity, traj);
-  RCLCPP_INFO(get_logger(), "apply external velocity limit: dist = %f, vel = %f",
+  RCLCPP_INFO(
+    get_logger(), "apply external velocity limit: dist = %f, vel = %f",
     external_velocity_limit_.dist, external_velocity_limit_.velocity);
 
   // create virtual wall

system/leader_election_converter/package.xml

@@ -11,13 +11,12 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>geometry_msgs</depend>
   <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

@@ -1,25 +1,43 @@
 #!/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
+import rclpy
+from rclpy.node import Node
 from tier4_system_msgs.msg import OperationModeAvailability
 
-class RemapNode(Node):
 
+class RemapNode(Node):
     def __init__(self):
-        super().__init__('remap_node')
+        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.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_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
@@ -36,7 +54,6 @@ def operation_mode_state_callback(self, msg):
             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:
@@ -50,12 +67,14 @@ def pose_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__':
+
+if __name__ == "__main__":
     main()