Merge pull request #1368 from tier4/feat/add-leader-election-converter

modify: fix qos setting

dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp

@@ -34,12 +34,12 @@ DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptio
 
   // Timer
   using namespace std::literals::chrono_literals;
-  timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this));
+  timer_ = rclcpp::create_timer(
+    this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this));
 
   // State
 
   // Diagnostics
-
 }
 
 void DummyOperationModePublisher::onTimer()

dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp

@@ -12,15 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_
-#define DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_
+#ifndef DUMMY_OPERATION_MODE_PUBLISHER_HPP_
+#define DUMMY_OPERATION_MODE_PUBLISHER_HPP_
 
 // include
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 
-
 namespace dummy_operation_mode_publisher
 {
 
@@ -36,7 +35,8 @@ class DummyOperationModePublisher : public rclcpp::Node
   // Subscriber
 
   // 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_;
 
   // Service
 
@@ -50,8 +50,7 @@ class DummyOperationModePublisher : public rclcpp::Node
   // State
 
   // Diagnostics
-
 };
 }  // namespace dummy_operation_mode_publisher
 
-#endif  // DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_
+#endif  // DUMMY_OPERATION_MODE_PUBLISHER_HPP_

launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml

@@ -160,7 +160,7 @@
     <include file="$(find-pkg-share control_cmd_switcher)/launch/control_cmd_switcher.launch.xml"/>
   </group>
 
-    <!-- Leader Election Converter-->
+  <!-- Leader Election Converter-->
   <group>
     <include file="$(find-pkg-share leader_election_converter)/launch/leader_election_converter.launch.xml"/>
   </group>
@@ -187,6 +187,4 @@
       <arg name="timeout" value=""/>
     </include> -->
   </group>
-
-
 </launch>

launch/tier4_system_launch/package.xml

@@ -12,11 +12,11 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <exec_depend>component_state_monitor</exec_depend>
+  <exec_depend>control_cmd_switcher</exec_depend>
   <exec_depend>emergency_handler</exec_depend>
+  <exec_depend>leader_election_converter</exec_depend>
   <exec_depend>system_error_monitor</exec_depend>
   <exec_depend>system_monitor</exec_depend>
-  <exec_depend>control_cmd_switcher</exec_depend>
-  <exec_depend>leader_election_converter</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

system/control_cmd_switcher/README.md

@@ -1 +1 @@
-# control_cmd_switcher
+# control_cmd_switcher

system/control_cmd_switcher/config/control_cmd_switcher.yaml

@@ -2,4 +2,3 @@
 ---
 /**:
   ros__parameters:
-

system/control_cmd_switcher/package.xml

@@ -13,8 +13,8 @@
 
   <depend>autoware_auto_control_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>tier4_system_msgs</depend>
-    <depend>rclcpp_components</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp

@@ -18,17 +18,23 @@
 #include <string>
 #include <utility>
 
-ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) : Node("control_cmd_switcher", node_options)
+ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options)
+: Node("control_cmd_switcher", node_options)
 {
   // Subscriber
-  sub_main_control_cmd_ = create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
-    "~/input/main/control_cmd", rclcpp::QoS{10}, std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1));
+  sub_main_control_cmd_ =
+    create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
+      "~/input/main/control_cmd", rclcpp::QoS{10},
+      std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1));
 
-  sub_sub_control_cmd_ = create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
-    "~/input/sub/control_cmd", rclcpp::QoS{10}, std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1));
+  sub_sub_control_cmd_ =
+    create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
+      "~/input/sub/control_cmd", rclcpp::QoS{10},
+      std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1));
 
   sub_election_status = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
-    "~/input/election/status", rclcpp::QoS{10}, std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1));
+    "~/input/election/status", rclcpp::QoS{10},
+    std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1));
   // Publisher
   pub_control_cmd_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_cmd", rclcpp::QoS{1});
@@ -37,21 +43,24 @@ ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options)
   use_main_control_cmd_ = true;
 }
 
-void ControlCmdSwitcher::onMainControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
+void ControlCmdSwitcher::onMainControlCmd(
+  const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
 {
   if (use_main_control_cmd_) {
     pub_control_cmd_->publish(*msg);
   }
 }
 
-void ControlCmdSwitcher::onSubControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
+void ControlCmdSwitcher::onSubControlCmd(
+  const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
 {
   if (!use_main_control_cmd_) {
     pub_control_cmd_->publish(*msg);
   }
 }
 
-void ControlCmdSwitcher::onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg)
+void ControlCmdSwitcher::onElectionStatus(
+  const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg)
 {
   if (((msg->path_info >> 3) & 0x01) == 1) {
     use_main_control_cmd_ = true;
@@ -62,4 +71,3 @@ void ControlCmdSwitcher::onElectionStatus(const tier4_system_msgs::msg::Election
 
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(ControlCmdSwitcher)
-

system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp

@@ -12,14 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef CONTROL_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
-#define CONTROL_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
+#ifndef CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
+#define CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
 
 // Core
+#include <atomic>
 #include <memory>
 #include <string>
-#include <atomic>
-
 
 // Autoware
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
@@ -28,28 +27,29 @@
 // ROS 2 core
 #include <rclcpp/rclcpp.hpp>
 
-
 class ControlCmdSwitcher : public rclcpp::Node
 {
 public:
   explicit ControlCmdSwitcher(const rclcpp::NodeOptions & node_options);
 
 private:
-
   // Subscribers
-  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr sub_main_control_cmd_;
-  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr sub_sub_control_cmd_;
+  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    sub_main_control_cmd_;
+  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    sub_sub_control_cmd_;
   rclcpp::Subscription<tier4_system_msgs::msg::ElectionStatus>::SharedPtr sub_election_status;
-  void onMainControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
-  void onSubControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
+  void onMainControlCmd(
+    const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
+  void onSubControlCmd(
+    const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
   void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg);
 
-
   // Publisher
-  rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr pub_control_cmd_;
-
+  rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    pub_control_cmd_;
 
   std::atomic<bool> use_main_control_cmd_;
 };
 
-#endif  // CONTROL_SWITCHER__CONTROL_SWITCHER_HPP_
+#endif  // CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_

system/control_cmd_switcher/tool/rely_trajectory.py

@@ -1,26 +1,28 @@
 #!/usr/bin/env python3
 
+import threading
+
+from autoware_auto_planning_msgs.msg import Trajectory
 import rclpy
 from rclpy.node import Node
-from autoware_auto_planning_msgs.msg import Trajectory
-import threading
 
-class RelayTrajectoryNode(Node):
 
+class RelayTrajectoryNode(Node):
     def __init__(self):
-        super().__init__('relay_trajectory')
+        super().__init__("relay_trajectory")
         self.subscription = self.create_subscription(
-            Trajectory,
-            '/tmp/planning/scenario_planning/trajectory',
-            self.listener_callback,
-            10)
-        self.publisher = self.create_publisher(Trajectory, '/planning/scenario_planning/trajectory', 10)
+            Trajectory, "/tmp/planning/scenario_planning/trajectory", self.listener_callback, 10
+        )
+        self.publisher = self.create_publisher(
+            Trajectory, "/planning/scenario_planning/trajectory", 10
+        )
         self.running = True
 
     def listener_callback(self, msg):
         if self.running:
             self.publisher.publish(msg)
 
+
 def main(args=None):
     rclpy.init(args=args)
     node = RelayTrajectoryNode()
@@ -29,7 +31,7 @@ def input_thread():
         nonlocal node
         while True:
             user_input = input("Enter 'y' to stop publishing: ")
-            if user_input.lower() == 'y':
+            if user_input.lower() == "y":
                 node.running = False
                 print("Publishing stopped.")
                 break
@@ -43,5 +45,6 @@ def input_thread():
     node.destroy_node()
     rclpy.shutdown()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()

system/leader_election_converter/src/common/converter/availability_converter.cpp

@@ -33,7 +33,7 @@ void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std:
 
 void AvailabilityConverter::setSubscriber()
 {
-  const auto qos = rclcpp::QoS(1).transient_local();
+  const auto qos = rclcpp::QoS(1);
   availability_callback_group_ =
     node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   rclcpp::SubscriptionOptions availability_options = rclcpp::SubscriptionOptions();
@@ -75,7 +75,8 @@ void AvailabilityConverter::convertToUdp(
     availability.pull_over = availability_msg->pull_over;
     udp_availability_sender_->send(availability);
   } else {
-    RCLCPP_ERROR(node_->get_logger(), "Failed to take control mode report");
+    RCLCPP_ERROR_THROTTLE(
+      node_->get_logger(), *node->get_clock(), 5000, "Failed to take control mode report");
   }
 }
 

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -25,13 +25,13 @@
 // Autoware
 #include <tier4_autoware_utils/ros/update_param.hpp>
 
-#include <tier4_system_msgs/msg/mrm_state.hpp>
-#include <tier4_system_msgs/msg/mrm_behavior.hpp>
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
+#include <tier4_system_msgs/msg/mrm_behavior.hpp>
 #include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
+#include <tier4_system_msgs/msg/mrm_state.hpp>
 #include <tier4_system_msgs/msg/operation_mode_availability.hpp>
 #include <tier4_system_msgs/srv/operate_mrm.hpp>
 

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

@@ -227,8 +227,8 @@ void MrmHandler::publishMrmState()
 
 void MrmHandler::operateMrm()
 {
-  using tier4_system_msgs::msg::MrmState;
   using tier4_system_msgs::msg::MrmBehavior;
+  using tier4_system_msgs::msg::MrmState;
 
   if (mrm_state_.state == MrmState::NORMAL) {
     // Cancel MRM behavior when returning to NORMAL state
@@ -271,8 +271,8 @@ void MrmHandler::operateMrm()
 
 void MrmHandler::handleFailedRequest()
 {
-  using tier4_system_msgs::msg::MrmState;
   using tier4_system_msgs::msg::MrmBehavior;
+  using tier4_system_msgs::msg::MrmState;
 
   if (requestMrmBehavior(MrmBehavior::EMERGENCY_STOP, CALL)) {
     if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING);
@@ -286,8 +286,8 @@ bool MrmHandler::requestMrmBehavior(
   const tier4_system_msgs::msg::MrmBehavior::_type_type & mrm_behavior,
   RequestType request_type) const
 {
-  using tier4_system_msgs::msg::MrmState;
   using tier4_system_msgs::msg::MrmBehavior;
+  using tier4_system_msgs::msg::MrmState;
 
   auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
   if (request_type == RequestType::CALL) {
@@ -465,9 +465,9 @@ void MrmHandler::transitionTo(const int new_state)
 
 void MrmHandler::updateMrmState()
 {
-  using tier4_system_msgs::msg::MrmState;
-  using tier4_system_msgs::msg::MrmBehavior;
   using autoware_auto_vehicle_msgs::msg::ControlModeReport;
+  using tier4_system_msgs::msg::MrmBehavior;
+  using tier4_system_msgs::msg::MrmState;
 
   // Check emergency
   const bool is_emergency = isEmergency();
@@ -522,8 +522,8 @@ void MrmHandler::updateMrmState()
 
 tier4_system_msgs::msg::MrmBehavior::_type_type MrmHandler::getCurrentMrmBehavior()
 {
-  using tier4_system_msgs::msg::MrmState;
   using tier4_system_msgs::msg::MrmBehavior;
+  using tier4_system_msgs::msg::MrmState;
   using tier4_system_msgs::msg::OperationModeAvailability;
 
   // State machine

system/mrm_stop_operator/config/mrm_stop_operator.param.yaml

@@ -3,4 +3,3 @@
     min_acceleration: -4.0  # min acceleration for sub ecu mrm stop [m/s^2]
     max_jerk: 5.0           # max jerk for sub ecu mrm stop [m/s^3]
     min_jerk: -5.0          # min jerk for sub ecu mrm stop [m/s^3]
-

system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml

@@ -3,7 +3,7 @@
 
   <node pkg="mrm_stop_operator" exec="mrm_stop_operator_node" name="mrm_stop_operator" output="screen">
     <param from="$(var mrm_stop_operator_param_path)"/>
-    
+
     <remap from="~/input/mrm_request" to="/system/mrm_request"/>
     <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
     <remap from="~/output/velocity_limit_clear_command" to="/planning/scenario_planning/clear_velocity_limit"/>