Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(default_ad_api): apply route_selector #6364

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 1 addition & 22 deletions common/component_interface_specs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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()
Original file line number Diff line number Diff line change
Expand Up @@ -17,77 +17,48 @@

#include <rclcpp/qos.hpp>

#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
#include <autoware_adapi_v1_msgs/srv/set_route.hpp>
#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
#include <tier4_planning_msgs/msg/route_state.hpp>
#include <tier4_planning_msgs/srv/clear_route.hpp>
#include <tier4_planning_msgs/srv/set_lanelet_route.hpp>
#include <tier4_planning_msgs/srv/set_waypoint_route.hpp>

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;
Expand Down
10 changes: 3 additions & 7 deletions common/component_interface_specs/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,8 @@
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>ament_cmake_core</buildtool_depend>
<buildtool_depend>ament_cmake_export_dependencies</buildtool_depend>
<buildtool_depend>ament_cmake_test</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<buildtool_export_depend>ament_cmake_core</buildtool_export_depend>
<buildtool_export_depend>ament_cmake_test</buildtool_export_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
Expand All @@ -28,9 +22,11 @@
<depend>rosidl_runtime_cpp</depend>
<depend>tier4_control_msgs</depend>
<depend>tier4_map_msgs</depend>
<depend>tier4_planning_msgs</depend>
<depend>tier4_system_msgs</depend>
<depend>tier4_vehicle_msgs</depend>
<test_depend>ament_cmake_ros</test_depend>

<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

Expand Down
22 changes: 2 additions & 20 deletions common/component_interface_specs/test/test_planning.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
99 changes: 91 additions & 8 deletions system/default_ad_api/src/routing.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,33 @@

#include <memory>

namespace
{

using autoware_adapi_v1_msgs::msg::ResponseStatus;

template <class InterfaceT>
ResponseStatus route_already_set()

Check warning on line 27 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L27

Added line #L27 was not covered by tests
{
ResponseStatus status;
status.success = false;
status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE;

Check warning on line 31 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L29-L31

Added lines #L29 - L31 were not covered by tests
status.message = "The route is already set.";
return status;

Check warning on line 33 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L33

Added line #L33 was not covered by tests
}

template <class InterfaceT>
ResponseStatus route_is_not_set()

Check warning on line 37 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L37

Added line #L37 was not covered by tests
{
ResponseStatus status;
status.success = false;
status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE;

Check warning on line 41 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L39-L41

Added lines #L39 - L41 were not covered by tests
status.message = "The route is not set yet.";
return status;

Check warning on line 43 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L43

Added line #L43 was not covered by tests
}

} // namespace

namespace default_ad_api
{

Expand All @@ -30,20 +57,25 @@
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<OperationModeRequest>();
req->mode = OperationModeRequest::STOP;
cli_operation_mode_->async_send_request(req);
Expand All @@ -52,12 +84,19 @@

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;

Check warning on line 87 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L87

Added line #L87 was not covered by tests
}

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;

Check warning on line 98 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L98

Added line #L98 was not covered by tests
pub_state_->publish(conversion::convert_state(*msg));

// Change operation mode to stop when the vehicle arrives.
if (msg->state == State::Message::ARRIVED) {
Expand All @@ -80,7 +119,51 @@
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);
}

Check warning on line 123 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L122-L123

Added lines #L122 - L123 were not covered by tests

void RoutingNode::on_set_route_points(

Check warning on line 125 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L125

Added line #L125 was not covered by tests
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<autoware_ad_api::routing::SetRoutePoints>();
return;

Check warning on line 131 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L130-L131

Added lines #L130 - L131 were not covered by tests
}
res->status = conversion::convert_call(cli_set_waypoint_route_, req);

Check warning on line 133 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L133

Added line #L133 was not covered by tests
}

void RoutingNode::on_set_route(

Check warning on line 136 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L136

Added line #L136 was not covered by tests
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<autoware_ad_api::routing::SetRoute>();
return;

Check warning on line 142 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L141-L142

Added lines #L141 - L142 were not covered by tests
}
res->status = conversion::convert_call(cli_set_lanelet_route_, req);

Check warning on line 144 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L144

Added line #L144 was not covered by tests
}

void RoutingNode::on_change_route_points(

Check warning on line 147 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L147

Added line #L147 was not covered by tests
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<autoware_ad_api::routing::SetRoutePoints>();
return;

Check warning on line 153 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L152-L153

Added lines #L152 - L153 were not covered by tests
}
res->status = conversion::convert_call(cli_set_waypoint_route_, req);

Check warning on line 155 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L155

Added line #L155 was not covered by tests
}

void RoutingNode::on_change_route(

Check warning on line 158 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L158

Added line #L158 was not covered by tests
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<autoware_ad_api::routing::SetRoute>();
return;

Check warning on line 164 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L163-L164

Added lines #L163 - L164 were not covered by tests
}
res->status = conversion::convert_call(cli_set_lanelet_route_, req);

Check warning on line 166 in system/default_ad_api/src/routing.cpp

View check run for this annotation

Codecov / codecov/patch

system/default_ad_api/src/routing.cpp#L166

Added line #L166 was not covered by tests
}

} // namespace default_ad_api
Expand Down
30 changes: 21 additions & 9 deletions system/default_ad_api/src/routing.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<autoware_ad_api::routing::RouteState> pub_state_;
Pub<autoware_ad_api::routing::Route> pub_route_;
Expand All @@ -42,26 +46,34 @@ class RoutingNode : public rclcpp::Node
Srv<autoware_ad_api::routing::ChangeRoute> srv_change_route_;
Srv<autoware_ad_api::routing::ClearRoute> srv_clear_route_;
Sub<planning_interface::RouteState> sub_state_;
Sub<planning_interface::Route> sub_route_;
Cli<planning_interface::SetRoutePoints> cli_set_route_points_;
Cli<planning_interface::SetRoute> cli_set_route_;
Cli<planning_interface::ChangeRoutePoints> cli_change_route_points_;
Cli<planning_interface::ChangeRoute> cli_change_route_;
Sub<planning_interface::LaneletRoute> sub_route_;
Cli<planning_interface::SetWaypointRoute> cli_set_waypoint_route_;
Cli<planning_interface::SetLaneletRoute> cli_set_lanelet_route_;
Cli<planning_interface::ClearRoute> cli_clear_route_;
Cli<system_interface::ChangeOperationMode> cli_operation_mode_;
Sub<system_interface::OperationModeState> 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);
void on_route(const Route::Message::ConstSharedPtr msg);
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
Expand Down
Loading
Loading