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..0f2247c3aada8 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,19 @@ 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); + // TODO(Takagi, Isamu): Add adapi initializing state. + // 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) { @@ -80,7 +119,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..70951fc337b68 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,65 @@ 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; + case InternalState::INTERRUPTED: 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_