diff --git a/planning/trajectory_repeater/src/trajectory_repeater.cpp b/planning/trajectory_repeater/src/trajectory_repeater.cpp index af6852c70398a..0b1b7880f9a17 100644 --- a/planning/trajectory_repeater/src/trajectory_repeater.cpp +++ b/planning/trajectory_repeater/src/trajectory_repeater.cpp @@ -24,10 +24,12 @@ TrajectoryRepeater::TrajectoryRepeater(const rclcpp::NodeOptions & node_options) // Subscriber sub_trajectory_ = create_subscription( - "~/input/trajectory", 10, std::bind(&TrajectoryRepeater::onTrajectory, this, std::placeholders::_1)); + "~/input/trajectory", 10, + std::bind(&TrajectoryRepeater::onTrajectory, this, std::placeholders::_1)); // Publisher - pub_trajectory_ = create_publisher("~/output/trajectory", 10); + pub_trajectory_ = + create_publisher("~/output/trajectory", 10); // Service @@ -35,15 +37,16 @@ TrajectoryRepeater::TrajectoryRepeater(const rclcpp::NodeOptions & node_options) // Timer using namespace std::literals::chrono_literals; - timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrajectoryRepeater::onTimer, this)); + timer_ = + rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrajectoryRepeater::onTimer, this)); // State // Diagnostics - } -void TrajectoryRepeater::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +void TrajectoryRepeater::onTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { last_trajectory_ = msg; } @@ -54,7 +57,7 @@ void TrajectoryRepeater::onTimer() RCLCPP_DEBUG(get_logger(), "No trajectory received"); return; } - + pub_trajectory_->publish(*last_trajectory_); } diff --git a/planning/trajectory_repeater/src/trajectory_repeater.hpp b/planning/trajectory_repeater/src/trajectory_repeater.hpp index 4b6d47823818b..83e9b396dae81 100644 --- a/planning/trajectory_repeater/src/trajectory_repeater.hpp +++ b/planning/trajectory_repeater/src/trajectory_repeater.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ -#define TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ +#ifndef TRAJECTORY_REPEATER_HPP_ +#define TRAJECTORY_REPEATER_HPP_ // include #include @@ -46,15 +46,14 @@ class TrajectoryRepeater : public rclcpp::Node // Timer rclcpp::TimerBase::SharedPtr timer_; - + void onTimer(); // State autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_trajectory_; // Diagnostics - }; } // namespace trajectory_repeater -#endif // TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ \ No newline at end of file +#endif // TRAJECTORY_REPEATER_HPP_ diff --git a/system/leader_election_converter/README.md b/system/leader_election_converter/README.md index af98223bfaf39..5f61820be7e83 100644 --- a/system/leader_election_converter/README.md +++ b/system/leader_election_converter/README.md @@ -23,12 +23,12 @@ In addition, it receives a udp packet`MrmState` and publish `/system/mrm_request ### Interface -| Interface Type | Interface Name | Data Type | Description | -| -------------- | ------------------------------ | ----------------------------------- | ------------------------ | -| subscriber | `/system/fail_safe/mrm_state` | `tier4_system_msgs/msg/MrmState` | MRM status of each ECU. | -| udp sender | none | `struct MrmState` | Same as above. | -| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. | -| udp receiver | none | `struct MrmRequest` | Same as above. | +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------ | ------------------------------------- | ------------------------ | +| subscriber | `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs/msg/MrmState` | MRM status of each ECU. | +| udp sender | none | `struct MrmState` | Same as above. | +| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. | +| udp receiver | none | `struct MrmRequest` | Same as above. | ## log converter diff --git a/system/leader_election_converter/src/common/converter/log_converter.hpp b/system/leader_election_converter/src/common/converter/log_converter.hpp index af0113544bf7f..24972b7117dbd 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.hpp +++ b/system/leader_election_converter/src/common/converter/log_converter.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.cpp b/system/leader_election_converter/src/common/converter/mrm_converter.cpp index 9fe6c58a04ab8..deeb8fa7b7dcb 100644 --- a/system/leader_election_converter/src/common/converter/mrm_converter.cpp +++ b/system/leader_election_converter/src/common/converter/mrm_converter.cpp @@ -65,7 +65,7 @@ void MrmConverter::setSubscriber() rclcpp::SubscriptionOptions options; options.callback_group = callback_group_; - sub_mrm_state_ = node_->create_subscription( + sub_mrm_state_ = node_->create_subscription( "~/input/mrm_state", qos, std::bind(&MrmConverter::convertToUdp, this, std::placeholders::_1), options); } @@ -77,11 +77,11 @@ void MrmConverter::setPublisher() } void MrmConverter::convertToUdp( - const tier4_system_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg) + const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg) { MrmState mrm_state; mrm_state.state = mrm_state_msg->state; - mrm_state.behavior = mrm_state_msg->behavior.type; + mrm_state.behavior = mrm_state_msg->behavior; udp_mrm_state_sender_->send(mrm_state); } diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.hpp b/system/leader_election_converter/src/common/converter/mrm_converter.hpp index 347a816029ecc..f01c5759cdb0b 100644 --- a/system/leader_election_converter/src/common/converter/mrm_converter.hpp +++ b/system/leader_election_converter/src/common/converter/mrm_converter.hpp @@ -20,8 +20,8 @@ #include +#include #include -#include #include #include @@ -33,8 +33,8 @@ namespace leader_election_converter typedef struct MrmState { - tier4_system_msgs::msg::MrmState::_state_type state; - tier4_system_msgs::msg::MrmBehavior::_type_type behavior; + autoware_adapi_v1_msgs::msg::MrmState::_state_type state; + autoware_adapi_v1_msgs::msg::MrmState::_behavior_type behavior; } MrmState; typedef struct MrmRequest @@ -55,14 +55,14 @@ class MrmConverter private: void startUdpReceiver(const std::string & src_ip, const std::string & src_port); - void convertToUdp(const tier4_system_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg); + void convertToUdp(const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg); void convertToTopic(const MrmRequest & mrm_request); rclcpp::Node * node_; std::unique_ptr> udp_mrm_state_sender_; std::unique_ptr> udp_mrm_request_receiver_; rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::Subscription::SharedPtr sub_mrm_state_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; rclcpp::Publisher::SharedPtr pub_mrm_request_; std::thread udp_receiver_thread_;