Skip to content

Commit 6013a40

Browse files
satoshi-otasatoshi-ota
authored
feat(awapi): add velocity factor converter (#129)
* feat(awapi): add velocity factor converter Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix: use velocity factor converter Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix: remove unused subscriber Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent cf7ebc3 commit 6013a40

6 files changed

+178
-8
lines changed

‎awapi_awiv_adapter/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ ament_auto_add_executable(awapi_awiv_adapter
88
src/awapi_awiv_adapter_node.cpp
99
src/awapi_awiv_adapter_core.cpp
1010
src/awapi_vehicle_state_publisher.cpp
11+
src/awapi_velocity_factor_converter.cpp
1112
src/awapi_autoware_state_publisher.cpp
1213
src/awapi_stop_reason_aggregator.cpp
1314
src/awapi_v2x_aggregator.cpp

‎awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp

+6-1
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp"
2424
#include "awapi_awiv_adapter/awapi_v2x_aggregator.hpp"
2525
#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp"
26+
#include "awapi_awiv_adapter/awapi_velocity_factor_converter.hpp"
2627

2728
#include <rclcpp/rclcpp.hpp>
2829

@@ -74,6 +75,8 @@ class AutowareIvAdapter : public rclcpp::Node
7475
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr sub_emergency_;
7576
rclcpp::Subscription<autoware_system_msgs::msg::HazardStatusStamped>::SharedPtr
7677
sub_hazard_status_;
78+
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
79+
sub_velocity_factor_;
7780
rclcpp::Subscription<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr sub_stop_reason_;
7881
rclcpp::Subscription<tier4_v2x_msgs::msg::InfrastructureCommandArray>::SharedPtr sub_v2x_command_;
7982
rclcpp::Subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
@@ -123,7 +126,8 @@ class AutowareIvAdapter : public rclcpp::Node
123126
void callbackMrmState(const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr msg_ptr);
124127
void callbackHazardStatus(
125128
const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr);
126-
void callbackStopReason(const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr);
129+
void callbackVelocityFactor(
130+
const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr msg_ptr);
127131
void callbackV2XCommand(
128132
const tier4_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr);
129133
void callbackV2XState(
@@ -156,6 +160,7 @@ class AutowareIvAdapter : public rclcpp::Node
156160
AutowareInfo aw_info_;
157161
std::unique_ptr<AutowareIvVehicleStatePublisher> vehicle_state_publisher_;
158162
std::unique_ptr<AutowareIvAutowareStatePublisher> autoware_state_publisher_;
163+
std::unique_ptr<AutowareIvVelocityFactorConverter> velocity_factor_converter_;
159164
std::unique_ptr<AutowareIvStopReasonAggregator> stop_reason_aggregator_;
160165
std::unique_ptr<AutowareIvV2XAggregator> v2x_aggregator_;
161166
std::unique_ptr<AutowareIvLaneChangeStatePublisher> lane_change_state_publisher_;

‎awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_velocity_factor_converter.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 AWAPI_AWIV_ADAPTER__AWAPI_VELOCITY_FACTOR_CONVERTER_HPP_
16+
#define AWAPI_AWIV_ADAPTER__AWAPI_VELOCITY_FACTOR_CONVERTER_HPP_
17+
18+
#include "awapi_awiv_adapter/awapi_autoware_util.hpp"
19+
20+
#include <rclcpp/rclcpp.hpp>
21+
22+
#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
23+
#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
24+
#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
25+
26+
#include <map>
27+
#include <string>
28+
#include <vector>
29+
30+
namespace autoware_api
31+
{
32+
33+
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
34+
using tier4_planning_msgs::msg::StopReason;
35+
36+
const std::map<std::string, std::string> conversion_map = {
37+
{PlanningBehavior::AVOIDANCE, StopReason::AVOIDANCE},
38+
{PlanningBehavior::CROSSWALK, StopReason::CROSSWALK},
39+
{PlanningBehavior::GOAL_PLANNER, StopReason::GOAL_PLANNER},
40+
{PlanningBehavior::INTERSECTION, StopReason::INTERSECTION},
41+
{PlanningBehavior::LANE_CHANGE, StopReason::LANE_CHANGE},
42+
{PlanningBehavior::MERGE, StopReason::MERGE_FROM_PRIVATE_ROAD},
43+
{PlanningBehavior::NO_DRIVABLE_LANE, StopReason::NO_DRIVABLE_LANE},
44+
{PlanningBehavior::NO_STOPPING_AREA, StopReason::NO_STOPPING_AREA},
45+
{PlanningBehavior::REAR_CHECK, StopReason::BLIND_SPOT},
46+
{PlanningBehavior::ROUTE_OBSTACLE, StopReason::OBSTACLE_STOP},
47+
{PlanningBehavior::SIDEWALK, StopReason::WALKWAY},
48+
{PlanningBehavior::START_PLANNER, StopReason::START_PLANNER},
49+
{PlanningBehavior::STOP_SIGN, StopReason::STOP_LINE},
50+
{PlanningBehavior::SURROUNDING_OBSTACLE, StopReason::SURROUND_OBSTACLE_CHECK},
51+
{PlanningBehavior::TRAFFIC_SIGNAL, StopReason::TRAFFIC_LIGHT},
52+
{PlanningBehavior::USER_DEFINED_DETECTION_AREA, StopReason::DETECTION_AREA},
53+
{PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT, StopReason::VIRTUAL_TRAFFIC_LIGHT},
54+
{PlanningBehavior::RUN_OUT, StopReason::OBSTACLE_STOP},
55+
{PlanningBehavior::ADAPTIVE_CRUISE, "AdaptiveCruise"}};
56+
57+
class AutowareIvVelocityFactorConverter
58+
{
59+
public:
60+
AutowareIvVelocityFactorConverter(rclcpp::Node & node, const double thresh_dist_to_stop_pose);
61+
62+
tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray(
63+
const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr);
64+
65+
private:
66+
tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr convert(
67+
const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr);
68+
69+
tier4_planning_msgs::msg::StopFactor convert(
70+
const autoware_adapi_v1_msgs::msg::VelocityFactor & factor);
71+
72+
rclcpp::Logger logger_;
73+
74+
rclcpp::Clock::SharedPtr clock_;
75+
76+
double thresh_dist_to_stop_pose_;
77+
};
78+
79+
} // namespace autoware_api
80+
81+
#endif // AWAPI_AWIV_ADAPTER__AWAPI_VELOCITY_FACTOR_CONVERTER_HPP_

‎awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
<arg name="input_gate_mode" default="/control/current_gate_mode"/>
1919
<arg name="input_mrm_state" default="/system/fail_safe/mrm_state"/>
2020
<arg name="input_hazard_status" default="/system/emergency/hazard_status"/>
21-
<arg name="input_stop_reason" default="/planning/scenario_planning/status/stop_reasons"/>
21+
<arg name="input_velocity_factors" default="/api/planning/velocity_factors"/>
2222
<arg name="input_v2x_command" default="/planning/scenario_planning/status/infrastructure_commands"/>
2323
<arg name="input_v2x_state" default="/system/v2x/virtual_traffic_light_states"/>
2424
<arg name="input_diagnostics" default="/diagnostics_agg"/>
@@ -112,7 +112,7 @@
112112
<remap from="input/gate_mode" to="$(var input_gate_mode)"/>
113113
<remap from="input/mrm_state" to="$(var input_mrm_state)"/>
114114
<remap from="input/hazard_status" to="$(var input_hazard_status)"/>
115-
<remap from="input/stop_reason" to="$(var input_stop_reason)"/>
115+
<remap from="input/velocity_factors" to="$(var input_velocity_factors)"/>
116116
<remap from="input/v2x_command" to="$(var input_v2x_command)"/>
117117
<remap from="input/v2x_state" to="$(var input_v2x_state)"/>
118118
<remap from="input/diagnostics" to="$(var input_diagnostics)"/>

‎awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp

+9-5
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,8 @@ AutowareIvAdapter::AutowareIvAdapter()
4141
autoware_state_publisher_ = std::make_unique<AutowareIvAutowareStatePublisher>(*this);
4242
stop_reason_aggregator_ = std::make_unique<AutowareIvStopReasonAggregator>(
4343
*this, stop_reason_timeout_, stop_reason_thresh_dist_);
44+
velocity_factor_converter_ =
45+
std::make_unique<AutowareIvVelocityFactorConverter>(*this, stop_reason_thresh_dist_);
4446
v2x_aggregator_ = std::make_unique<AutowareIvV2XAggregator>(*this);
4547
lane_change_state_publisher_ = std::make_unique<AutowareIvLaneChangeStatePublisher>(*this);
4648
obstacle_avoidance_state_publisher_ =
@@ -85,8 +87,10 @@ AutowareIvAdapter::AutowareIvAdapter()
8587
"input/mrm_state", 1, std::bind(&AutowareIvAdapter::callbackMrmState, this, _1));
8688
sub_hazard_status_ = this->create_subscription<autoware_system_msgs::msg::HazardStatusStamped>(
8789
"input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1));
88-
sub_stop_reason_ = this->create_subscription<tier4_planning_msgs::msg::StopReasonArray>(
89-
"input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1));
90+
sub_velocity_factor_ =
91+
this->create_subscription<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
92+
"input/velocity_factors", 100,
93+
std::bind(&AutowareIvAdapter::callbackVelocityFactor, this, _1));
9094
sub_v2x_command_ = this->create_subscription<tier4_v2x_msgs::msg::InfrastructureCommandArray>(
9195
"input/v2x_command", 100, std::bind(&AutowareIvAdapter::callbackV2XCommand, this, _1));
9296
sub_v2x_state_ = this->create_subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>(
@@ -257,10 +261,10 @@ void AutowareIvAdapter::callbackHazardStatus(
257261
aw_info_.hazard_status_ptr = msg_ptr;
258262
}
259263

260-
void AutowareIvAdapter::callbackStopReason(
261-
const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr)
264+
void AutowareIvAdapter::callbackVelocityFactor(
265+
const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr msg_ptr)
262266
{
263-
aw_info_.stop_reason_ptr = stop_reason_aggregator_->updateStopReasonArray(msg_ptr, aw_info_);
267+
aw_info_.stop_reason_ptr = velocity_factor_converter_->updateStopReasonArray(msg_ptr);
264268
}
265269

266270
void AutowareIvAdapter::callbackV2XCommand(

‎awapi_awiv_adapter/src/awapi_velocity_factor_converter.cpp

+79
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
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 "awapi_awiv_adapter/awapi_velocity_factor_converter.hpp"
16+
17+
#include <memory>
18+
#include <vector>
19+
20+
namespace autoware_api
21+
{
22+
AutowareIvVelocityFactorConverter::AutowareIvVelocityFactorConverter(
23+
rclcpp::Node & node, const double thresh_dist_to_stop_pose)
24+
: logger_(node.get_logger().get_child("awapi_awiv_velocity_factor_converter")),
25+
clock_(node.get_clock()),
26+
thresh_dist_to_stop_pose_(thresh_dist_to_stop_pose)
27+
{
28+
}
29+
30+
tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr
31+
AutowareIvVelocityFactorConverter::updateStopReasonArray(
32+
const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr)
33+
{
34+
return convert(msg_ptr);
35+
}
36+
37+
tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr
38+
AutowareIvVelocityFactorConverter::convert(
39+
const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr)
40+
{
41+
tier4_planning_msgs::msg::StopReasonArray stop_reason_array_msg;
42+
// input header
43+
stop_reason_array_msg.header.frame_id = "map";
44+
stop_reason_array_msg.header.stamp = clock_->now();
45+
46+
// input stop reason
47+
for (const auto & factor : msg_ptr->factors) {
48+
// stop reason doesn't has corresponding factor.
49+
if (conversion_map.count(factor.behavior) == 0) {
50+
continue;
51+
}
52+
53+
// append only near velocity factor.
54+
if (factor.distance > thresh_dist_to_stop_pose_) {
55+
continue;
56+
}
57+
58+
// TODO(satoshi-ota): it's better to check if it is stop factor.
59+
tier4_planning_msgs::msg::StopReason near_stop_reason;
60+
near_stop_reason.reason = conversion_map.at(factor.behavior);
61+
near_stop_reason.stop_factors.push_back(convert(factor));
62+
63+
stop_reason_array_msg.stop_reasons.push_back(near_stop_reason);
64+
}
65+
66+
return std::make_shared<tier4_planning_msgs::msg::StopReasonArray>(stop_reason_array_msg);
67+
}
68+
69+
tier4_planning_msgs::msg::StopFactor AutowareIvVelocityFactorConverter::convert(
70+
const autoware_adapi_v1_msgs::msg::VelocityFactor & factor)
71+
{
72+
tier4_planning_msgs::msg::StopFactor stop_factor;
73+
stop_factor.stop_pose = factor.pose;
74+
stop_factor.dist_to_stop_pose = factor.distance;
75+
76+
return stop_factor;
77+
}
78+
79+
} // namespace autoware_api

0 commit comments

Comments
 (0)