Skip to content

Commit 841a0b0

Browse files
TetsuKawaTetsuKawa
committed
feat: add mrm stop operator
Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
1 parent db51b36 commit 841a0b0

7 files changed

+305
-0
lines changed

‎system/mrm_stop_operator/CMakeLists.txt

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(mrm_stop_operator)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(mrm_stop_operator SHARED
8+
src/mrm_stop_operator.cpp
9+
)
10+
ament_target_dependencies(mrm_stop_operator)
11+
12+
rclcpp_components_register_node(${PROJECT_NAME}
13+
PLUGIN "mrm_stop_operator::MrmStopOperator"
14+
EXECUTABLE ${PROJECT_NAME}_node
15+
)
16+
17+
ament_auto_package(
18+
INSTALL_TO_SHARE
19+
launch
20+
config
21+
)

‎system/mrm_stop_operator/README.md

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
# Mrm Stop Operator
2+
3+
## Purpose
4+
5+
## Inner-workings / Algorithms
6+
7+
## Inputs / Outputs
8+
9+
### Input
10+
11+
### Output
12+
13+
## Parameters
14+
15+
## Assumptions / Known limits
16+
17+
## (Optional) Error detection and handling
18+
19+
## (Optional) Performance characterization
20+
21+
## (Optional) References/External links
22+
23+
## (Optional) Future extensions / Unimplemented parts

‎system/mrm_stop_operator/config/mrm_stop_operator.param.yaml

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
/**:
2+
ros__parameters:
3+
min_acceleration: -4.0 # min acceleration for sub ecu mrm stop [m/s^2]
4+
max_jerk: 5.0 # max jerk for sub ecu mrm stop [m/s^3]
5+
min_jerk: -5.0 # min jerk for sub ecu mrm stop [m/s^3]

‎system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<launch>
2+
<arg name="mrm_stop_operator_param_path" default="$(find-pkg-share mrm_stop_operator)/config/mrm_stop_operator.param.yaml"/>
3+
<arg name="input_mrm_request" default="/system/mrm_request"/>
4+
<arg name="input_velocity" default="/vehicle/status/velocity_status"/>
5+
<arg name="output_max_velocity" default="/planning/scenario_planning/max_velocity_candidates"/>
6+
<arg name="output_max_velocity_clear_command" default="/planning/scenario_planning/clear_velocity_limit"/>
7+
<arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
8+
9+
<node pkg="mrm_stop_operator" exec="mrm_stop_operator_node" name="mrm_stop_operator" output="screen">
10+
<param from="$(var mrm_stop_operator_param_path)"/>
11+
12+
<remap from="~/input/mrm_request" to="$(var input_mrm_request)"/>
13+
<remap from="~/input/velocity" to="$(var input_velocity)"/>
14+
<remap from="~/output/velocity_limit" to="$(var output_max_velocity)"/>
15+
<remap from="~/output/velocity_limit_clear_command" to="$(var output_max_velocity_clear_command)"/>
16+
<remap from="~/output/mrm_state" to="$(var output_mrm_state)"/>
17+
</node>
18+
</launch>

‎system/mrm_stop_operator/package.xml

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>mrm_stop_operator</name>
5+
<version>0.1.0</version>
6+
<description>The mrm_stop_operator package</description>
7+
<maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
11+
12+
<build_depend>autoware_cmake</build_depend>
13+
14+
<!-- depend -->
15+
<depend>autoware_adapi_v1_msgs</depend>
16+
<depend>autoware_vehicle_msgs</depend>
17+
<depend>rclcpp</depend>
18+
<depend>rclcpp_components</depend>
19+
<depend>tier4_planning_msgs</depend>
20+
<depend>tier4_system_msgs</depend>
21+
22+
<test_depend>ament_lint_auto</test_depend>
23+
<test_depend>autoware_lint_common</test_depend>
24+
25+
<export>
26+
<build_type>ament_cmake</build_type>
27+
</export>
28+
</package>

‎system/mrm_stop_operator/src/mrm_stop_operator.cpp

+129
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,129 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "mrm_stop_operator.hpp"
16+
17+
namespace mrm_stop_operator
18+
{
19+
20+
MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options)
21+
: Node("mrm_stop_operator", node_options)
22+
{
23+
// Parameter
24+
params_.min_acceleration = declare_parameter<double>("min_acceleration", -4.0);
25+
params_.max_jerk = declare_parameter<double>("max_jerk", 5.0);
26+
params_.min_jerk = declare_parameter<double>("min_jerk", -5.0);
27+
28+
// Subscriber
29+
sub_mrm_request_ = create_subscription<tier4_system_msgs::msg::MrmBehavior>(
30+
"~/input/mrm_request", 10,
31+
std::bind(&MrmStopOperator::onMrmRequest, this, std::placeholders::_1));
32+
33+
sub_velocity_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
34+
rclcpp::SubscriptionOptions velocity_options = rclcpp::SubscriptionOptions();
35+
velocity_options.callback_group = sub_velocity_group_;
36+
auto not_executed_callback = []([[maybe_unused]] const typename autoware_vehicle_msgs::msg::
37+
VelocityReport::ConstSharedPtr msg) {};
38+
sub_velocity_ = create_subscription<autoware_vehicle_msgs::msg::VelocityReport>(
39+
"~/input/velocity", 10, not_executed_callback, velocity_options);
40+
41+
// Publisher
42+
pub_velocity_limit_ = create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
43+
"~/output/velocity_limit", rclcpp::QoS{10}.transient_local());
44+
pub_velocity_limit_clear_command_ =
45+
create_publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>(
46+
"~/output/velocity_limit_clear_command", rclcpp::QoS{10}.transient_local());
47+
pub_mrm_state_ =
48+
create_publisher<autoware_adapi_v1_msgs::msg::MrmState>("~/output/mrm_state", rclcpp::QoS{1});
49+
50+
// Timer
51+
const auto update_period_ns = rclcpp::Rate(10).period();
52+
timer_ = rclcpp::create_timer(
53+
this, get_clock(), update_period_ns, std::bind(&MrmStopOperator::onTimer, this));
54+
55+
// Service
56+
57+
// Client
58+
59+
// Timer
60+
61+
// State
62+
initState();
63+
64+
// Diagnostics
65+
}
66+
67+
void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg)
68+
{
69+
if (
70+
msg->type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP &&
71+
last_mrm_request_.type != tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) {
72+
tier4_planning_msgs::msg::VelocityLimit velocity_limit;
73+
velocity_limit.stamp = this->now();
74+
velocity_limit.max_velocity = 0.0;
75+
velocity_limit.use_constraints = true;
76+
velocity_limit.constraints.min_acceleration = params_.min_acceleration;
77+
velocity_limit.constraints.max_jerk = params_.max_jerk;
78+
velocity_limit.constraints.min_jerk = params_.min_jerk;
79+
velocity_limit.sender = "mrm_stop_operator";
80+
pub_velocity_limit_->publish(velocity_limit);
81+
82+
last_mrm_request_ = *msg;
83+
current_mrm_state_.behavior = msg->type;
84+
current_mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING;
85+
}
86+
}
87+
88+
void MrmStopOperator::initState()
89+
{
90+
last_mrm_request_.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;
93+
}
94+
95+
void MrmStopOperator::onTimer()
96+
{
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) {
99+
if (isStopped()) {
100+
current_mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED;
101+
} else {
102+
// nothing to do
103+
}
104+
} else {
105+
// TODO
106+
}
107+
}
108+
current_mrm_state_.stamp = this->now();
109+
pub_mrm_state_->publish(current_mrm_state_);
110+
}
111+
112+
bool MrmStopOperator::isStopped()
113+
{
114+
constexpr auto th_stopped_velocity = 0.001;
115+
auto current_velocity = std::make_shared<autoware_vehicle_msgs::msg::VelocityReport>();
116+
rclcpp::MessageInfo message_info;
117+
118+
const bool success = sub_velocity_->take(*current_velocity, message_info);
119+
if (success) {
120+
return current_velocity->longitudinal_velocity < th_stopped_velocity;
121+
} else {
122+
return false;
123+
}
124+
}
125+
126+
} // namespace mrm_stop_operator
127+
128+
#include <rclcpp_components/register_node_macro.hpp>
129+
RCLCPP_COMPONENTS_REGISTER_NODE(mrm_stop_operator::MrmStopOperator)

‎system/mrm_stop_operator/src/mrm_stop_operator.hpp

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef MRM_STOP_OPERATOR_HPP_
16+
#define MRM_STOP_OPERATOR_HPP_
17+
18+
// include
19+
#include <rclcpp/rclcpp.hpp>
20+
21+
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
22+
#include <autoware_vehicle_msgs/msg/velocity_report.hpp>
23+
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
24+
#include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
25+
#include <tier4_system_msgs/msg/mrm_behavior.hpp>
26+
27+
namespace mrm_stop_operator
28+
{
29+
30+
struct Parameters
31+
{
32+
double min_acceleration;
33+
double max_jerk;
34+
double min_jerk;
35+
};
36+
37+
class MrmStopOperator : public rclcpp::Node
38+
{
39+
public:
40+
explicit MrmStopOperator(const rclcpp::NodeOptions & node_options);
41+
~MrmStopOperator() = default;
42+
43+
private:
44+
// Parameter
45+
Parameters params_;
46+
47+
// Subscriber
48+
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehavior>::SharedPtr sub_mrm_request_;
49+
rclcpp::Subscription<autoware_vehicle_msgs::msg::VelocityReport>::SharedPtr sub_velocity_;
50+
51+
void onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg);
52+
53+
// Service
54+
55+
// Publisher
56+
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
57+
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>::SharedPtr
58+
pub_velocity_limit_clear_command_;
59+
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
60+
// Service
61+
62+
// Client
63+
64+
// Timer
65+
rclcpp::TimerBase::SharedPtr timer_;
66+
67+
rclcpp::CallbackGroup::SharedPtr sub_velocity_group_;
68+
69+
// State
70+
tier4_system_msgs::msg::MrmBehavior last_mrm_request_;
71+
autoware_adapi_v1_msgs::msg::MrmState current_mrm_state_;
72+
73+
void initState();
74+
void onTimer();
75+
bool isStopped();
76+
77+
// Diagnostics
78+
};
79+
} // namespace mrm_stop_operator
80+
81+
#endif // MRM_STOP_OPERATOR_HPP_

0 commit comments

Comments
 (0)