Skip to content

Commit

Permalink
fix(autoware_iv_internal_api_adaptor): iv msg types (#121)
Browse files Browse the repository at this point in the history
Signed-off-by: Takagi, Isamu <[email protected]>
  • Loading branch information
isamu-takagi authored Jul 8, 2024
1 parent c2f6a89 commit d800841
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 43 deletions.
40 changes: 20 additions & 20 deletions autoware_iv_internal_api_adaptor/src/iv_msgs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,60 +22,60 @@ IVMsgs::IVMsgs(const rclcpp::NodeOptions & options) : Node("external_api_iv_msgs
{
using std::placeholders::_1;

pub_state_ = create_publisher<AutowareStateIV>("/api/iv_msgs/autoware/state", rclcpp::QoS(1));
sub_state_ = create_subscription<AutowareStateAuto>(
pub_state_ = create_publisher<AutowareStateOutput>("/api/iv_msgs/autoware/state", rclcpp::QoS(1));
sub_state_ = create_subscription<AutowareStateInput>(
"/autoware/state", rclcpp::QoS(1), std::bind(&IVMsgs::onState, this, _1));
sub_emergency_ = create_subscription<EmergencyStateAuto>(
sub_emergency_ = create_subscription<EmergencyStateInput>(
"/system/fail_safe/mrm_state", rclcpp::QoS(1), std::bind(&IVMsgs::onEmergency, this, _1));

pub_control_mode_ =
create_publisher<ControlModeAuto>("/api/iv_msgs/vehicle/status/control_mode", rclcpp::QoS(1));
sub_control_mode_ = create_subscription<ControlMode>(
create_publisher<ControlModeOutput>("/api/iv_msgs/vehicle/status/control_mode", rclcpp::QoS(1));
sub_control_mode_ = create_subscription<ControlModeInput>(
"/vehicle/status/control_mode", rclcpp::QoS(1), std::bind(&IVMsgs::onControlMode, this, _1));

pub_trajectory_ = create_publisher<TrajectoryIV>(
pub_trajectory_ = create_publisher<TrajectoryOutput>(
"/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1));
sub_trajectory_ = create_subscription<TrajectoryAuto>(
sub_trajectory_ = create_subscription<TrajectoryInput>(
"/planning/scenario_planning/trajectory", rclcpp::QoS(1),
std::bind(&IVMsgs::onTrajectory, this, _1));

pub_dynamic_objects_ = create_publisher<DynamicObjectsIV>(
pub_dynamic_objects_ = create_publisher<DynamicObjectsOutput>(
"/api/iv_msgs/perception/object_recognition/tracking/objects", rclcpp::QoS(1));
sub_tracked_objects_ = create_subscription<TrackedObjectsAuto>(
sub_tracked_objects_ = create_subscription<TrackedObjectsInput>(
"/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));
}
Expand Down
46 changes: 23 additions & 23 deletions autoware_iv_internal_api_adaptor/src/iv_msgs.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<EmergencyStateAuto>::SharedPtr sub_emergency_;
rclcpp::Subscription<AutowareStateAuto>::SharedPtr sub_state_;
rclcpp::Publisher<AutowareStateIV>::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<EmergencyStateInput>::SharedPtr sub_emergency_;
rclcpp::Subscription<AutowareStateInput>::SharedPtr sub_state_;
rclcpp::Publisher<AutowareStateOutput>::SharedPtr pub_state_;

using ControlModeAuto = autoware_vehicle_msgs::msg::ControlModeReport;
using ControlMode = autoware_auto_vehicle_msgs::msg::ControlModeReport;
rclcpp::Subscription<ControlMode>::SharedPtr sub_control_mode_;
rclcpp::Publisher<ControlModeAuto>::SharedPtr pub_control_mode_;
using ControlModeInput = autoware_vehicle_msgs::msg::ControlModeReport;
using ControlModeOutput = autoware_auto_vehicle_msgs::msg::ControlModeReport;
rclcpp::Subscription<ControlModeInput>::SharedPtr sub_control_mode_;
rclcpp::Publisher<ControlModeOutput>::SharedPtr pub_control_mode_;

using TrajectoryAuto = autoware_planning_msgs::msg::Trajectory;
using TrajectoryIV = tier4_planning_msgs::msg::Trajectory;
rclcpp::Subscription<TrajectoryAuto>::SharedPtr sub_trajectory_;
rclcpp::Publisher<TrajectoryIV>::SharedPtr pub_trajectory_;
using TrajectoryInput = autoware_planning_msgs::msg::Trajectory;
using TrajectoryOutput = tier4_planning_msgs::msg::Trajectory;
rclcpp::Subscription<TrajectoryInput>::SharedPtr sub_trajectory_;
rclcpp::Publisher<TrajectoryOutput>::SharedPtr pub_trajectory_;

using TrackedObjectsAuto = autoware_perception_msgs::msg::TrackedObjects;
using DynamicObjectsIV = tier4_perception_msgs::msg::DynamicObjectArray;
rclcpp::Subscription<TrackedObjectsAuto>::SharedPtr sub_tracked_objects_;
rclcpp::Publisher<DynamicObjectsIV>::SharedPtr pub_dynamic_objects_;
using TrackedObjectsInput = autoware_perception_msgs::msg::TrackedObjects;
using DynamicObjectsOutput = tier4_perception_msgs::msg::DynamicObjectArray;
rclcpp::Subscription<TrackedObjectsInput>::SharedPtr sub_tracked_objects_;
rclcpp::Publisher<DynamicObjectsOutput>::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_;
};
Expand Down

0 comments on commit d800841

Please sign in to comment.