diff --git a/dependency_humble.repos b/dependency_humble.repos index 726e7cceb6c..055e9c614e6 100644 --- a/dependency_humble.repos +++ b/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 diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 17c3880e8b9..48d22522178 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -3,19 +3,19 @@ ### Subscribers -| topic | type | note | -|--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| -| `/autoware/state` | [`autoware_auto_system_msgs/msg/AutowareState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl)
or [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | -| `/control/command/control_cmd` | [`autoware_auto_control_msgs/msg/AckermannControlCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_control_msgs/msg/AckermannControlCommand.idl) | | -| `/control/command/gear_cmd` | [`autoware_auto_vehicle_msgs/msg/GearCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearCommand.idl) | | -| `/control/command/turn_indicators_cmd` | [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | -| `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | -| `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`autoware_auto_planning_msgs/msg/PathWithLaneId`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/PathWithLaneId.idl) | | -| `/planning/scenario_planning/trajectory` | [`autoware_auto_planning_msgs/msg/Trajectory`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/Trajectory.idl) | | -| `/system/emergency/emergency_state` | [`autoware_auto_system_msgs/msg/EmergencyState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl) | Used in UserDefinedValueCondition : `currentEmergencyState` | -| `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | -| `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in /simulation/openscenario_interpreter | -| `/api/external/get/rtc_status` | [`tier4_rtc_msgs::msg::CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | +| topic | type | note | +|--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| +| `/autoware/state` | [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | +| `/control/command/control_cmd` | [`autoware_control_msgs/msg/Control`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_control_msgs/msg/Control.msg) | | +| `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/GearCommand.msg) | | +| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | +| `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | +| `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`tier4_planning_msgs/msg/PathWithLaneId`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/PathWithLaneId.msg) | | +| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | +| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition : `currentEmergencyState` | +| `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | +| `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in /simulation/openscenario_interpreter | +| `/api/external/get/rtc_status` | [`tier4_rtc_msgs::msg::CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | ### Publishers @@ -27,20 +27,22 @@ | `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | | | `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | | `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/vehicle/status/control_mode` | [`autoware_auto_vehicle_msgs/msg/ControlModeReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl) | | -| `/vehicle/status/gear_status` | [`autoware_auto_vehicle_msgs/msg/GearReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearReport.idl) | | -| `/vehicle/status/steering_status` | [`autoware_auto_vehicle_msgs/msg/SteeringReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/SteeringReport.idl) | | -| `/vehicle/status/turn_indicators_status` | [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl) | | -| `/vehicle/status/velocity_status` | [`autoware_auto_vehicle_msgs/msg/VelocityReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/VelocityReport.idl) | | -| `/perception/object_recognition/detection/objects` | [`autoware_auto_perception_msgs/msg/DetectedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/DetectedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/object_recognition/ground_truth/objects` | [`autoware_auto_perception_msgs/msg/TrackedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrackedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | +| `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | +| `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | +| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | +| `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | +| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | | `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | | `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | | `/sensing/imu/imu_data` | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg) | | -| `/perception/traffic_light_recognition/traffic_signals` | [`autoware_auto_perception_msgs/msg/TrafficSignalArray`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl) | for `architecture_type` equal to `awf/universe` | -| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | for `architecture_type` greater or equal to `awf/universe/20230906` | -| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | | -| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | [//]: # (| /rosout | rcl_interfaces/msg/Log | | |) [//]: # (| /tf | tf2_msgs/msg/TFMessage | | |) diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp index 12a50710a92..fcda771cf9f 100644 --- a/external/concealer/include/concealer/autoware.hpp +++ b/external/concealer/include/concealer/autoware.hpp @@ -16,10 +16,10 @@ #define CONCEALER__AUTOWARE_HPP_ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -49,7 +49,7 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster 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; @@ -59,16 +59,14 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster 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 = 0; virtual auto getRouteLanelets() const -> std::vector = 0; - virtual auto getControlModeReport() const - -> autoware_auto_vehicle_msgs::msg::ControlModeReport = 0; + virtual auto getControlModeReport() const -> autoware_vehicle_msgs::msg::ControlModeReport = 0; auto set(const geometry_msgs::msg::Accel &) -> void; diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 8e96c0f4abf..d1b4a7bcc8d 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -15,19 +15,19 @@ #ifndef CONCEALER__AUTOWARE_UNIVERSE_HPP_ #define CONCEALER__AUTOWARE_UNIVERSE_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include #include +#include namespace concealer { @@ -38,30 +38,30 @@ namespace concealer class AutowareUniverse : public Autoware { // clang-format off - using ControlModeCommand = autoware_auto_vehicle_msgs::srv::ControlModeCommand; - using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport; - using GearCommand = autoware_auto_vehicle_msgs::msg::GearCommand; - using GearReport = autoware_auto_vehicle_msgs::msg::GearReport; - using TurnIndicatorsCommand = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; - using PathWithLaneId = autoware_auto_planning_msgs::msg::PathWithLaneId; - using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; - using VelocityReport = autoware_auto_vehicle_msgs::msg::VelocityReport; - using TurnIndicatorsReport = autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; using AccelWithCovarianceStamped = geometry_msgs::msg::AccelWithCovarianceStamped; - - SubscriberWrapper getAckermannControlCommand; - SubscriberWrapper getGearCommandImpl; - SubscriberWrapper getTurnIndicatorsCommand; - SubscriberWrapper getPathWithLaneId; - - PublisherWrapper setAcceleration; - PublisherWrapper setOdometry; - PublisherWrapper setSteeringReport; - PublisherWrapper setGearReport; - PublisherWrapper setControlModeReport; - PublisherWrapper setVelocityReport; - PublisherWrapper setTurnIndicatorsReport; + using Control = autoware_control_msgs::msg::Control; + using ControlModeCommand = autoware_vehicle_msgs::srv::ControlModeCommand; + using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; + using GearCommand = autoware_vehicle_msgs::msg::GearCommand; + using GearReport = autoware_vehicle_msgs::msg::GearReport; + using PathWithLaneId = tier4_planning_msgs::msg::PathWithLaneId; + using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; + using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + using TurnIndicatorsReport = autoware_vehicle_msgs::msg::TurnIndicatorsReport; + using VelocityReport = autoware_vehicle_msgs::msg::VelocityReport; + + SubscriberWrapper getCommand; + SubscriberWrapper getGearCommandImpl; + SubscriberWrapper getTurnIndicatorsCommand; + SubscriberWrapper getPathWithLaneId; + + PublisherWrapper setAcceleration; + PublisherWrapper setOdometry; + PublisherWrapper setSteeringReport; + PublisherWrapper setGearReport; + PublisherWrapper setControlModeReport; + PublisherWrapper setVelocityReport; + PublisherWrapper setTurnIndicatorsReport; // clang-format on rclcpp::Service::SharedPtr control_mode_request_server; @@ -103,7 +103,7 @@ class AutowareUniverse : public Autoware auto getGearSign() const -> double override; - auto getVehicleCommand() const -> std::tuple override; + auto getVehicleCommand() const -> std::tuple override; auto getRouteLanelets() const -> std::vector override; diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 57b7e0b4776..2e35afb5ef6 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -20,10 +20,8 @@ #include #include -#include -#include -#include -#include +#include +#include #include #include #include @@ -147,7 +145,7 @@ 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; @@ -159,11 +157,11 @@ class FieldOperatorApplication : public rclcpp::Node }; } // 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_ diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 481a33b87c0..512e1d6545a 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -19,24 +19,15 @@ #include #endif -#if __has_include() -#include -#endif - -#if __has_include() -#include -#endif - #include #include #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -47,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -62,13 +54,8 @@ class FieldOperatorApplicationFor friend struct TransitionAssertion>; // clang-format off - SubscriberWrapper getAckermannControlCommand; -#if __has_include() - SubscriberWrapper getAutowareState; -#endif -#if __has_include() - SubscriberWrapper getAutowareAutoState; -#endif + SubscriberWrapper getCommand; + SubscriberWrapper getAutowareState; SubscriberWrapper getCooperateStatusArray; SubscriberWrapper getEmergencyState; #if __has_include() @@ -76,7 +63,7 @@ class FieldOperatorApplicationFor #endif SubscriberWrapper getMrmState; SubscriberWrapper getTrajectory; - SubscriberWrapper getTurnIndicatorsCommandImpl; + SubscriberWrapper getTurnIndicatorsCommandImpl; ServiceWithValidation requestClearRoute; ServiceWithValidation requestCooperateCommands; @@ -145,35 +132,16 @@ class FieldOperatorApplicationFor auto sendSIGINT() -> void override; public: - SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getPathWithLaneId; public: template CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) : FieldOperatorApplication(std::forward(xs)...), // clang-format off - getAckermannControlCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), -#if __has_include() + getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { - /* - There are multiple places that assignments to `autoware_state` in the callback for the /autoware/state topic to accommodate multiple messages. - But only one of them is used as long as correct configuration Autoware is. - Even if the topic comes in multiple types, as long as the content is the same, - there is basically no problem, but there is a possibility that potential problems may occur. - */ autoware_state = getAutowareStateString(v.state); }), -#endif -#if __has_include() - getAutowareAutoState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { - /* - There are multiple places that assignments to `autoware_state` in the callback for the /autoware/state topic to accommodate multiple messages. - But only one of them is used as long as correct configuration Autoware is. - Even if the topic comes in multiple types, as long as the content is the same, - there is basically no problem, but there is a possibility that potential problems may occur. - */ - autoware_state = getAutowareStateString(v.state); - }), -#endif 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); }), #if __has_include() @@ -209,7 +177,7 @@ class FieldOperatorApplicationFor 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; diff --git a/external/concealer/include/concealer/transition_assertion.hpp b/external/concealer/include/concealer/transition_assertion.hpp index 7850899362e..2734f307c27 100644 --- a/external/concealer/include/concealer/transition_assertion.hpp +++ b/external/concealer/include/concealer/transition_assertion.hpp @@ -15,6 +15,7 @@ #ifndef CONCEALER__TRANSITION_ASSERTION_HPP_ #define CONCEALER__TRANSITION_ASSERTION_HPP_ +#include #include #include #include diff --git a/external/concealer/package.xml b/external/concealer/package.xml index f256c0d1d70..5d855fa2236 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -12,11 +12,10 @@ autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_perception_msgs + autoware_system_msgs + autoware_vehicle_msgs tier4_external_api_msgs diff --git a/external/concealer/src/autoware.cpp b/external/concealer/src/autoware.cpp index a209e4c8f84..f29cbd93e2d 100644 --- a/external/concealer/src/autoware.cpp +++ b/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(); diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 78395952151..02aea619014 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -17,7 +17,7 @@ namespace concealer { AutowareUniverse::AutowareUniverse() -: getAckermannControlCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), +: getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), getGearCommandImpl("/control/command/gear_cmd", rclcpp::QoS(1), *this), getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), getPathWithLaneId( @@ -85,17 +85,14 @@ auto AutowareUniverse::stopAndJoin() -> void auto AutowareUniverse::getAcceleration() const -> double { - return getAckermannControlCommand().longitudinal.acceleration; + return getCommand().longitudinal.acceleration; } -auto AutowareUniverse::getVelocity() const -> double -{ - return getAckermannControlCommand().longitudinal.speed; -} +auto AutowareUniverse::getVelocity() const -> double { return getCommand().longitudinal.velocity; } auto AutowareUniverse::getSteeringAngle() const -> double { - return getAckermannControlCommand().lateral.steering_tire_angle; + return getCommand().lateral.steering_tire_angle; } auto AutowareUniverse::updateLocalization() -> void @@ -176,9 +173,9 @@ auto AutowareUniverse::getGearSign() const -> double : 1.0; } -auto AutowareUniverse::getVehicleCommand() const -> std::tuple +auto AutowareUniverse::getVehicleCommand() const -> std::tuple { - return std::make_tuple(getAckermannControlCommand(), getGearCommand()); + return std::make_tuple(getCommand(), getGearCommand()); } auto AutowareUniverse::getRouteLanelets() const -> std::vector diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 9247027ab73..747c204a68c 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -147,12 +147,11 @@ auto FieldOperatorApplication::shutdownAutoware() -> void } auto FieldOperatorApplication::getTurnIndicatorsCommand() const - -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand + -> 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(); @@ -162,15 +161,15 @@ auto FieldOperatorApplication::getTurnIndicatorsCommand() const auto FieldOperatorApplication::rethrow() const -> void { task_queue.rethrow(); } } // namespace concealer -namespace autoware_auto_vehicle_msgs::msg +namespace autoware_vehicle_msgs::msg { auto operator<<( - std::ostream & out, const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & message) + std::ostream & out, const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) -> std::ostream & { -#define CASE(IDENTIFIER) \ - case autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER: \ - out << #IDENTIFIER; \ +#define CASE(IDENTIFIER) \ + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER: \ + out << #IDENTIFIER; \ break switch (message.command) { @@ -189,11 +188,11 @@ auto operator<<( return out; } -auto operator>>(std::istream & is, autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & message) +auto operator>>(std::istream & is, autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) -> std::istream & { #define STATE(IDENTIFIER) \ - {#IDENTIFIER, autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER} + {#IDENTIFIER, autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER} std::unordered_map state_dictionary{ STATE(DISABLE), @@ -215,4 +214,4 @@ auto operator>>(std::istream & is, autoware_auto_vehicle_msgs::msg::TurnIndicato return is; } -} // namespace autoware_auto_vehicle_msgs::msg +} // namespace autoware_vehicle_msgs::msg diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 072a42b628f..e75ff70ab01 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -400,7 +400,7 @@ auto FieldOperatorApplicationFor::getWaypoints() const } auto FieldOperatorApplicationFor::getTurnIndicatorsCommand() const - -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand + -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand { return getTurnIndicatorsCommandImpl(); } diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 5dfb3e1043e..0d4115d2532 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -101,7 +101,7 @@ def default_rviz_config_file(): def launch_setup(context, *args, **kwargs): # fmt: off - architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20230906") + architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20240605") autoware_launch_file = LaunchConfiguration("autoware_launch_file", default=default_autoware_launch_file_of(architecture_type.perform(context))) autoware_launch_package = LaunchConfiguration("autoware_launch_package", default=default_autoware_launch_package_of(architecture_type.perform(context))) consider_acceleration_by_road_slope = LaunchConfiguration("consider_acceleration_by_road_slope", default=False) diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index 7864980bdab..4fb9bd39245 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -112,7 +112,7 @@ void CppScenarioNode::spawnEgoEntity( api_.attachOccupancyGridSensor([this] { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(api_.getROS2Parameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(api_.getROS2Parameter("architecture_type", "awf/universe/20240605")); configuration.set_entity("ego"); configuration.set_filter_by_range(true); configuration.set_height(200); diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index b989d75630a..51fbab9de2c 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -10,7 +10,7 @@ ament_cmake - autoware_auto_system_msgs + autoware_system_msgs boost libboost-dev rclcpp diff --git a/openscenario/openscenario_interpreter_example/src/timeout.cpp b/openscenario/openscenario_interpreter_example/src/timeout.cpp index 7559a3dcc60..55738bc2415 100644 --- a/openscenario/openscenario_interpreter_example/src/timeout.cpp +++ b/openscenario/openscenario_interpreter_example/src/timeout.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -85,13 +85,11 @@ int main(const int argc, char const * const * const argv) using tier4_simulation_msgs::msg::UserDefinedValue; using tier4_simulation_msgs::msg::UserDefinedValueType; - autoware_auto_system_msgs::msg::AutowareState status; + autoware_system_msgs::msg::AutowareState status; - auto subscription = node->create_subscription( + auto subscription = node->create_subscription( "/autoware/state", rclcpp::QoS(1).reliable(), - [&](const autoware_auto_system_msgs::msg::AutowareState::SharedPtr message) { - status = *message; - }); + [&](const autoware_system_msgs::msg::AutowareState::SharedPtr message) { status = *message; }); auto publisher = node->create_publisher("/timeout", rclcpp::QoS(1).reliable()); @@ -99,7 +97,7 @@ int main(const int argc, char const * const * const argv) { static auto duration_since_autoware_engaged = std::chrono::high_resolution_clock::now(); - if (status.state != autoware_auto_system_msgs::msg::AutowareState::DRIVING) { + if (status.state != autoware_system_msgs::msg::AutowareState::DRIVING) { duration_since_autoware_engaged = std::chrono::high_resolution_clock::now(); } diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index f5545a0d9d4..ec4dcf6ef18 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -57,7 +57,7 @@ class DetectionSensorBase const std::vector & lidar_detected_entities) = 0; }; -template +template class DetectionSensor : public DetectionSensorBase { const typename rclcpp::Publisher::SharedPtr detected_objects_publisher; @@ -66,10 +66,10 @@ class DetectionSensor : public DetectionSensorBase std::default_random_engine random_engine_; - std::queue> + std::queue> detected_objects_queue; - std::queue> + std::queue> ground_truth_objects_queue; public: diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 55050bbea5d..cf78cbf2959 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -29,6 +29,14 @@ #include #include +// This message will be deleted in the future +#if __has_include() +#include +#endif +#if __has_include() +#include +#endif + namespace simple_sensor_simulator { class SensorSimulation @@ -57,8 +65,8 @@ class SensorSimulation -> void { if (configuration.architecture_type().find("awf/universe") != std::string::npos) { - using Message = autoware_auto_perception_msgs::msg::DetectedObjects; - using GroundTruthMessage = autoware_auto_perception_msgs::msg::TrackedObjects; + using Message = autoware_perception_msgs::msg::DetectedObjects; + using GroundTruthMessage = autoware_perception_msgs::msg::TrackedObjects; detection_sensors_.push_back(std::make_unique>( current_simulation_time, configuration, node.create_publisher("/perception/object_recognition/detection/objects", 1), diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp index f075c03887e..4b4dbaed766 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp @@ -15,7 +15,6 @@ #ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_DETECTOR_HPP_ #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_DETECTOR_HPP_ -#include #include #include #include @@ -60,16 +59,11 @@ class TrafficLightsDetector TrafficLightsDetector in SimpleSensorSimulator publishes using architecture-dependent topics: "/perception/traffic_light_recognition/internal/traffic_signals" for >= "awf/universe/20240605" "/perception/traffic_light_recognition/internal/traffic_signals" for == "awf/universe/20230906" - "/perception/traffic_light_recognition/traffic_signals" for "awf/universe" V2ITrafficLights in TrafficSimulator publishes publishes using architecture-independent topics ("awf/universe..."): "/v2x/traffic_signals" and "/perception/traffic_light_recognition/external/traffic_signals" */ - if (architecture_type == "awf/universe") { - using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray; - return std::make_unique>( - &node, "/perception/traffic_light_recognition/traffic_signals"); - } else if (architecture_type == "awf/universe/20230906") { + if (architecture_type == "awf/universe/20230906") { using Message = autoware_perception_msgs::msg::TrafficSignalArray; return std::make_unique>( &node, "/perception/traffic_light_recognition/internal/traffic_signals"); diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 1704f7872d2..6499c742ff4 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -148,7 +148,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index c91ec05a327..cb6d9179a11 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -327,7 +327,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index c0c5a9822c5..a44afb7874b 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -105,7 +105,7 @@ class SimModelIdealSteerAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp index 3fab57e9f50..b6145ccf413 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp @@ -15,7 +15,7 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ -#include +#include #include /** @@ -30,8 +30,8 @@ class SimModelInterface Eigen::VectorXd state_; //!< @brief vehicle state vector Eigen::VectorXd input_; //!< @brief vehicle input vector - //!< @brief gear command defined in autoware_auto_msgs/GearCommand - uint8_t gear_ = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; + //!< @brief gear command defined in autoware_msgs/GearCommand + uint8_t gear_ = autoware_vehicle_msgs::msg::GearCommand::DRIVE; public: /** @@ -72,7 +72,7 @@ class SimModelInterface /** * @brief set gear - * @param [in] gear gear command defined in autoware_auto_msgs/GearCommand + * @param [in] gear gear command defined in autoware_msgs/GearCommand */ void setGear(const uint8_t gear); diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index d6eb836c172..e2891649a95 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -35,7 +35,6 @@ tf2_ros tf2 - autoware_auto_perception_msgs autoware_perception_msgs boost eigen diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 23b38f4b094..dc6c1837f0d 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include -#include -#include +#include +#include #include #include #include @@ -77,28 +77,28 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) -> unique_identif template <> auto make(const traffic_simulator_msgs::EntityStatus & status) - -> autoware_auto_perception_msgs::msg::ObjectClassification + -> autoware_perception_msgs::msg::ObjectClassification { - auto object_classification = autoware_auto_perception_msgs::msg::ObjectClassification(); + auto object_classification = autoware_perception_msgs::msg::ObjectClassification(); object_classification.label = [&]() { switch (status.subtype().value()) { case traffic_simulator_msgs::EntitySubtype::CAR: - return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + return autoware_perception_msgs::msg::ObjectClassification::CAR; case traffic_simulator_msgs::EntitySubtype::TRUCK: - return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; + return autoware_perception_msgs::msg::ObjectClassification::TRUCK; case traffic_simulator_msgs::EntitySubtype::BUS: - return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; + return autoware_perception_msgs::msg::ObjectClassification::BUS; case traffic_simulator_msgs::EntitySubtype::TRAILER: - return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; + return autoware_perception_msgs::msg::ObjectClassification::TRAILER; case traffic_simulator_msgs::EntitySubtype::MOTORCYCLE: - return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + return autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE; case traffic_simulator_msgs::EntitySubtype::BICYCLE: - return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + return autoware_perception_msgs::msg::ObjectClassification::BICYCLE; case traffic_simulator_msgs::EntitySubtype::PEDESTRIAN: - return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; default: - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; } }(); @@ -136,9 +136,9 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) -> geometry_msgs: template <> auto make(const traffic_simulator_msgs::EntityStatus & status) - -> autoware_auto_perception_msgs::msg::DetectedObjectKinematics + -> autoware_perception_msgs::msg::DetectedObjectKinematics { - auto kinematics = autoware_auto_perception_msgs::msg::DetectedObjectKinematics(); + auto kinematics = autoware_perception_msgs::msg::DetectedObjectKinematics(); kinematics.pose_with_covariance.pose = make(status); @@ -165,9 +165,9 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) switch (status.subtype().value()) { case traffic_simulator_msgs::EntitySubtype::BICYCLE: case traffic_simulator_msgs::EntitySubtype::MOTORCYCLE: - return autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + return autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; default: - return autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + return autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; } }(); @@ -176,35 +176,35 @@ auto make(const traffic_simulator_msgs::EntityStatus & status) template <> auto make(const traffic_simulator_msgs::EntityStatus & status) - -> autoware_auto_perception_msgs::msg::Shape + -> autoware_perception_msgs::msg::Shape { - auto shape = autoware_auto_perception_msgs::msg::Shape(); + auto shape = autoware_perception_msgs::msg::Shape(); simulation_interface::toMsg(status.bounding_box().dimensions(), shape.dimensions); - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; return shape; } template <> auto make(const traffic_simulator_msgs::EntityStatus & status) - -> autoware_auto_perception_msgs::msg::DetectedObject + -> autoware_perception_msgs::msg::DetectedObject { - auto detected_object = autoware_auto_perception_msgs::msg::DetectedObject(); + auto detected_object = autoware_perception_msgs::msg::DetectedObject(); detected_object.classification.push_back( - make(status)); + make(status)); detected_object.kinematics = - make(status); - detected_object.shape = make(status); + make(status); + detected_object.shape = make(status); return detected_object; } template <> auto make( const traffic_simulator_msgs::EntityStatus & status, - const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) - -> autoware_auto_perception_msgs::msg::TrackedObject + const autoware_perception_msgs::msg::DetectedObject & detected_object) + -> autoware_perception_msgs::msg::TrackedObject { // ref: https://github.com/autowarefoundation/autoware.universe/blob/main/common/perception_utils/src/conversion.cpp - auto tracked_object = autoware_auto_perception_msgs::msg::TrackedObject(); + auto tracked_object = autoware_perception_msgs::msg::TrackedObject(); // clang-format off tracked_object.object_id = make(status); tracked_object.existence_probability = detected_object.existence_probability; @@ -250,8 +250,7 @@ struct DefaultNoiseApplicator auto operator=(DefaultNoiseApplicator &&) = delete; - auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_objects) - -> decltype(auto) + auto operator()(autoware_perception_msgs::msg::DetectedObjects detected_objects) -> decltype(auto) { auto position_noise_distribution = std::normal_distribution<>(0.0, detection_sensor_configuration.pos_noise_stddev()); @@ -291,7 +290,7 @@ struct CustomNoiseApplicator : public DefaultNoiseApplicator This class inherits from DefaultNoiseApplicator, so you can use its data members, or you can explicitly call DefaultNoiseApplicator::operator(). */ - // auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_objects) + // auto operator()(autoware_perception_msgs::msg::DetectedObjects detected_objects) // -> decltype(auto) // { // return detected_objects; @@ -299,7 +298,7 @@ struct CustomNoiseApplicator : public DefaultNoiseApplicator }; template <> -auto DetectionSensor::update( +auto DetectionSensor::update( const double current_simulation_time, const std::vector & statuses, const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) @@ -310,11 +309,11 @@ auto DetectionSensor::updat -0.002) { previous_simulation_time_ = current_simulation_time; - autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; + autoware_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header.stamp = current_ros_time; detected_objects.header.frame_id = "map"; - autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_objects; + autoware_perception_msgs::msg::TrackedObjects ground_truth_objects; ground_truth_objects.header = detected_objects.header; const auto ego_entity_status = findEgoEntityStatusToWhichThisSensorIsAttached(statuses); @@ -330,11 +329,10 @@ auto DetectionSensor::updat for (const auto & status : statuses) { if (is_in_range(status)) { - const auto detected_object = - make(status); + const auto detected_object = make(status); detected_objects.objects.push_back(detected_object); ground_truth_objects.objects.push_back( - make(status, detected_object)); + make(status, detected_object)); } } diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index a4b64bfd044..ea98c5ad36b 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -166,7 +166,7 @@ auto ScenarioSimulator::updateEntityStatus( if ( req.overwrite_ego_status() or ego_entity_simulation_->autoware->getControlModeReport().mode == - autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL) { + autoware_vehicle_msgs::msg::ControlModeReport::MANUAL) { ego_entity_simulation_->autoware->setManualMode(); traffic_simulator_msgs::msg::EntityStatus ego_status_msg; simulation_interface::toMsg(status, ego_status_msg); diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 74002da0849..64fe47c3c11 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( @@ -131,7 +131,7 @@ void SimModelDelaySteerAccGeared::updateStateWithGear( state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 033581b66a5..936f4a7b104 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include // copied from https://github.com/tier4/autoware.universe/blob/v0.17.0/common/interpolation/src/linear_interpolation.cpp @@ -280,7 +280,7 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( void SimModelDelaySteerMapAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 0976349bb0e..615a06f11f6 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(double wheelbase) @@ -69,7 +69,7 @@ void SimModelIdealSteerAccGeared::updateStateWithGear( state(IDX::YAW) = prev_state(IDX::YAW); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulation/simulation_interface/CMakeLists.txt b/simulation/simulation_interface/CMakeLists.txt index 6e2cc1fb292..00f59161a4d 100644 --- a/simulation/simulation_interface/CMakeLists.txt +++ b/simulation/simulation_interface/CMakeLists.txt @@ -23,8 +23,8 @@ include(FindProtobuf REQUIRED) ament_auto_find_build_dependencies() set(PROTO_FILES - "proto/autoware_auto_control_msgs.proto" - "proto/autoware_auto_vehicle_msgs.proto" + "proto/autoware_control_msgs.proto" + "proto/autoware_vehicle_msgs.proto" "proto/builtin_interfaces.proto" "proto/geometry_msgs.proto" "proto/rosgraph_msgs.proto" diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 4ec8320616b..0bd1f09e53f 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -172,17 +172,15 @@ void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void -DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); -DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); -DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannControlCommand); -DEFINE_CONVERSION(autoware_auto_vehicle_msgs, GearCommand); +DEFINE_CONVERSION(autoware_control_msgs, Lateral); +DEFINE_CONVERSION(autoware_control_msgs, Longitudinal); +DEFINE_CONVERSION(autoware_control_msgs, Control); +DEFINE_CONVERSION(autoware_vehicle_msgs, GearCommand); #undef DEFINE_CONVERSION auto toProto( - const std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> &, + const std::tuple &, traffic_simulator_msgs::VehicleCommand &) -> void; template diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 3a4732e7e0c..e39637dfaa0 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -10,9 +10,8 @@ ament_cmake ament_cmake_auto - autoware_auto_control_msgs - autoware_auto_vehicle_msgs - autoware_auto_perception_msgs + autoware_control_msgs + autoware_vehicle_msgs boost builtin_interfaces geometry_msgs diff --git a/simulation/simulation_interface/proto/autoware_auto_control_msgs.proto b/simulation/simulation_interface/proto/autoware_auto_control_msgs.proto deleted file mode 100644 index 9b65d53125c..00000000000 --- a/simulation/simulation_interface/proto/autoware_auto_control_msgs.proto +++ /dev/null @@ -1,24 +0,0 @@ -syntax = "proto3"; - -import "builtin_interfaces.proto"; - -package autoware_auto_control_msgs; - -message AckermannLateralCommand { - builtin_interfaces.Time stamp = 1; - float steering_tire_angle = 2; - float steering_tire_rotation_rate = 3; -} - -message LongitudinalCommand { - builtin_interfaces.Time stamp = 1; - float speed = 2; - float acceleration = 3; - float jerk = 4; -} - -message AckermannControlCommand { - builtin_interfaces.Time stamp = 1; - AckermannLateralCommand lateral = 2; - LongitudinalCommand longitudinal = 3; -} diff --git a/simulation/simulation_interface/proto/autoware_control_msgs.proto b/simulation/simulation_interface/proto/autoware_control_msgs.proto new file mode 100644 index 00000000000..1a760f54da4 --- /dev/null +++ b/simulation/simulation_interface/proto/autoware_control_msgs.proto @@ -0,0 +1,30 @@ +syntax = "proto3"; + +import "builtin_interfaces.proto"; + +package autoware_control_msgs; + +message Lateral { + builtin_interfaces.Time stamp = 1; + builtin_interfaces.Time control_time = 2; + float steering_tire_angle = 3; + float steering_tire_rotation_rate = 4; + bool is_defined_steering_tire_rotation_rate = 5; +} + +message Longitudinal { + builtin_interfaces.Time stamp = 1; + builtin_interfaces.Time control_time = 2; + float velocity = 3; + float acceleration = 4; + float jerk = 5; + bool is_defined_acceleration = 6; + bool is_defined_jerk = 7; +} + +message Control { + builtin_interfaces.Time stamp = 1; + builtin_interfaces.Time control_time = 2; + Lateral lateral = 3; + Longitudinal longitudinal = 4; +} diff --git a/simulation/simulation_interface/proto/autoware_auto_vehicle_msgs.proto b/simulation/simulation_interface/proto/autoware_vehicle_msgs.proto similarity index 94% rename from simulation/simulation_interface/proto/autoware_auto_vehicle_msgs.proto rename to simulation/simulation_interface/proto/autoware_vehicle_msgs.proto index 6b0b314a780..cf1f9767f7c 100644 --- a/simulation/simulation_interface/proto/autoware_auto_vehicle_msgs.proto +++ b/simulation/simulation_interface/proto/autoware_vehicle_msgs.proto @@ -2,7 +2,7 @@ syntax = "proto3"; import "builtin_interfaces.proto"; -package autoware_auto_vehicle_msgs; +package autoware_vehicle_msgs; enum GearCommand_Constants { NONE = 0; diff --git a/simulation/simulation_interface/proto/traffic_simulator_msgs.proto b/simulation/simulation_interface/proto/traffic_simulator_msgs.proto index 3e7a8be5b91..0f571a5cff8 100644 --- a/simulation/simulation_interface/proto/traffic_simulator_msgs.proto +++ b/simulation/simulation_interface/proto/traffic_simulator_msgs.proto @@ -1,7 +1,7 @@ syntax = "proto3"; -import "autoware_auto_control_msgs.proto"; -import "autoware_auto_vehicle_msgs.proto"; +import "autoware_control_msgs.proto"; +import "autoware_vehicle_msgs.proto"; import "geometry_msgs.proto"; package traffic_simulator_msgs; @@ -147,8 +147,8 @@ message Waypoints { } message VehicleCommand { - autoware_auto_control_msgs.AckermannControlCommand ackermann_control_command = 1; - autoware_auto_vehicle_msgs.GearCommand gear_command = 2; + autoware_control_msgs.Control control = 1; + autoware_vehicle_msgs.GearCommand gear_command = 2; } /** diff --git a/simulation/simulation_interface/src/conversions.cpp b/simulation/simulation_interface/src/conversions.cpp index 1804eb11c1a..5d006d319e9 100644 --- a/simulation/simulation_interface/src/conversions.cpp +++ b/simulation/simulation_interface/src/conversions.cpp @@ -490,8 +490,7 @@ void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header) } void toProto( - const autoware_auto_control_msgs::msg::AckermannLateralCommand & message, - autoware_auto_control_msgs::AckermannLateralCommand & proto) + const autoware_control_msgs::msg::Lateral & message, autoware_control_msgs::Lateral & proto) { toProto(message.stamp, *proto.mutable_stamp()); proto.set_steering_tire_angle(message.steering_tire_angle); @@ -499,8 +498,7 @@ void toProto( } void toMsg( - const autoware_auto_control_msgs::AckermannLateralCommand & proto, - autoware_auto_control_msgs::msg::AckermannLateralCommand & message) + const autoware_control_msgs::Lateral & proto, autoware_control_msgs::msg::Lateral & message) { toMsg(proto.stamp(), message.stamp); message.steering_tire_angle = proto.steering_tire_angle(); @@ -508,28 +506,27 @@ void toMsg( } void toProto( - const autoware_auto_control_msgs::msg::LongitudinalCommand & message, - autoware_auto_control_msgs::LongitudinalCommand & proto) + const autoware_control_msgs::msg::Longitudinal & message, + autoware_control_msgs::Longitudinal & proto) { toProto(message.stamp, *proto.mutable_stamp()); - proto.set_speed(message.speed); + proto.set_velocity(message.velocity); proto.set_acceleration(message.acceleration); proto.set_jerk(message.jerk); } void toMsg( - const autoware_auto_control_msgs::LongitudinalCommand & proto, - autoware_auto_control_msgs::msg::LongitudinalCommand & message) + const autoware_control_msgs::Longitudinal & proto, + autoware_control_msgs::msg::Longitudinal & message) { toMsg(proto.stamp(), message.stamp); - message.speed = proto.speed(); + message.velocity = proto.velocity(); message.acceleration = proto.acceleration(); message.jerk = proto.jerk(); } void toProto( - const autoware_auto_control_msgs::msg::AckermannControlCommand & message, - autoware_auto_control_msgs::AckermannControlCommand & proto) + const autoware_control_msgs::msg::Control & message, autoware_control_msgs::Control & proto) { toProto(message.stamp, *proto.mutable_stamp()); toProto(message.lateral, *proto.mutable_lateral()); @@ -537,8 +534,7 @@ void toProto( } void toMsg( - const autoware_auto_control_msgs::AckermannControlCommand & proto, - autoware_auto_control_msgs::msg::AckermannControlCommand & message) + const autoware_control_msgs::Control & proto, autoware_control_msgs::msg::Control & message) { toMsg(proto.stamp(), message.stamp); toMsg(proto.lateral(), message.lateral); @@ -546,14 +542,14 @@ void toMsg( } auto toProto( - const autoware_auto_vehicle_msgs::msg::GearCommand & message, - autoware_auto_vehicle_msgs::GearCommand & proto) -> void + const autoware_vehicle_msgs::msg::GearCommand & message, + autoware_vehicle_msgs::GearCommand & proto) -> void { toProto(message.stamp, *proto.mutable_stamp()); -#define CASE(NAME) \ - case autoware_auto_vehicle_msgs::msg::GearCommand::NAME: \ - proto.set_command(autoware_auto_vehicle_msgs::GearCommand_Constants::NAME); \ +#define CASE(NAME) \ + case autoware_vehicle_msgs::msg::GearCommand::NAME: \ + proto.set_command(autoware_vehicle_msgs::GearCommand_Constants::NAME); \ break switch (message.command) { @@ -586,20 +582,19 @@ auto toProto( } auto toMsg( - const autoware_auto_vehicle_msgs::GearCommand & proto, - autoware_auto_vehicle_msgs::msg::GearCommand & message) -> void + const autoware_vehicle_msgs::GearCommand & proto, + autoware_vehicle_msgs::msg::GearCommand & message) -> void { toMsg(proto.stamp(), message.stamp); message.command = proto.command(); } auto toProto( - const std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> & message, + const std::tuple & + message, traffic_simulator_msgs::VehicleCommand & proto) -> void { - toProto(std::get<0>(message), *proto.mutable_ackermann_control_command()); + toProto(std::get<0>(message), *proto.mutable_control()); toProto(std::get<1>(message), *proto.mutable_gear_command()); } diff --git a/simulation/simulation_interface/test/expect_equal_macros.hpp b/simulation/simulation_interface/test/expect_equal_macros.hpp index 2620c1af700..f3862a9bb6b 100644 --- a/simulation/simulation_interface/test/expect_equal_macros.hpp +++ b/simulation/simulation_interface/test/expect_equal_macros.hpp @@ -156,14 +156,14 @@ * @brief Expect equal macros for autoware related messages. */ #define EXPECT_CONTROL_COMMAND_EQ(MSG, PROTO) \ - EXPECT_DOUBLE_EQ(MSG.longitudinal.speed, PROTO.longitudinal().speed()); \ + EXPECT_DOUBLE_EQ(MSG.longitudinal.velocity, PROTO.longitudinal().velocity()); \ EXPECT_DOUBLE_EQ(MSG.longitudinal.acceleration, PROTO.longitudinal().acceleration()); \ EXPECT_DOUBLE_EQ(MSG.lateral.steering_tire_angle, PROTO.lateral().steering_tire_angle()); \ EXPECT_DOUBLE_EQ( \ MSG.lateral.steering_tire_rotation_rate, PROTO.lateral().steering_tire_rotation_rate()); #define EXPECT_VEHICLE_COMMAND_EQ(CONTROL_MSG, GEAR_MSG, EMERGENCY_MSG, PROTO) \ - EXPECT_CONTROL_COMMAND_EQ(CONTROL_MSG, PROTO.ackermann_control_command()); \ + EXPECT_CONTROL_COMMAND_EQ(CONTROL_MSG, PROTO.control()); \ EXPECT_DOUBLE_EQ(GEAR_MSG.command, PROTO.gear_command().data()); \ EXPECT_DOUBLE_EQ(EMERGENCY_MSG.emergency, PROTO.vehicle_emergency_stamped().emergency()); diff --git a/simulation/simulation_interface/test/test_conversions.cpp b/simulation/simulation_interface/test/test_conversions.cpp index 0a7bf9bccb7..833baa4fd6d 100644 --- a/simulation/simulation_interface/test/test_conversions.cpp +++ b/simulation/simulation_interface/test/test_conversions.cpp @@ -438,20 +438,20 @@ TEST(Conversion, Clock) EXPECT_CLOCK_EQ(msg, proto); } -TEST(Conversion, AckermannControlCommand) +TEST(Conversion, Control) { - autoware_auto_control_msgs::AckermannControlCommand proto; - autoware_auto_control_msgs::msg::AckermannControlCommand msg; + autoware_control_msgs::Control proto; + autoware_control_msgs::msg::Control msg; msg.longitudinal.acceleration = 3; msg.lateral.steering_tire_angle = 1.4; msg.lateral.steering_tire_rotation_rate = 13.4; - msg.longitudinal.speed = 11.3; + msg.longitudinal.velocity = 11.3; simulation_interface::toProto(msg, proto); EXPECT_CONTROL_COMMAND_EQ(msg, proto); msg.longitudinal.acceleration = 0; msg.lateral.steering_tire_angle = 0; msg.lateral.steering_tire_rotation_rate = 0; - msg.longitudinal.speed = 0; + msg.longitudinal.velocity = 0; simulation_interface::toMsg(proto, msg); EXPECT_CONTROL_COMMAND_EQ(msg, proto); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 1d5f44795fa..32b54cd4383 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -17,8 +17,6 @@ #include -#include -#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index dbe2668f758..69fc2a2de1a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_SIMULATOR__ENTITY__EGO_ENTITY_HPP_ #include -#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 5d362585dfb..1bf628fb474 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -15,8 +15,8 @@ #ifndef TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_ #define TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_ -#include -#include +#include +#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index d758b1e49e3..352ec861ecc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -28,10 +28,10 @@ #include #include -#include #include #include #include +#include #include #include #include @@ -309,7 +309,7 @@ class HdMapUtils const bool include_opposite_direction = true) const -> std::vector; - auto toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin; + auto toMapBin() const -> autoware_map_msgs::msg::LaneletMapBin; auto toMapPoints(const lanelet::Id, const std::vector & s) const -> std::vector; @@ -397,7 +397,7 @@ class HdMapUtils auto getVectorFromPose(const geometry_msgs::msg::Pose &, const double magnitude) const -> geometry_msgs::msg::Vector3; - auto mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin &) const -> void; + auto mapCallback(const autoware_map_msgs::msg::LaneletMapBin &) const -> void; auto overwriteLaneletsCenterline() -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp index 4893edf995c..e3b408cbbdb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp @@ -15,7 +15,6 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_HPP_ #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_HPP_ -#include #include #if __has_include() diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index c77a28f1124..6338946567f 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -36,8 +36,8 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - autoware_auto_perception_msgs autoware_perception_msgs + autoware_map_msgs tinyxml2_vendor pluginlib traffic_simulator_msgs diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index c0da52c49ac..e7c37d92dd6 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1607,7 +1607,7 @@ auto HdMapUtils::getLongitudinalDistance( } } -auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin +auto HdMapUtils::toMapBin() const -> autoware_map_msgs::msg::LaneletMapBin { std::stringstream ss; boost::archive::binary_oarchive oa(ss); @@ -1615,7 +1615,7 @@ auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin auto id_counter = lanelet::utils::getId(); oa << id_counter; std::string tmp_str = ss.str(); - autoware_auto_mapping_msgs::msg::HADMapBin msg; + autoware_map_msgs::msg::LaneletMapBin msg; msg.data.clear(); msg.data.resize(tmp_str.size()); msg.data.assign(tmp_str.begin(), tmp_str.end()); diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index 3da4d9f6b6b..93c180cd4b1 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -12,11 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include #include +// This message will be deleted in the future +#if __has_include() +#include +#endif + #if __has_include() #include #endif @@ -25,31 +28,7 @@ namespace traffic_simulator { -template <> -auto TrafficLightPublisher::publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void -{ - autoware_auto_perception_msgs::msg::TrafficSignalArray message; - - message.header.frame_id = frame_; - message.header.stamp = current_ros_time; - - using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; - using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; - for (const auto & traffic_light : request.states()) { - TrafficLightType traffic_light_message; - traffic_light_message.map_primitive_id = traffic_light.id(); - for (const auto & bulb_status : traffic_light.traffic_light_status()) { - TrafficLightBulbType light_bulb_message; - simulation_interface::toMsg(bulb_status, light_bulb_message); - traffic_light_message.lights.push_back(light_bulb_message); - } - message.signals.push_back(traffic_light_message); - } - traffic_light_state_array_publisher_->publish(message); -} - +#if __has_include() template <> auto TrafficLightPublisher::publish( const rclcpp::Time & current_ros_time, @@ -79,6 +58,7 @@ auto TrafficLightPublisher::p } traffic_light_state_array_publisher_->publish(message); } +#endif template <> auto TrafficLightPublisher::publish( diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index eaf5a2ab722..98897b9badf 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -33,10 +33,9 @@ def architecture_types(): - # awf/universe: autoware_auto_perception_msgs/TrafficSignalArray for traffic lights # awf/universe/20230906: autoware_perception_msgs/TrafficSignalArray for traffic lights # awf/universe/20240605: autoware_perception_msgs/TrafficLightGroupArray for traffic lights - return ["awf/universe", "awf/universe/20230906", "awf/universe/20240605"] + return ["awf/universe/20230906", "awf/universe/20240605"] def default_autoware_launch_package_of(architecture_type): @@ -45,7 +44,6 @@ def default_autoware_launch_package_of(architecture_type): f"architecture_type := {architecture_type} is not supported. Choose one of {architecture_types()}." ) return { - "awf/universe": "autoware_launch", "awf/universe/20230906": "autoware_launch", "awf/universe/20240605": "autoware_launch", }[architecture_type] @@ -57,7 +55,6 @@ def default_autoware_launch_file_of(architecture_type): f"architecture_type := {architecture_type} is not supported. Choose one of {architecture_types()}." ) return { - "awf/universe": "planning_simulator.launch.xml", "awf/universe/20230906": "planning_simulator.launch.xml", "awf/universe/20240605": "planning_simulator.launch.xml", }[architecture_type] @@ -69,7 +66,7 @@ def default_rviz_config_file(): def launch_setup(context, *args, **kwargs): # fmt: off - architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20230906") + architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20240605") autoware_launch_file = LaunchConfiguration("autoware_launch_file", default=default_autoware_launch_file_of(architecture_type.perform(context))) autoware_launch_package = LaunchConfiguration("autoware_launch_package", default=default_autoware_launch_package_of(architecture_type.perform(context))) consider_acceleration_by_road_slope = LaunchConfiguration("consider_acceleration_by_road_slope", default=False)