-
Notifications
You must be signed in to change notification settings - Fork 27
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(awapi): add velocity factor converter (#129)
* feat(awapi): add velocity factor converter Signed-off-by: satoshi-ota <[email protected]> * fix: use velocity factor converter Signed-off-by: satoshi-ota <[email protected]> * fix: remove unused subscriber Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
- Loading branch information
1 parent
cf7ebc3
commit 6013a40
Showing
6 changed files
with
178 additions
and
8 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
81 changes: 81 additions & 0 deletions
81
awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_velocity_factor_converter.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,81 @@ | ||
| // Copyright 2024 TIER IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef AWAPI_AWIV_ADAPTER__AWAPI_VELOCITY_FACTOR_CONVERTER_HPP_ | ||
| #define AWAPI_AWIV_ADAPTER__AWAPI_VELOCITY_FACTOR_CONVERTER_HPP_ | ||
|
|
||
| #include "awapi_awiv_adapter/awapi_autoware_util.hpp" | ||
|
|
||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp> | ||
| #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp> | ||
| #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp> | ||
|
|
||
| #include <map> | ||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| namespace autoware_api | ||
| { | ||
|
|
||
| using autoware_adapi_v1_msgs::msg::PlanningBehavior; | ||
| using tier4_planning_msgs::msg::StopReason; | ||
|
|
||
| const std::map<std::string, std::string> conversion_map = { | ||
| {PlanningBehavior::AVOIDANCE, StopReason::AVOIDANCE}, | ||
| {PlanningBehavior::CROSSWALK, StopReason::CROSSWALK}, | ||
| {PlanningBehavior::GOAL_PLANNER, StopReason::GOAL_PLANNER}, | ||
| {PlanningBehavior::INTERSECTION, StopReason::INTERSECTION}, | ||
| {PlanningBehavior::LANE_CHANGE, StopReason::LANE_CHANGE}, | ||
| {PlanningBehavior::MERGE, StopReason::MERGE_FROM_PRIVATE_ROAD}, | ||
| {PlanningBehavior::NO_DRIVABLE_LANE, StopReason::NO_DRIVABLE_LANE}, | ||
| {PlanningBehavior::NO_STOPPING_AREA, StopReason::NO_STOPPING_AREA}, | ||
| {PlanningBehavior::REAR_CHECK, StopReason::BLIND_SPOT}, | ||
| {PlanningBehavior::ROUTE_OBSTACLE, StopReason::OBSTACLE_STOP}, | ||
| {PlanningBehavior::SIDEWALK, StopReason::WALKWAY}, | ||
| {PlanningBehavior::START_PLANNER, StopReason::START_PLANNER}, | ||
| {PlanningBehavior::STOP_SIGN, StopReason::STOP_LINE}, | ||
| {PlanningBehavior::SURROUNDING_OBSTACLE, StopReason::SURROUND_OBSTACLE_CHECK}, | ||
| {PlanningBehavior::TRAFFIC_SIGNAL, StopReason::TRAFFIC_LIGHT}, | ||
| {PlanningBehavior::USER_DEFINED_DETECTION_AREA, StopReason::DETECTION_AREA}, | ||
| {PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT, StopReason::VIRTUAL_TRAFFIC_LIGHT}, | ||
| {PlanningBehavior::RUN_OUT, StopReason::OBSTACLE_STOP}, | ||
| {PlanningBehavior::ADAPTIVE_CRUISE, "AdaptiveCruise"}}; | ||
|
|
||
| class AutowareIvVelocityFactorConverter | ||
| { | ||
| public: | ||
| AutowareIvVelocityFactorConverter(rclcpp::Node & node, const double thresh_dist_to_stop_pose); | ||
|
|
||
| tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray( | ||
| const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr); | ||
|
|
||
| private: | ||
| tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr convert( | ||
| const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr); | ||
|
|
||
| tier4_planning_msgs::msg::StopFactor convert( | ||
| const autoware_adapi_v1_msgs::msg::VelocityFactor & factor); | ||
|
|
||
| rclcpp::Logger logger_; | ||
|
|
||
| rclcpp::Clock::SharedPtr clock_; | ||
|
|
||
| double thresh_dist_to_stop_pose_; | ||
| }; | ||
|
|
||
| } // namespace autoware_api | ||
|
|
||
| #endif // AWAPI_AWIV_ADAPTER__AWAPI_VELOCITY_FACTOR_CONVERTER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
79 changes: 79 additions & 0 deletions
79
awapi_awiv_adapter/src/awapi_velocity_factor_converter.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,79 @@ | ||
| // Copyright 2024 TIER IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #include "awapi_awiv_adapter/awapi_velocity_factor_converter.hpp" | ||
|
|
||
| #include <memory> | ||
| #include <vector> | ||
|
|
||
| namespace autoware_api | ||
| { | ||
| AutowareIvVelocityFactorConverter::AutowareIvVelocityFactorConverter( | ||
| rclcpp::Node & node, const double thresh_dist_to_stop_pose) | ||
| : logger_(node.get_logger().get_child("awapi_awiv_velocity_factor_converter")), | ||
| clock_(node.get_clock()), | ||
| thresh_dist_to_stop_pose_(thresh_dist_to_stop_pose) | ||
| { | ||
| } | ||
|
|
||
| tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr | ||
| AutowareIvVelocityFactorConverter::updateStopReasonArray( | ||
| const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr) | ||
| { | ||
| return convert(msg_ptr); | ||
| } | ||
|
|
||
| tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr | ||
| AutowareIvVelocityFactorConverter::convert( | ||
| const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr) | ||
| { | ||
| tier4_planning_msgs::msg::StopReasonArray stop_reason_array_msg; | ||
| // input header | ||
| stop_reason_array_msg.header.frame_id = "map"; | ||
| stop_reason_array_msg.header.stamp = clock_->now(); | ||
|
|
||
| // input stop reason | ||
| for (const auto & factor : msg_ptr->factors) { | ||
| // stop reason doesn't has corresponding factor. | ||
| if (conversion_map.count(factor.behavior) == 0) { | ||
| continue; | ||
| } | ||
|
|
||
| // append only near velocity factor. | ||
| if (factor.distance > thresh_dist_to_stop_pose_) { | ||
| continue; | ||
| } | ||
|
|
||
| // TODO(satoshi-ota): it's better to check if it is stop factor. | ||
| tier4_planning_msgs::msg::StopReason near_stop_reason; | ||
| near_stop_reason.reason = conversion_map.at(factor.behavior); | ||
| near_stop_reason.stop_factors.push_back(convert(factor)); | ||
|
|
||
| stop_reason_array_msg.stop_reasons.push_back(near_stop_reason); | ||
| } | ||
|
|
||
| return std::make_shared<tier4_planning_msgs::msg::StopReasonArray>(stop_reason_array_msg); | ||
| } | ||
|
|
||
| tier4_planning_msgs::msg::StopFactor AutowareIvVelocityFactorConverter::convert( | ||
| const autoware_adapi_v1_msgs::msg::VelocityFactor & factor) | ||
| { | ||
| tier4_planning_msgs::msg::StopFactor stop_factor; | ||
| stop_factor.stop_pose = factor.pose; | ||
| stop_factor.dist_to_stop_pose = factor.distance; | ||
|
|
||
| return stop_factor; | ||
| } | ||
|
|
||
| } // namespace autoware_api |