From c2555b1227016e60a585e9c01667598ab6f9e2c6 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 8 Feb 2024 21:49:28 +0900 Subject: [PATCH 1/3] feat(default_ad_api): apply route_selector Signed-off-by: Takagi, Isamu --- .../component_interface_specs/CMakeLists.txt | 23 +---- .../component_interface_specs/planning.hpp | 63 ++++-------- common/component_interface_specs/package.xml | 10 +- .../test/test_planning.cpp | 22 +---- system/default_ad_api/src/routing.cpp | 97 +++++++++++++++++-- system/default_ad_api/src/routing.hpp | 30 ++++-- .../src/utils/route_conversion.cpp | 82 ++++++++++++++++ .../src/utils/route_conversion.hpp | 34 ++++++- 8 files changed, 248 insertions(+), 113 deletions(-) diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt index b519a0ddce86a..146f3688513cd 100644 --- a/common/component_interface_specs/CMakeLists.txt +++ b/common/component_interface_specs/CMakeLists.txt @@ -4,26 +4,8 @@ project(component_interface_specs) find_package(autoware_cmake REQUIRED) autoware_package() -include_directories( - include - SYSTEM - ${rclcpp_INCLUDE_DIRS} - ${rosidl_runtime_cpp_INCLUDE_DIRS} - ${rcl_INCLUDE_DIRS} - ${autoware_adapi_v1_msgs_INCLUDE_DIRS} - ${autoware_auto_planning_msgs_INCLUDE_DIRS} - ${autoware_planning_msgs_INCLUDE_DIRS} - ${autoware_auto_vehicle_msgs_INCLUDE_DIRS} - ${tier4_control_msgs_INCLUDE_DIRS} - ${nav_msgs_INCLUDE_DIRS} - ${tier4_system_msgs_INCLUDE_DIRS} - ${tier4_vehicle_msgs_INCLUDE_DIRS} - ${autoware_auto_perception_msgs_INCLUDE_DIRS} - ${tier4_map_msgs_INCLUDE_DIRS} -) - if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_component_interface_specs + ament_auto_add_gtest(gtest_${PROJECT_NAME} test/gtest_main.cpp test/test_planning.cpp test/test_control.cpp @@ -33,9 +15,6 @@ if(BUILD_TESTING) test/test_perception.cpp test/test_vehicle.cpp ) - target_include_directories(test_component_interface_specs - PRIVATE include - ) endif() ament_auto_package() diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index 567ea9d69f2a7..9efd8c871f68f 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -17,77 +17,48 @@ #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include namespace planning_interface { -struct SetRoutePoints +struct SetLaneletRoute { - using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; - static constexpr char name[] = "/planning/mission_planning/set_route_points"; + using Service = tier4_planning_msgs::srv::SetLaneletRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/set_lanelet_route"; }; -struct SetRoute +struct SetWaypointRoute { - using Service = autoware_adapi_v1_msgs::srv::SetRoute; - static constexpr char name[] = "/planning/mission_planning/set_route"; -}; - -struct ChangeRoutePoints -{ - using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; - static constexpr char name[] = "/planning/mission_planning/change_route_points"; -}; - -struct ChangeRoute -{ - using Service = autoware_adapi_v1_msgs::srv::SetRoute; - static constexpr char name[] = "/planning/mission_planning/change_route"; + using Service = tier4_planning_msgs::srv::SetWaypointRoute; + static constexpr char name[] = + "/planning/mission_planning/route_selector/main/set_waypoint_route"; }; struct ClearRoute { - using Service = autoware_adapi_v1_msgs::srv::ClearRoute; - static constexpr char name[] = "/planning/mission_planning/clear_route"; + using Service = tier4_planning_msgs::srv::ClearRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/clear_route"; }; struct RouteState { - using Message = autoware_adapi_v1_msgs::msg::RouteState; - static constexpr char name[] = "/planning/mission_planning/route_state"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; -}; - -struct Route -{ - using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/route"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; -}; - -struct NormalRoute -{ - using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/normal_route"; + using Message = tier4_planning_msgs::msg::RouteState; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/state"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; -struct MrmRoute +struct LaneletRoute { using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/mrm_route"; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/route"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 67279d0ae2b7f..1ad5f410a814a 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -9,14 +9,8 @@ Apache License 2.0 ament_cmake_auto - ament_cmake_core - ament_cmake_export_dependencies - ament_cmake_test autoware_cmake - ament_cmake_core - ament_cmake_test - autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs @@ -28,9 +22,11 @@ rosidl_runtime_cpp tier4_control_msgs tier4_map_msgs + tier4_planning_msgs tier4_system_msgs tier4_vehicle_msgs - ament_cmake_ros + + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/common/component_interface_specs/test/test_planning.cpp b/common/component_interface_specs/test/test_planning.cpp index 8c504cb119854..c9cf353de5f34 100644 --- a/common/component_interface_specs/test/test_planning.cpp +++ b/common/component_interface_specs/test/test_planning.cpp @@ -27,26 +27,8 @@ TEST(planning, interface) } { - using planning_interface::Route; - Route route; - size_t depth = 1; - EXPECT_EQ(route.depth, depth); - EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); - EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - } - - { - using planning_interface::NormalRoute; - NormalRoute route; - size_t depth = 1; - EXPECT_EQ(route.depth, depth); - EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); - EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - } - - { - using planning_interface::MrmRoute; - MrmRoute route; + using planning_interface::LaneletRoute; + LaneletRoute route; size_t depth = 1; EXPECT_EQ(route.depth, depth); EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index 5204146201c8a..d08f74d172bd3 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -18,6 +18,33 @@ #include +namespace +{ + +using autoware_adapi_v1_msgs::msg::ResponseStatus; + +template +ResponseStatus route_already_set() +{ + ResponseStatus status; + status.success = false; + status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE; + status.message = "The route is already set."; + return status; +} + +template +ResponseStatus route_is_not_set() +{ + ResponseStatus status; + status.success = false; + status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE; + status.message = "The route is not set yet."; + return status; +} + +} // namespace + namespace default_ad_api { @@ -30,20 +57,25 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_sub(sub_state_, this, &RoutingNode::on_state); adaptor.init_sub(sub_route_, this, &RoutingNode::on_route); adaptor.init_cli(cli_clear_route_, group_cli_); + adaptor.init_cli(cli_set_waypoint_route_, group_cli_); + adaptor.init_cli(cli_set_lanelet_route_, group_cli_); adaptor.init_srv(srv_clear_route_, this, &RoutingNode::on_clear_route); - adaptor.relay_service(cli_set_route_, srv_set_route_, group_cli_); - adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_); - adaptor.relay_service(cli_change_route_, srv_change_route_, group_cli_); - adaptor.relay_service(cli_change_route_points_, srv_change_route_points_, group_cli_); + adaptor.init_srv(srv_set_route_, this, &RoutingNode::on_set_route); + adaptor.init_srv(srv_set_route_points_, this, &RoutingNode::on_set_route_points); + adaptor.init_srv(srv_change_route_, this, &RoutingNode::on_change_route); + adaptor.init_srv(srv_change_route_points_, this, &RoutingNode::on_change_route_points); adaptor.init_cli(cli_operation_mode_, group_cli_); adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); + + is_auto_mode_ = false; + state_.state = State::Message::UNKNOWN; } void RoutingNode::change_stop_mode() { using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; - if (is_auto_mode) { + if (is_auto_mode_) { const auto req = std::make_shared(); req->mode = OperationModeRequest::STOP; cli_operation_mode_->async_send_request(req); @@ -52,12 +84,17 @@ void RoutingNode::change_stop_mode() void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg) { - is_auto_mode = msg->mode == OperationModeState::Message::AUTONOMOUS; + is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS; } void RoutingNode::on_state(const State::Message::ConstSharedPtr msg) { - pub_state_->publish(*msg); + state_ = *msg; + + // TODO(Takagi, Isamu): Add adapi initializing state. + if (State::Message::INITIALIZING) { + pub_state_->publish(conversion::convert_state(*msg)); + } // Change operation mode to stop when the vehicle arrives. if (msg->state == State::Message::ARRIVED) { @@ -80,7 +117,51 @@ void RoutingNode::on_clear_route( const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res) { change_stop_mode(); - *res = *cli_clear_route_->call(req); + res->status = conversion::convert_call(cli_clear_route_, req); +} + +void RoutingNode::on_set_route_points( + const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res) +{ + if (state_.state != State::Message::UNSET) { + res->status = route_already_set(); + return; + } + res->status = conversion::convert_call(cli_set_waypoint_route_, req); +} + +void RoutingNode::on_set_route( + const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res) +{ + if (state_.state != State::Message::UNSET) { + res->status = route_already_set(); + return; + } + res->status = conversion::convert_call(cli_set_lanelet_route_, req); +} + +void RoutingNode::on_change_route_points( + const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res) +{ + if (state_.state != State::Message::SET) { + res->status = route_is_not_set(); + return; + } + res->status = conversion::convert_call(cli_set_waypoint_route_, req); +} + +void RoutingNode::on_change_route( + const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res) +{ + if (state_.state != State::Message::SET) { + res->status = route_is_not_set(); + return; + } + res->status = conversion::convert_call(cli_set_lanelet_route_, req); } } // namespace default_ad_api diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp index 7c15f6d64608f..406569aa0a36b 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/default_ad_api/src/routing.hpp @@ -33,6 +33,10 @@ class RoutingNode : public rclcpp::Node explicit RoutingNode(const rclcpp::NodeOptions & options); private: + using OperationModeState = system_interface::OperationModeState; + using State = planning_interface::RouteState; + using Route = planning_interface::LaneletRoute; + rclcpp::CallbackGroup::SharedPtr group_cli_; Pub pub_state_; Pub pub_route_; @@ -42,19 +46,15 @@ class RoutingNode : public rclcpp::Node Srv srv_change_route_; Srv srv_clear_route_; Sub sub_state_; - Sub sub_route_; - Cli cli_set_route_points_; - Cli cli_set_route_; - Cli cli_change_route_points_; - Cli cli_change_route_; + Sub sub_route_; + Cli cli_set_waypoint_route_; + Cli cli_set_lanelet_route_; Cli cli_clear_route_; Cli cli_operation_mode_; Sub sub_operation_mode_; - bool is_auto_mode = false; + bool is_auto_mode_; + State::Message state_; - using OperationModeState = system_interface::OperationModeState; - using State = planning_interface::RouteState; - using Route = planning_interface::Route; void change_stop_mode(); void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg); void on_state(const State::Message::ConstSharedPtr msg); @@ -62,6 +62,18 @@ class RoutingNode : public rclcpp::Node void on_clear_route( const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req, const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res); + void on_set_route_points( + const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res); + void on_set_route( + const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res); + void on_change_route_points( + const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res); + void on_change_route( + const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res); }; } // namespace default_ad_api diff --git a/system/default_ad_api/src/utils/route_conversion.cpp b/system/default_ad_api/src/utils/route_conversion.cpp index 88226c1fed063..08f40f3a746b9 100644 --- a/system/default_ad_api/src/utils/route_conversion.cpp +++ b/system/default_ad_api/src/utils/route_conversion.cpp @@ -14,6 +14,7 @@ #include "route_conversion.hpp" +#include #include namespace @@ -47,6 +48,15 @@ RoutePrimitive convert(const LaneletPrimitive & in) return api; } +template <> +LaneletPrimitive convert(const RoutePrimitive & in) +{ + LaneletPrimitive out; + out.id = in.id; + out.primitive_type = in.type; + return out; +} + template <> RouteSegment convert(const LaneletSegment & in) { @@ -62,6 +72,18 @@ RouteSegment convert(const LaneletSegment & in) return api; } +template <> +LaneletSegment convert(const RouteSegment & in) +{ + LaneletSegment out; + out.preferred_primitive = convert(in.preferred); + out.primitives.push_back(out.preferred_primitive); + for (const auto & primitive : in.alternatives) { + out.primitives.push_back(convert(primitive)); + } + return out; +} + } // namespace namespace default_ad_api::conversion @@ -87,4 +109,64 @@ ExternalRoute convert_route(const InternalRoute & internal) return external; } +ExternalState convert_state(const InternalState & internal) +{ + // clang-format off + const auto convert = [](InternalState::_state_type state) { + switch(state) { + // TODO(Takagi, Isamu): Add adapi state. + case InternalState::INITIALIZING: return ExternalState::UNSET; + case InternalState::UNSET: return ExternalState::UNSET; + case InternalState::ROUTING: return ExternalState::UNSET; + case InternalState::SET: return ExternalState::SET; + case InternalState::REROUTING: return ExternalState::CHANGING; + case InternalState::ARRIVED: return ExternalState::ARRIVED; + case InternalState::ABORTED: return ExternalState::SET; + default: return ExternalState::UNKNOWN; + } + }; + // clang-format on + + ExternalState external; + external.stamp = internal.stamp; + external.state = convert(internal.state); + return external; +} + +InternalClearRequest convert_request(const ExternalClearRequest &) +{ + auto internal = std::make_shared(); + return internal; +} + +InternalLaneletRequest convert_request(const ExternalLaneletRequest & external) +{ + auto internal = std::make_shared(); + internal->header = external->header; + internal->goal_pose = external->goal; + internal->segments = convert_vector(external->segments); + internal->allow_modification = external->option.allow_goal_modification; + return internal; +} + +InternalWaypointRequest convert_request(const ExternalWaypointRequest & external) +{ + auto internal = std::make_shared(); + internal->header = external->header; + internal->goal_pose = external->goal; + internal->waypoints = external->waypoints; + internal->allow_modification = external->option.allow_goal_modification; + return internal; +} + +ExternalResponse convert_response(const InternalResponse & internal) +{ + // TODO(Takagi, Isamu): check error code correspondence + ExternalResponse external; + external.success = internal.success; + external.code = internal.code; + external.message = internal.message; + return external; +} + } // namespace default_ad_api::conversion diff --git a/system/default_ad_api/src/utils/route_conversion.hpp b/system/default_ad_api/src/utils/route_conversion.hpp index 19f4320228fa8..9a31b9e80c064 100644 --- a/system/default_ad_api/src/utils/route_conversion.hpp +++ b/system/default_ad_api/src/utils/route_conversion.hpp @@ -18,18 +18,50 @@ #include #include +#include +#include #include +#include #include +#include +#include +#include +#include namespace default_ad_api::conversion { using ExternalRoute = autoware_adapi_v1_msgs::msg::Route; using InternalRoute = autoware_planning_msgs::msg::LaneletRoute; - ExternalRoute create_empty_route(const rclcpp::Time & stamp); ExternalRoute convert_route(const InternalRoute & internal); +using ExternalState = autoware_adapi_v1_msgs::msg::RouteState; +using InternalState = tier4_planning_msgs::msg::RouteState; +ExternalState convert_state(const InternalState & internal); + +using ExternalClearRequest = autoware_adapi_v1_msgs::srv::ClearRoute::Request::SharedPtr; +using InternalClearRequest = tier4_planning_msgs::srv::ClearRoute::Request::SharedPtr; +InternalClearRequest convert_request(const ExternalClearRequest & external); + +using ExternalLaneletRequest = autoware_adapi_v1_msgs::srv::SetRoute::Request::SharedPtr; +using InternalLaneletRequest = tier4_planning_msgs::srv::SetLaneletRoute::Request::SharedPtr; +InternalLaneletRequest convert_request(const ExternalLaneletRequest & external); + +using ExternalWaypointRequest = autoware_adapi_v1_msgs::srv::SetRoutePoints::Request::SharedPtr; +using InternalWaypointRequest = tier4_planning_msgs::srv::SetWaypointRoute::Request::SharedPtr; +InternalWaypointRequest convert_request(const ExternalWaypointRequest & external); + +using ExternalResponse = autoware_adapi_v1_msgs::msg::ResponseStatus; +using InternalResponse = autoware_common_msgs::msg::ResponseStatus; +ExternalResponse convert_response(const InternalResponse & internal); + +template +ExternalResponse convert_call(ClientT & client, RequestT & req) +{ + return convert_response(client->call(convert_request(req))->status); +} + } // namespace default_ad_api::conversion #endif // UTILS__ROUTE_CONVERSION_HPP_ From 305b9c44318c4fc11ed2b52a2bb9b2d0d7686da2 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 14 Feb 2024 03:12:46 +0900 Subject: [PATCH 2/3] add interrupted state Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/utils/route_conversion.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/system/default_ad_api/src/utils/route_conversion.cpp b/system/default_ad_api/src/utils/route_conversion.cpp index 08f40f3a746b9..70951fc337b68 100644 --- a/system/default_ad_api/src/utils/route_conversion.cpp +++ b/system/default_ad_api/src/utils/route_conversion.cpp @@ -122,6 +122,7 @@ ExternalState convert_state(const InternalState & internal) case InternalState::REROUTING: return ExternalState::CHANGING; case InternalState::ARRIVED: return ExternalState::ARRIVED; case InternalState::ABORTED: return ExternalState::SET; + case InternalState::INTERRUPTED: return ExternalState::SET; default: return ExternalState::UNKNOWN; } }; From b6c37aae69c033f84f952920d51f693b1b592dbe Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 21 Feb 2024 03:47:16 +0900 Subject: [PATCH 3/3] fix initializing state handling Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/routing.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index d08f74d172bd3..0f2247c3aada8 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -89,13 +89,15 @@ void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstShar void RoutingNode::on_state(const State::Message::ConstSharedPtr msg) { - state_ = *msg; - // TODO(Takagi, Isamu): Add adapi initializing state. - if (State::Message::INITIALIZING) { - pub_state_->publish(conversion::convert_state(*msg)); + // Represent initializing state by not publishing the topic for now. + if (msg->state == State::Message::INITIALIZING) { + return; } + state_ = *msg; + pub_state_->publish(conversion::convert_state(*msg)); + // Change operation mode to stop when the vehicle arrives. if (msg->state == State::Message::ARRIVED) { change_stop_mode();