From d8008411140d75e8b0d5fd8167b4a37b07efb79e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 8 Jul 2024 12:07:45 +0900 Subject: [PATCH] fix(autoware_iv_internal_api_adaptor): iv msg types (#121) Signed-off-by: Takagi, Isamu --- .../src/iv_msgs.cpp | 40 ++++++++-------- .../src/iv_msgs.hpp | 46 +++++++++---------- 2 files changed, 43 insertions(+), 43 deletions(-) diff --git a/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp b/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp index 356c17f..af53f5c 100644 --- a/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp +++ b/autoware_iv_internal_api_adaptor/src/iv_msgs.cpp @@ -22,60 +22,60 @@ IVMsgs::IVMsgs(const rclcpp::NodeOptions & options) : Node("external_api_iv_msgs { using std::placeholders::_1; - pub_state_ = create_publisher("/api/iv_msgs/autoware/state", rclcpp::QoS(1)); - sub_state_ = create_subscription( + pub_state_ = create_publisher("/api/iv_msgs/autoware/state", rclcpp::QoS(1)); + sub_state_ = create_subscription( "/autoware/state", rclcpp::QoS(1), std::bind(&IVMsgs::onState, this, _1)); - sub_emergency_ = create_subscription( + sub_emergency_ = create_subscription( "/system/fail_safe/mrm_state", rclcpp::QoS(1), std::bind(&IVMsgs::onEmergency, this, _1)); pub_control_mode_ = - create_publisher("/api/iv_msgs/vehicle/status/control_mode", rclcpp::QoS(1)); - sub_control_mode_ = create_subscription( + create_publisher("/api/iv_msgs/vehicle/status/control_mode", rclcpp::QoS(1)); + sub_control_mode_ = create_subscription( "/vehicle/status/control_mode", rclcpp::QoS(1), std::bind(&IVMsgs::onControlMode, this, _1)); - pub_trajectory_ = create_publisher( + pub_trajectory_ = create_publisher( "/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1)); - sub_trajectory_ = create_subscription( + sub_trajectory_ = create_subscription( "/planning/scenario_planning/trajectory", rclcpp::QoS(1), std::bind(&IVMsgs::onTrajectory, this, _1)); - pub_dynamic_objects_ = create_publisher( + pub_dynamic_objects_ = create_publisher( "/api/iv_msgs/perception/object_recognition/tracking/objects", rclcpp::QoS(1)); - sub_tracked_objects_ = create_subscription( + sub_tracked_objects_ = create_subscription( "/perception/object_recognition/tracking/objects", rclcpp::QoS(1), std::bind(&IVMsgs::onTrackedObjects, this, _1)); is_emergency_ = false; } -void IVMsgs::onState(const AutowareStateAuto::ConstSharedPtr message) +void IVMsgs::onState(const AutowareStateInput::ConstSharedPtr message) { auto state = tier4_auto_msgs_converter::convert(*message); if (is_emergency_) { - state.state = AutowareStateIV::EMERGENCY; + state.state = AutowareStateOutput::EMERGENCY; } pub_state_->publish(state); } -void IVMsgs::onEmergency(const EmergencyStateAuto::ConstSharedPtr message) +void IVMsgs::onEmergency(const EmergencyStateInput::ConstSharedPtr message) { - is_emergency_ = message->state != EmergencyStateAuto::NORMAL; + is_emergency_ = message->state != EmergencyStateInput::NORMAL; } -void IVMsgs::onControlMode(const ControlMode::ConstSharedPtr message) +void IVMsgs::onControlMode(const ControlModeInput::ConstSharedPtr message) { - ControlModeAuto control_mode_auto; - control_mode_auto.stamp = message->stamp; - control_mode_auto.mode = message->mode; - pub_control_mode_->publish(control_mode_auto); + ControlModeOutput control_mode; + control_mode.stamp = message->stamp; + control_mode.mode = message->mode; + pub_control_mode_->publish(control_mode); } -void IVMsgs::onTrajectory(const TrajectoryAuto::ConstSharedPtr message) +void IVMsgs::onTrajectory(const TrajectoryInput::ConstSharedPtr message) { pub_trajectory_->publish(tier4_auto_msgs_converter::convert(*message)); } -void IVMsgs::onTrackedObjects(const TrackedObjectsAuto::ConstSharedPtr message) +void IVMsgs::onTrackedObjects(const TrackedObjectsInput::ConstSharedPtr message) { pub_dynamic_objects_->publish(tier4_auto_msgs_converter::convert(*message)); } diff --git a/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp b/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp index 37a6cc7..ef5eb3c 100644 --- a/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp +++ b/autoware_iv_internal_api_adaptor/src/iv_msgs.hpp @@ -35,33 +35,33 @@ class IVMsgs : public rclcpp::Node explicit IVMsgs(const rclcpp::NodeOptions & options); private: - using EmergencyStateAuto = autoware_adapi_v1_msgs::msg::MrmState; - using AutowareStateAuto = autoware_system_msgs::msg::AutowareState; - using AutowareStateIV = tier4_system_msgs::msg::AutowareState; - rclcpp::Subscription::SharedPtr sub_emergency_; - rclcpp::Subscription::SharedPtr sub_state_; - rclcpp::Publisher::SharedPtr pub_state_; + using EmergencyStateInput = autoware_adapi_v1_msgs::msg::MrmState; + using AutowareStateInput = autoware_system_msgs::msg::AutowareState; + using AutowareStateOutput = tier4_system_msgs::msg::AutowareState; + rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr sub_state_; + rclcpp::Publisher::SharedPtr pub_state_; - using ControlModeAuto = autoware_vehicle_msgs::msg::ControlModeReport; - using ControlMode = autoware_auto_vehicle_msgs::msg::ControlModeReport; - rclcpp::Subscription::SharedPtr sub_control_mode_; - rclcpp::Publisher::SharedPtr pub_control_mode_; + using ControlModeInput = autoware_vehicle_msgs::msg::ControlModeReport; + using ControlModeOutput = autoware_auto_vehicle_msgs::msg::ControlModeReport; + rclcpp::Subscription::SharedPtr sub_control_mode_; + rclcpp::Publisher::SharedPtr pub_control_mode_; - using TrajectoryAuto = autoware_planning_msgs::msg::Trajectory; - using TrajectoryIV = tier4_planning_msgs::msg::Trajectory; - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Publisher::SharedPtr pub_trajectory_; + using TrajectoryInput = autoware_planning_msgs::msg::Trajectory; + using TrajectoryOutput = tier4_planning_msgs::msg::Trajectory; + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Publisher::SharedPtr pub_trajectory_; - using TrackedObjectsAuto = autoware_perception_msgs::msg::TrackedObjects; - using DynamicObjectsIV = tier4_perception_msgs::msg::DynamicObjectArray; - rclcpp::Subscription::SharedPtr sub_tracked_objects_; - rclcpp::Publisher::SharedPtr pub_dynamic_objects_; + using TrackedObjectsInput = autoware_perception_msgs::msg::TrackedObjects; + using DynamicObjectsOutput = tier4_perception_msgs::msg::DynamicObjectArray; + rclcpp::Subscription::SharedPtr sub_tracked_objects_; + rclcpp::Publisher::SharedPtr pub_dynamic_objects_; - void onState(const AutowareStateAuto::ConstSharedPtr message); - void onEmergency(const EmergencyStateAuto::ConstSharedPtr message); - void onControlMode(const ControlMode::ConstSharedPtr message); - void onTrajectory(const TrajectoryAuto::ConstSharedPtr message); - void onTrackedObjects(const TrackedObjectsAuto::ConstSharedPtr message); + void onState(const AutowareStateInput::ConstSharedPtr message); + void onEmergency(const EmergencyStateInput::ConstSharedPtr message); + void onControlMode(const ControlModeInput::ConstSharedPtr message); + void onTrajectory(const TrajectoryInput::ConstSharedPtr message); + void onTrackedObjects(const TrackedObjectsInput::ConstSharedPtr message); bool is_emergency_; };