Skip to content

Commit 447d3b7

Browse files
fix(mrm_stop_operator): use adapi mrm state (#1473)
* fix: use adapi mrm state Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * style(pre-commit): autofix --------- Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 12da511 commit 447d3b7

File tree

3 files changed

+12
-11
lines changed

3 files changed

+12
-11
lines changed

system/mrm_stop_operator/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
<build_depend>autoware_cmake</build_depend>
1313

1414
<!-- depend -->
15+
<depend>autoware_adapi_v1_msgs</depend>
1516
<depend>autoware_auto_vehicle_msgs</depend>
1617
<depend>rclcpp</depend>
1718
<depend>rclcpp_components</depend>

system/mrm_stop_operator/src/mrm_stop_operator.cpp

+8-8
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options)
4545
create_publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>(
4646
"~/output/velocity_limit_clear_command", rclcpp::QoS{10}.transient_local());
4747
pub_mrm_state_ =
48-
create_publisher<tier4_system_msgs::msg::MrmState>("~/output/mrm_state", rclcpp::QoS{1});
48+
create_publisher<autoware_adapi_v1_msgs::msg::MrmState>("~/output/mrm_state", rclcpp::QoS{1});
4949

5050
// Timer
5151
const auto update_period_ns = rclcpp::Rate(10).period();
@@ -80,24 +80,24 @@ void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::Co
8080
pub_velocity_limit_->publish(velocity_limit);
8181

8282
last_mrm_request_ = *msg;
83-
current_mrm_state_.behavior = *msg;
84-
current_mrm_state_.state = tier4_system_msgs::msg::MrmState::MRM_OPERATING;
83+
current_mrm_state_.behavior = msg->type;
84+
current_mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING;
8585
}
8686
}
8787

8888
void MrmStopOperator::initState()
8989
{
9090
last_mrm_request_.type = tier4_system_msgs::msg::MrmBehavior::NONE;
91-
current_mrm_state_.state = tier4_system_msgs::msg::MrmState::NORMAL;
92-
current_mrm_state_.behavior.type = tier4_system_msgs::msg::MrmBehavior::NONE;
91+
current_mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL;
92+
current_mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
9393
}
9494

9595
void MrmStopOperator::onTimer()
9696
{
97-
if (current_mrm_state_.state == tier4_system_msgs::msg::MrmState::MRM_OPERATING) {
98-
if (current_mrm_state_.behavior.type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) {
97+
if (current_mrm_state_.state == autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING) {
98+
if (current_mrm_state_.behavior == autoware_adapi_v1_msgs::msg::MrmState::COMFORTABLE_STOP) {
9999
if (isStopped()) {
100-
current_mrm_state_.state = tier4_system_msgs::msg::MrmState::MRM_SUCCEEDED;
100+
current_mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED;
101101
} else {
102102
// nothing to do
103103
}

system/mrm_stop_operator/src/mrm_stop_operator.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -18,11 +18,11 @@
1818
// include
1919
#include <rclcpp/rclcpp.hpp>
2020

21+
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
2122
#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
2223
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
2324
#include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
2425
#include <tier4_system_msgs/msg/mrm_behavior.hpp>
25-
#include <tier4_system_msgs/msg/mrm_state.hpp>
2626

2727
namespace mrm_stop_operator
2828
{
@@ -56,7 +56,7 @@ class MrmStopOperator : public rclcpp::Node
5656
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
5757
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>::SharedPtr
5858
pub_velocity_limit_clear_command_;
59-
rclcpp::Publisher<tier4_system_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
59+
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
6060
// Service
6161

6262
// Client
@@ -68,7 +68,7 @@ class MrmStopOperator : public rclcpp::Node
6868

6969
// State
7070
tier4_system_msgs::msg::MrmBehavior last_mrm_request_;
71-
tier4_system_msgs::msg::MrmState current_mrm_state_;
71+
autoware_adapi_v1_msgs::msg::MrmState current_mrm_state_;
7272

7373
void initState();
7474
void onTimer();

0 commit comments

Comments
 (0)