feat(external_cmd_selector): apply agnocast_wrapper::Node to external_cmd_selector (autowarefoundation#13013)

* feat(external_cmd_selector): apply agnocast_wrapper::Node to external_cmd_selector

Co-authored-by: Koichi Imai <koichi.imai.2@tier4.jp>
Signed-off-by: kobayu858 <yutaro.kobayashi.2@tier4.jp>

* refactor: revert comment

Signed-off-by: kobayu858 <yutaro.kobayashi.2@tier4.jp>

* refactor: revert comment

Signed-off-by: kobayu858 <yutaro.kobayashi.2@tier4.jp>

---------

Signed-off-by: kobayu858 <yutaro.kobayashi.2@tier4.jp>
Co-authored-by: Koichi Imai <koichi.imai.2@tier4.jp>

Author: kobayu858

control/autoware_external_cmd_selector/CMakeLists.txt

@@ -8,9 +8,11 @@ ament_auto_add_library(${PROJECT_NAME}_node SHARED
   src/autoware_external_cmd_selector/external_cmd_selector_node.cpp
 )
 
-rclcpp_components_register_node(${PROJECT_NAME}_node
+autoware_agnocast_wrapper_register_node(${PROJECT_NAME}_node
   PLUGIN "autoware::external_cmd_selector::ExternalCmdSelector"
   EXECUTABLE autoware_external_cmd_selector
+  AGNOCAST_EXECUTOR AgnocastOnlyCallbackIsolatedExecutor
+  ROS2_EXECUTOR SingleThreadedExecutor
 )
 
 ament_auto_package(INSTALL_TO_SHARE

control/autoware_external_cmd_selector/include/autoware/external_cmd_selector/external_cmd_selector_node.hpp

@@ -15,6 +15,8 @@
 #ifndef AUTOWARE__EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_
 #define AUTOWARE__EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_
 
+#include <autoware/agnocast_wrapper/diagnostic_updater.hpp>
+#include <autoware/agnocast_wrapper/node.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <diagnostic_updater/update_functions.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -33,7 +35,7 @@
 
 namespace autoware::external_cmd_selector
 {
-class ExternalCmdSelector : public rclcpp::Node
+class ExternalCmdSelector : public autoware::agnocast_wrapper::Node
 {
 public:
   explicit ExternalCmdSelector(const rclcpp::NodeOptions & node_options);
@@ -55,54 +57,58 @@ class ExternalCmdSelector : public rclcpp::Node
   rclcpp::CallbackGroup::SharedPtr callback_group_services_;
 
   // Publisher
-  rclcpp::Publisher<CommandSourceMode>::SharedPtr pub_current_selector_mode_;
-  rclcpp::Publisher<PedalsCommand>::SharedPtr pub_pedals_cmd_;
-  rclcpp::Publisher<SteeringCommand>::SharedPtr pub_steering_cmd_;
-  rclcpp::Publisher<OperatorHeartbeat>::SharedPtr pub_heartbeat_;
-  rclcpp::Publisher<GearCommand>::SharedPtr pub_gear_cmd_;
-  rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr pub_turn_indicators_cmd_;
-  rclcpp::Publisher<HazardLightsCommand>::SharedPtr pub_hazard_lights_cmd_;
+  AUTOWARE_PUBLISHER_PTR(CommandSourceMode) pub_current_selector_mode_;
+  AUTOWARE_PUBLISHER_PTR(PedalsCommand) pub_pedals_cmd_;
+  AUTOWARE_PUBLISHER_PTR(SteeringCommand) pub_steering_cmd_;
+  AUTOWARE_PUBLISHER_PTR(OperatorHeartbeat) pub_heartbeat_;
+  AUTOWARE_PUBLISHER_PTR(GearCommand) pub_gear_cmd_;
+  AUTOWARE_PUBLISHER_PTR(TurnIndicatorsCommand) pub_turn_indicators_cmd_;
+  AUTOWARE_PUBLISHER_PTR(HazardLightsCommand) pub_hazard_lights_cmd_;
 
   // Subscriber
-  rclcpp::Subscription<PedalsCommand>::SharedPtr sub_local_pedals_cmd_;
-  rclcpp::Subscription<SteeringCommand>::SharedPtr sub_local_steering_cmd_;
-  rclcpp::Subscription<OperatorHeartbeat>::SharedPtr sub_local_heartbeat_;
-  rclcpp::Subscription<GearCommand>::SharedPtr sub_local_gear_cmd_;
-  rclcpp::Subscription<TurnIndicatorsCommand>::SharedPtr sub_local_turn_indicators_cmd_;
-  rclcpp::Subscription<HazardLightsCommand>::SharedPtr sub_local_hazard_lights_cmd_;
-
-  rclcpp::Subscription<PedalsCommand>::SharedPtr sub_remote_pedals_cmd_;
-  rclcpp::Subscription<SteeringCommand>::SharedPtr sub_remote_steering_cmd_;
-  rclcpp::Subscription<OperatorHeartbeat>::SharedPtr sub_remote_heartbeat_;
-  rclcpp::Subscription<GearCommand>::SharedPtr sub_remote_gear_cmd_;
-  rclcpp::Subscription<TurnIndicatorsCommand>::SharedPtr sub_remote_turn_indicators_cmd_;
-  rclcpp::Subscription<HazardLightsCommand>::SharedPtr sub_remote_hazard_lights_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(PedalsCommand) sub_local_pedals_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(SteeringCommand) sub_local_steering_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(OperatorHeartbeat) sub_local_heartbeat_;
+  AUTOWARE_SUBSCRIPTION_PTR(GearCommand) sub_local_gear_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(TurnIndicatorsCommand) sub_local_turn_indicators_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(HazardLightsCommand) sub_local_hazard_lights_cmd_;
+
+  AUTOWARE_SUBSCRIPTION_PTR(PedalsCommand) sub_remote_pedals_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(SteeringCommand) sub_remote_steering_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(OperatorHeartbeat) sub_remote_heartbeat_;
+  AUTOWARE_SUBSCRIPTION_PTR(GearCommand) sub_remote_gear_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(TurnIndicatorsCommand) sub_remote_turn_indicators_cmd_;
+  AUTOWARE_SUBSCRIPTION_PTR(HazardLightsCommand) sub_remote_hazard_lights_cmd_;
 
   template <class T, class F>
-  std::function<void(const T &)> bind_on_cmd(F && func, uint8_t mode)
+  std::function<void(const AUTOWARE_MESSAGE_CONST_SHARED_PTR(T) &)> bind_on_cmd(
+    F && func, uint8_t mode)
   {
     return std::bind(func, this, std::placeholders::_1, mode);
   }
-  void on_pedals_cmd(const PedalsCommand & msg, uint8_t mode);
-  void on_steering_cmd(const SteeringCommand & msg, uint8_t mode);
-  void on_heartbeat(const OperatorHeartbeat & msg, uint8_t mode);
-  void on_gear_cmd(const GearCommand & msg, uint8_t mode);
-  void on_turn_indicators_cmd(const TurnIndicatorsCommand & msg, uint8_t mode);
-  void on_hazard_lights_cmd(const HazardLightsCommand & msg, uint8_t mode);
+  void on_pedals_cmd(const AUTOWARE_MESSAGE_CONST_SHARED_PTR(PedalsCommand) & msg, uint8_t mode);
+  void on_steering_cmd(
+    const AUTOWARE_MESSAGE_CONST_SHARED_PTR(SteeringCommand) & msg, uint8_t mode);
+  void on_heartbeat(const AUTOWARE_MESSAGE_CONST_SHARED_PTR(OperatorHeartbeat) & msg, uint8_t mode);
+  void on_gear_cmd(const AUTOWARE_MESSAGE_CONST_SHARED_PTR(GearCommand) & msg, uint8_t mode);
+  void on_turn_indicators_cmd(
+    const AUTOWARE_MESSAGE_CONST_SHARED_PTR(TurnIndicatorsCommand) & msg, uint8_t mode);
+  void on_hazard_lights_cmd(
+    const AUTOWARE_MESSAGE_CONST_SHARED_PTR(HazardLightsCommand) & msg, uint8_t mode);
 
   // Service
-  rclcpp::Service<CommandSourceSelect>::SharedPtr srv_select_external_command_;
+  AUTOWARE_SERVICE_PTR(CommandSourceSelect) srv_select_external_command_;
   std::atomic<uint8_t> current_selector_mode_;
   bool on_select_external_command(
-    const CommandSourceSelect::Request::SharedPtr req,
-    const CommandSourceSelect::Response::SharedPtr res);
+    const AUTOWARE_SERVER_REQUEST_PTR(CommandSourceSelect) & req,
+    const AUTOWARE_SERVER_RESPONSE_PTR(CommandSourceSelect) & res);
 
   // Timer
-  rclcpp::TimerBase::SharedPtr timer_;
+  AUTOWARE_TIMER_PTR timer_;
   void on_timer();
 
   // Diagnostics Updater
-  diagnostic_updater::Updater updater_{this};
+  autoware::agnocast_wrapper::diagnostic_updater::Updater updater_{this};
 };
 }  // namespace autoware::external_cmd_selector
 #endif  // AUTOWARE__EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_

control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py

@@ -15,11 +15,14 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.actions import GroupAction
+from launch.actions import IncludeLaunchDescription
 from launch.actions import OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import LoadComposableNodes
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
 from launch_ros.actions import PushRosNamespace
-from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
 import yaml
 
 
@@ -31,51 +34,45 @@ def launch_setup(context, *args, **kwargs):
     with open(LaunchConfiguration("external_cmd_selector_param_path").perform(context), "r") as f:
         external_cmd_selector_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
-    component = ComposableNode(
+    remappings = [
+        _create_mapping_tuple("service/select_external_command"),
+        _create_mapping_tuple("output/current_selector_mode"),
+        _create_mapping_tuple("input/local/pedals_cmd"),
+        _create_mapping_tuple("input/local/steering_cmd"),
+        _create_mapping_tuple("input/local/heartbeat"),
+        _create_mapping_tuple("input/local/gear_cmd"),
+        _create_mapping_tuple("input/local/turn_indicators_cmd"),
+        _create_mapping_tuple("input/local/hazard_lights_cmd"),
+        _create_mapping_tuple("input/remote/pedals_cmd"),
+        _create_mapping_tuple("input/remote/steering_cmd"),
+        _create_mapping_tuple("input/remote/heartbeat"),
+        _create_mapping_tuple("input/remote/gear_cmd"),
+        _create_mapping_tuple("input/remote/turn_indicators_cmd"),
+        _create_mapping_tuple("input/remote/hazard_lights_cmd"),
+        _create_mapping_tuple("output/pedals_cmd"),
+        _create_mapping_tuple("output/steering_cmd"),
+        _create_mapping_tuple("output/heartbeat"),
+        _create_mapping_tuple("output/gear_cmd"),
+        _create_mapping_tuple("output/turn_indicators_cmd"),
+        _create_mapping_tuple("output/hazard_lights_cmd"),
+    ]
+
+    node = Node(
         package="autoware_external_cmd_selector",
-        plugin="autoware::external_cmd_selector::ExternalCmdSelector",
+        executable="autoware_external_cmd_selector",
         name="external_cmd_selector",
-        remappings=[
-            _create_mapping_tuple("service/select_external_command"),
-            _create_mapping_tuple("output/current_selector_mode"),
-            _create_mapping_tuple("input/local/pedals_cmd"),
-            _create_mapping_tuple("input/local/steering_cmd"),
-            _create_mapping_tuple("input/local/heartbeat"),
-            _create_mapping_tuple("input/local/gear_cmd"),
-            _create_mapping_tuple("input/local/turn_indicators_cmd"),
-            _create_mapping_tuple("input/local/hazard_lights_cmd"),
-            _create_mapping_tuple("input/remote/pedals_cmd"),
-            _create_mapping_tuple("input/remote/steering_cmd"),
-            _create_mapping_tuple("input/remote/heartbeat"),
-            _create_mapping_tuple("input/remote/gear_cmd"),
-            _create_mapping_tuple("input/remote/turn_indicators_cmd"),
-            _create_mapping_tuple("input/remote/hazard_lights_cmd"),
-            _create_mapping_tuple("output/pedals_cmd"),
-            _create_mapping_tuple("output/steering_cmd"),
-            _create_mapping_tuple("output/heartbeat"),
-            _create_mapping_tuple("output/gear_cmd"),
-            _create_mapping_tuple("output/turn_indicators_cmd"),
-            _create_mapping_tuple("output/hazard_lights_cmd"),
-        ],
+        remappings=remappings,
         parameters=[
             external_cmd_selector_param,
         ],
-        extra_arguments=[
-            {
-                "use_intra_process_comms": LaunchConfiguration("use_intra_process"),
-            }
-        ],
-    )
-
-    loader = LoadComposableNodes(
-        composable_node_descriptions=[component],
-        target_container=LaunchConfiguration("target_container"),
+        additional_env={"LD_PRELOAD": LaunchConfiguration("ld_preload_value")},
+        output="screen",
     )
 
     group = GroupAction(
         [
             PushRosNamespace(""),
-            loader,
+            node,
         ]
     )
 
@@ -85,9 +82,6 @@ def launch_setup(context, *args, **kwargs):
 def generate_launch_description():
     # fmt: off
     arguments = [
-        # component
-        DeclareLaunchArgument("use_intra_process"),
-        DeclareLaunchArgument("target_container"),
         # mode select
         DeclareLaunchArgument("service/select_external_command", default_value="~/select_external_command"),
         DeclareLaunchArgument("output/current_selector_mode", default_value="~/current_selector_mode"),
@@ -115,4 +109,16 @@ def generate_launch_description():
     ]
     # fmt: on
 
-    return LaunchDescription(arguments + [OpaqueFunction(function=launch_setup)])
+    agnocast_env = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("autoware_agnocast_wrapper"),
+                    "launch",
+                    "agnocast_env.launch.py",
+                ]
+            )
+        )
+    )
+
+    return LaunchDescription(arguments + [agnocast_env, OpaqueFunction(function=launch_setup)])

control/autoware_external_cmd_selector/package.xml

@@ -17,7 +17,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_adapi_v1_msgs</depend>
-  <depend>autoware_qos_utils</depend>
+  <depend>autoware_agnocast_wrapper</depend>
   <depend>autoware_vehicle_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>

control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp

@@ -14,8 +14,6 @@
 
 #include "autoware/external_cmd_selector/external_cmd_selector_node.hpp"
 
-#include <autoware/qos_utils/qos_compatibility.hpp>
-
 #include <chrono>
 #include <cstdint>
 #include <memory>
@@ -26,7 +24,7 @@ namespace autoware::external_cmd_selector
 {
 
 ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_options)
-: Node("external_cmd_selector", node_options)
+: autoware::agnocast_wrapper::Node("external_cmd_selector", node_options)
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
@@ -52,7 +50,7 @@ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_option
   callback_group_services_ =
     this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
-  auto subscriber_option = rclcpp::SubscriptionOptions();
+  AUTOWARE_SUBSCRIPTION_OPTIONS subscriber_option;
   subscriber_option.callback_group = callback_group_subscribers_;
 
   // Subscriber
@@ -114,7 +112,7 @@ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_option
   srv_select_external_command_ = create_service<CommandSourceSelect>(
     "~/service/select_external_command",
     std::bind(&ExternalCmdSelector::on_select_external_command, this, _1, _2),
-    AUTOWARE_DEFAULT_SERVICES_QOS_PROFILE(), callback_group_services_);
+    rclcpp::ServicesQoS(), callback_group_services_);
 
   // Initialize mode
   auto convert_selector_mode = [](const std::string & mode_text) {
@@ -136,50 +134,56 @@ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_option
 
   // Timer
   const auto period_ns = rclcpp::Rate(update_rate).period();
-  timer_ = rclcpp::create_timer(
+  timer_ = autoware::agnocast_wrapper::create_timer(
     this, get_clock(), period_ns, std::bind(&ExternalCmdSelector::on_timer, this),
     callback_group_subscribers_);
 }
 
-void ExternalCmdSelector::on_pedals_cmd(const PedalsCommand & msg, uint8_t mode)
+void ExternalCmdSelector::on_pedals_cmd(
+  const AUTOWARE_MESSAGE_CONST_SHARED_PTR(PedalsCommand) & msg, uint8_t mode)
 {
   if (current_selector_mode_.load() != mode) return;
-  pub_pedals_cmd_->publish(msg);
+  pub_pedals_cmd_->publish(*msg);
 }
 
-void ExternalCmdSelector::on_steering_cmd(const SteeringCommand & msg, uint8_t mode)
+void ExternalCmdSelector::on_steering_cmd(
+  const AUTOWARE_MESSAGE_CONST_SHARED_PTR(SteeringCommand) & msg, uint8_t mode)
 {
   if (current_selector_mode_.load() != mode) return;
-  pub_steering_cmd_->publish(msg);
+  pub_steering_cmd_->publish(*msg);
 }
 
-void ExternalCmdSelector::on_heartbeat(const OperatorHeartbeat & msg, uint8_t mode)
+void ExternalCmdSelector::on_heartbeat(
+  const AUTOWARE_MESSAGE_CONST_SHARED_PTR(OperatorHeartbeat) & msg, uint8_t mode)
 {
   if (current_selector_mode_.load() != mode) return;
-  pub_heartbeat_->publish(msg);
+  pub_heartbeat_->publish(*msg);
 }
 
-void ExternalCmdSelector::on_gear_cmd(const GearCommand & msg, uint8_t mode)
+void ExternalCmdSelector::on_gear_cmd(
+  const AUTOWARE_MESSAGE_CONST_SHARED_PTR(GearCommand) & msg, uint8_t mode)
 {
   if (current_selector_mode_.load() != mode) return;
-  pub_gear_cmd_->publish(msg);
+  pub_gear_cmd_->publish(*msg);
 }
 
-void ExternalCmdSelector::on_turn_indicators_cmd(const TurnIndicatorsCommand & msg, uint8_t mode)
+void ExternalCmdSelector::on_turn_indicators_cmd(
+  const AUTOWARE_MESSAGE_CONST_SHARED_PTR(TurnIndicatorsCommand) & msg, uint8_t mode)
 {
   if (current_selector_mode_.load() != mode) return;
-  pub_turn_indicators_cmd_->publish(msg);
+  pub_turn_indicators_cmd_->publish(*msg);
 }
 
-void ExternalCmdSelector::on_hazard_lights_cmd(const HazardLightsCommand & msg, uint8_t mode)
+void ExternalCmdSelector::on_hazard_lights_cmd(
+  const AUTOWARE_MESSAGE_CONST_SHARED_PTR(HazardLightsCommand) & msg, uint8_t mode)
 {
   if (current_selector_mode_.load() != mode) return;
-  pub_hazard_lights_cmd_->publish(msg);
+  pub_hazard_lights_cmd_->publish(*msg);
 }
 
 bool ExternalCmdSelector::on_select_external_command(
-  const CommandSourceSelect::Request::SharedPtr req,
-  const CommandSourceSelect::Response::SharedPtr res)
+  const AUTOWARE_SERVER_REQUEST_PTR(CommandSourceSelect) & req,
+  const AUTOWARE_SERVER_RESPONSE_PTR(CommandSourceSelect) & res)
 {
   current_selector_mode_.store(req->mode.data);
   res->success = true;