Temporary PR: RJD-736/autoware_msgs_support

dependency_humble.repos

@@ -7,10 +7,11 @@ repositories:
     type: git
     url: https://github.com/autowarefoundation/autoware_msgs.git
     version: main
-  autoware_auto_msgs:
+  # TODO(youtalk): Remove autoware_common when https://github.com/autowarefoundation/autoware/issues/4911 is closed
+  autoware_common:
     type: git
-    url: https://github.com/tier4/autoware_auto_msgs.git
-    version: tier4/main
+    url: https://github.com/autowarefoundation/autoware_common.git
+    version: remove-autoware-cmake-utils
   autoware_cmake:
     type: git
     url: https://github.com/autowarefoundation/autoware_cmake.git

external/concealer/include/concealer/autoware.hpp

@@ -16,9 +16,9 @@
 #define CONCEALER__AUTOWARE_HPP_
 
 #include <atomic>
-#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
-#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <autoware_control_msgs/msg/control.hpp>
+#include <autoware_vehicle_msgs/msg/gear_command.hpp>
+#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <concealer/continuous_transform_broadcaster.hpp>
 #include <concealer/visibility.hpp>
 #include <geometry_msgs/msg/accel.hpp>
@@ -48,7 +48,7 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster<Auto
 
   virtual auto getAcceleration() const -> double = 0;
 
-  virtual auto getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand;
+  virtual auto getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand;
 
   virtual auto getSteeringAngle() const -> double = 0;
 
@@ -58,11 +58,10 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster<Auto
   virtual auto getGearSign() const -> double = 0;
 
   virtual auto getTurnIndicatorsCommand() const
-    -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+    -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
 
-  virtual auto getVehicleCommand() const -> std::tuple<
-    autoware_auto_control_msgs::msg::AckermannControlCommand,
-    autoware_auto_vehicle_msgs::msg::GearCommand> = 0;
+  virtual auto getVehicleCommand() const
+    -> std::tuple<autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> = 0;
 
   virtual auto getRouteLanelets() const -> std::vector<std::int64_t> = 0;
 
@@ -73,6 +72,10 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster<Auto
   auto set(const geometry_msgs::msg::Pose &) -> void;
 
   virtual auto rethrow() -> void;
+
+  virtual auto setManualMode() -> void = 0;
+
+  virtual auto setAutonomousMode() -> void = 0;
 };
 }  // namespace concealer
 

external/concealer/include/concealer/autoware_universe.hpp

@@ -15,18 +15,18 @@
 #ifndef CONCEALER__AUTOWARE_UNIVERSE_HPP_
 #define CONCEALER__AUTOWARE_UNIVERSE_HPP_
 
-#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp>
-#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <autoware_control_msgs/msg/control.hpp>
+#include <autoware_vehicle_msgs/msg/control_mode_report.hpp>
+#include <autoware_vehicle_msgs/msg/gear_report.hpp>
+#include <autoware_vehicle_msgs/msg/steering_report.hpp>
+#include <autoware_vehicle_msgs/msg/turn_indicators_report.hpp>
+#include <autoware_vehicle_msgs/msg/velocity_report.hpp>
 #include <concealer/autoware.hpp>
 #include <concealer/publisher_wrapper.hpp>
 #include <concealer/subscriber_wrapper.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 namespace concealer
 {
@@ -37,18 +37,18 @@ namespace concealer
 class AutowareUniverse : public Autoware
 {
   // clang-format off
-  SubscriberWrapper<autoware_auto_control_msgs::msg::AckermannControlCommand, ThreadSafety::safe> getAckermannControlCommand;
-  SubscriberWrapper<autoware_auto_vehicle_msgs::msg::GearCommand,             ThreadSafety::safe> getGearCommandImpl;
-  SubscriberWrapper<autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand,   ThreadSafety::safe> getTurnIndicatorsCommand;
-  SubscriberWrapper<autoware_auto_planning_msgs::msg::PathWithLaneId,         ThreadSafety::safe> getPathWithLaneId;
+  SubscriberWrapper<autoware_control_msgs::msg::Control, ThreadSafety::safe> getCommand;
+  SubscriberWrapper<autoware_vehicle_msgs::msg::GearCommand,             ThreadSafety::safe> getGearCommandImpl;
+  SubscriberWrapper<autoware_vehicle_msgs::msg::TurnIndicatorsCommand,   ThreadSafety::safe> getTurnIndicatorsCommand;
+  SubscriberWrapper<tier4_planning_msgs::msg::PathWithLaneId,         ThreadSafety::safe> getPathWithLaneId;
 
   PublisherWrapper<geometry_msgs::msg::AccelWithCovarianceStamped>        setAcceleration;
   PublisherWrapper<nav_msgs::msg::Odometry>                               setOdometry;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::SteeringReport>       setSteeringReport;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::GearReport>           setGearReport;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::ControlModeReport>    setControlModeReport;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::VelocityReport>       setVelocityReport;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport> setTurnIndicatorsReport;
+  PublisherWrapper<autoware_vehicle_msgs::msg::SteeringReport>       setSteeringReport;
+  PublisherWrapper<autoware_vehicle_msgs::msg::GearReport>           setGearReport;
+  PublisherWrapper<autoware_vehicle_msgs::msg::ControlModeReport>    setControlModeReport;
+  PublisherWrapper<autoware_vehicle_msgs::msg::VelocityReport>       setVelocityReport;
+  PublisherWrapper<autoware_vehicle_msgs::msg::TurnIndicatorsReport> setTurnIndicatorsReport;
   // clang-format on
 
   const rclcpp::TimerBase::SharedPtr localization_update_timer;
@@ -59,6 +59,9 @@ class AutowareUniverse : public Autoware
 
   std::atomic<bool> is_stop_requested = false;
 
+  std::atomic<uint8_t> current_control_mode =
+    autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;
+
   std::atomic<bool> is_thrown = false;
 
   std::exception_ptr thrown;
@@ -82,15 +85,18 @@ class AutowareUniverse : public Autoware
 
   auto updateVehicleState() -> void;
 
-  auto getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand override;
+  auto getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand override;
 
   auto getGearSign() const -> double override;
 
   auto getVehicleCommand() const -> std::tuple<
-    autoware_auto_control_msgs::msg::AckermannControlCommand,
-    autoware_auto_vehicle_msgs::msg::GearCommand> override;
+    autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> override;
 
   auto getRouteLanelets() const -> std::vector<std::int64_t>;
+
+  auto setManualMode() -> void override;
+
+  auto setAutonomousMode() -> void override;
 };
 
 }  // namespace concealer

external/concealer/include/concealer/field_operator_application.hpp

@@ -20,10 +20,8 @@
 #include <sys/wait.h>
 
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
-#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <autoware_auto_system_msgs/msg/emergency_state.hpp>
-#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <autoware_control_msgs/msg/control.hpp>
+#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <chrono>
 #include <concealer/autoware_stream.hpp>
 #include <concealer/launch.hpp>
@@ -147,21 +145,25 @@ class FieldOperatorApplication : public rclcpp::Node
   virtual auto restrictTargetSpeed(double) const -> double = 0;
 
   virtual auto getTurnIndicatorsCommand() const
-    -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+    -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
 
   virtual auto rethrow() const noexcept(false) -> void;
 
   virtual auto sendCooperateCommand(const std::string &, const std::string &) -> void = 0;
 
   virtual auto setVelocityLimit(double) -> void = 0;
+
+  virtual auto enableAutowareControl() -> void = 0;
+
+  virtual auto disableAutowareControl() -> void = 0;
 };
 }  // namespace concealer
 
-namespace autoware_auto_vehicle_msgs::msg
+namespace autoware_vehicle_msgs::msg
 {
 auto operator<<(std::ostream &, const TurnIndicatorsCommand &) -> std::ostream &;
 
 auto operator>>(std::istream &, TurnIndicatorsCommand &) -> std::istream &;
-}  // namespace autoware_auto_vehicle_msgs::msg
+}  // namespace autoware_vehicle_msgs::msg
 
 #endif  // CONCEALER__AUTOWARE_USER_HPP_

external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp

@@ -20,14 +20,13 @@
 #endif
 
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+#include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
-#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <autoware_auto_system_msgs/msg/emergency_state.hpp>
-#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
+#include <autoware_control_msgs/msg/control.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
+#include <autoware_vehicle_msgs/msg/gear_command.hpp>
 #include <concealer/autoware_universe.hpp>
 #include <concealer/field_operator_application.hpp>
 #include <concealer/publisher_wrapper.hpp>
@@ -38,6 +37,7 @@
 #include <tier4_external_api_msgs/msg/emergency.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
 #include <tier4_external_api_msgs/srv/set_velocity_limit.hpp>
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_planning_msgs/msg/trajectory.hpp>
 #include <tier4_rtc_msgs/msg/cooperate_status_array.hpp>
 #include <tier4_rtc_msgs/srv/auto_mode_with_module.hpp>
@@ -54,7 +54,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   friend class TransitionAssertion<FieldOperatorApplicationFor<AutowareUniverse>>;
 
   // clang-format off
-  SubscriberWrapper<autoware_auto_control_msgs::msg::AckermannControlCommand>     getAckermannControlCommand;
+  SubscriberWrapper<autoware_control_msgs::msg::Control>     getCommand;
   SubscriberWrapper<tier4_system_msgs::msg::AutowareState, ThreadSafety::safe>    getAutowareState;
   SubscriberWrapper<tier4_rtc_msgs::msg::CooperateStatusArray>                    getCooperateStatusArray;
   SubscriberWrapper<tier4_external_api_msgs::msg::Emergency>                      getEmergencyState;
@@ -63,7 +63,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
 #endif
   SubscriberWrapper<autoware_adapi_v1_msgs::msg::MrmState>                        getMrmState;
   SubscriberWrapper<tier4_planning_msgs::msg::Trajectory>                         getTrajectory;
-  SubscriberWrapper<autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand>       getTurnIndicatorsCommandImpl;
+  SubscriberWrapper<autoware_vehicle_msgs::msg::TurnIndicatorsCommand>       getTurnIndicatorsCommandImpl;
 
   ServiceWithValidation<autoware_adapi_v1_msgs::srv::ClearRoute>                  requestClearRoute;
   ServiceWithValidation<tier4_rtc_msgs::srv::CooperateCommands>                   requestCooperateCommands;
@@ -72,6 +72,8 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   ServiceWithValidation<autoware_adapi_v1_msgs::srv::SetRoutePoints>              requestSetRoutePoints;
   ServiceWithValidation<tier4_rtc_msgs::srv::AutoModeWithModule>                  requestSetRtcAutoMode;
   ServiceWithValidation<tier4_external_api_msgs::srv::SetVelocityLimit>           requestSetVelocityLimit;
+  ServiceWithValidation<autoware_adapi_v1_msgs::srv::ChangeOperationMode>         requestEnableAutowareControl;
+  ServiceWithValidation<autoware_adapi_v1_msgs::srv::ChangeOperationMode>         requestDisableAutowareControl;
   // clang-format on
 
   tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array;
@@ -112,14 +114,14 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   auto sendSIGINT() -> void override;
 
 public:
-  SubscriberWrapper<autoware_auto_planning_msgs::msg::PathWithLaneId> getPathWithLaneId;
+  SubscriberWrapper<tier4_planning_msgs::msg::PathWithLaneId> getPathWithLaneId;
 
 public:
   template <typename... Ts>
   CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs)
   : FieldOperatorApplication(std::forward<decltype(xs)>(xs)...),
     // clang-format off
-    getAckermannControlCommand("/control/command/control_cmd", rclcpp::QoS(1), *this),
+    getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this),
     getAutowareState("/api/iv_msgs/autoware/state", rclcpp::QoS(1), *this),
     getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }),
     getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & v) { receiveEmergencyState(v); }),
@@ -137,7 +139,9 @@ class FieldOperatorApplicationFor<AutowareUniverse>
     // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons.
     requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)),
     requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this),
-    requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this)
+    requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this),
+    requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this),
+    requestDisableAutowareControl("/api/operation_mode/disable_autoware_control", *this)
   // clang-format on
   {
   }
@@ -155,7 +159,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray override;
 
   auto getTurnIndicatorsCommand() const
-    -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand override;
+    -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand override;
 
   auto getEmergencyStateName() const -> std::string override;
 
@@ -176,6 +180,10 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   auto sendCooperateCommand(const std::string &, const std::string &) -> void override;
 
   auto setVelocityLimit(double) -> void override;
+
+  auto enableAutowareControl() -> void override;
+
+  auto disableAutowareControl() -> void override;
 };
 }  // namespace concealer
 

external/concealer/include/concealer/service_with_validation.hpp

@@ -116,11 +116,12 @@ class ServiceWithValidation
               return;
             } else {
               RCLCPP_ERROR_STREAM(
-                logger, service_name
-                          << " service request was accepted, but ResponseStatus::success is false "
-                          << (service_call_status.message.empty()
-                                ? ""
-                                : " (" + service_call_status.message + ")"));
+                logger, service_name << " service request was accepted, but "
+                                        "ResponseStatus::success is false with error code: "
+                                     << service_call_status.code << ", and message: "
+                                     << (service_call_status.message.empty()
+                                           ? ""
+                                           : " (" + service_call_status.message + ")"));
             }
           } else {
             RCLCPP_INFO_STREAM(logger, service_name << " service request has been accepted.");

external/concealer/include/concealer/transition_assertion.hpp

@@ -15,6 +15,7 @@
 #ifndef CONCEALER__TRANSITION_ASSERTION_HPP_
 #define CONCEALER__TRANSITION_ASSERTION_HPP_
 
+#include <autoware_system_msgs/msg/autoware_state.hpp>
 #include <chrono>
 #include <rclcpp/node.hpp>
 #include <rclcpp/rate.hpp>

external/concealer/package.xml

@@ -12,11 +12,10 @@
 
   <!-- Autoware.Universe -->
   <depend>autoware_adapi_v1_msgs</depend>
-  <depend>autoware_auto_control_msgs</depend>
-  <depend>autoware_auto_perception_msgs</depend>
-  <depend>autoware_auto_planning_msgs</depend>
-  <depend>autoware_auto_system_msgs</depend>
-  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
+  <depend>autoware_system_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
 
   <!-- TIER IV Autoware.Universe -->
   <depend>tier4_external_api_msgs</depend>

external/concealer/src/autoware.cpp

@@ -24,11 +24,11 @@ Autoware::Autoware()
 {
 }
 
-auto Autoware::getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand
+auto Autoware::getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand
 {
   static auto gear_command = []() {
-    autoware_auto_vehicle_msgs::msg::GearCommand gear_command;
-    gear_command.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE;
+    autoware_vehicle_msgs::msg::GearCommand gear_command;
+    gear_command.command = autoware_vehicle_msgs::msg::GearCommand::DRIVE;
     return gear_command;
   }();
   gear_command.stamp = now();
@@ -44,13 +44,11 @@ auto Autoware::set(const geometry_msgs::msg::Twist & twist) -> void { current_tw
 
 auto Autoware::set(const geometry_msgs::msg::Pose & pose) -> void { current_pose.store(pose); }
 
-auto Autoware::getTurnIndicatorsCommand() const
-  -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand
+auto Autoware::getTurnIndicatorsCommand() const -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand
 {
   static auto turn_indicators_command = []() {
-    autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command;
-    turn_indicators_command.command =
-      autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND;
+    autoware_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command;
+    turn_indicators_command.command = autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND;
     return turn_indicators_command;
   }();
   turn_indicators_command.stamp = now();