1818
1919#include < memory>
2020
21+ namespace
22+ {
23+
24+ using autoware_adapi_v1_msgs::msg::ResponseStatus;
25+
26+ template <class InterfaceT >
27+ ResponseStatus route_already_set ()
28+ {
29+ ResponseStatus status;
30+ status.success = false ;
31+ status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE;
32+ status.message = " The route is already set."
33+ return status;
34+ }
35+
36+ template <class InterfaceT >
37+ ResponseStatus route_is_not_set ()
38+ {
39+ ResponseStatus status;
40+ status.success = false ;
41+ status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE;
42+ status.message = " The route is not set yet."
43+ return status;
44+ }
45+
46+ } // namespace
47+
2148namespace default_ad_api
2249{
2350
@@ -30,20 +57,25 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing",
3057 adaptor.init_sub (sub_state_, this , &RoutingNode::on_state);
3158 adaptor.init_sub (sub_route_, this , &RoutingNode::on_route);
3259 adaptor.init_cli (cli_clear_route_, group_cli_);
60+ adaptor.init_cli (cli_set_waypoint_route_, group_cli_);
61+ adaptor.init_cli (cli_set_lanelet_route_, group_cli_);
3362 adaptor.init_srv (srv_clear_route_, this , &RoutingNode::on_clear_route);
34- adaptor.relay_service (cli_set_route_, srv_set_route_, group_cli_ );
35- adaptor.relay_service (cli_set_route_points_, srv_set_route_points_, group_cli_ );
36- adaptor.relay_service (cli_change_route_, srv_change_route_, group_cli_ );
37- adaptor.relay_service (cli_change_route_points_, srv_change_route_points_, group_cli_ );
63+ adaptor.init_srv (srv_set_route_, this , &RoutingNode::on_set_route );
64+ adaptor.init_srv (srv_set_route_points_, this , &RoutingNode::on_set_route_points );
65+ adaptor.init_srv (srv_change_route_, this , &RoutingNode::on_change_route );
66+ adaptor.init_srv (srv_change_route_points_, this , &RoutingNode::on_change_route_points );
3867
3968 adaptor.init_cli (cli_operation_mode_, group_cli_);
4069 adaptor.init_sub (sub_operation_mode_, this , &RoutingNode::on_operation_mode);
70+
71+ is_auto_mode_ = false ;
72+ state_.state = State::Message::UNKNOWN;
4173}
4274
4375void RoutingNode::change_stop_mode ()
4476{
4577 using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request;
46- if (is_auto_mode ) {
78+ if (is_auto_mode_ ) {
4779 const auto req = std::make_shared<OperationModeRequest>();
4880 req->mode = OperationModeRequest::STOP;
4981 cli_operation_mode_->async_send_request (req);
@@ -52,12 +84,17 @@ void RoutingNode::change_stop_mode()
5284
5385void RoutingNode::on_operation_mode (const OperationModeState::Message::ConstSharedPtr msg)
5486{
55- is_auto_mode = msg->mode == OperationModeState::Message::AUTONOMOUS;
87+ is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS;
5688}
5789
5890void RoutingNode::on_state (const State::Message::ConstSharedPtr msg)
5991{
60- pub_state_->publish (*msg);
92+ state_ = *msg;
93+
94+ // TODO(Takagi, Isamu): Add adapi initializing state.
95+ if (State::Message::INITIALIZING) {
96+ pub_state_->publish (conversion::convert_state (*msg));
97+ }
6198
6299 // Change operation mode to stop when the vehicle arrives.
63100 if (msg->state == State::Message::ARRIVED) {
@@ -80,7 +117,51 @@ void RoutingNode::on_clear_route(
80117 const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res)
81118{
82119 change_stop_mode ();
83- *res = *cli_clear_route_->call (req);
120+ res->status = conversion::convert_call (cli_clear_route_, req);
121+ }
122+
123+ void RoutingNode::on_set_route_points (
124+ const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req,
125+ const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res)
126+ {
127+ if (state_.state != State::Message::UNSET) {
128+ res->
status = route_already_set<autoware_ad_api::routing::SetRoutePoints>();
129+ return ;
130+ }
131+ res->status = conversion::convert_call (cli_set_waypoint_route_, req);
132+ }
133+
134+ void RoutingNode::on_set_route (
135+ const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req,
136+ const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res)
137+ {
138+ if (state_.state != State::Message::UNSET) {
139+ res->
status = route_already_set<autoware_ad_api::routing::SetRoute>();
140+ return ;
141+ }
142+ res->status = conversion::convert_call (cli_set_lanelet_route_, req);
143+ }
144+
145+ void RoutingNode::on_change_route_points (
146+ const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req,
147+ const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res)
148+ {
149+ if (state_.state != State::Message::SET) {
150+ res->
status = route_is_not_set<autoware_ad_api::routing::SetRoutePoints>();
151+ return ;
152+ }
153+ res->status = conversion::convert_call (cli_set_waypoint_route_, req);
154+ }
155+
156+ void RoutingNode::on_change_route (
157+ const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req,
158+ const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res)
159+ {
160+ if (state_.state != State::Message::SET) {
161+ res->
status = route_is_not_set<autoware_ad_api::routing::SetRoute>();
162+ return ;
163+ }
164+ res->status = conversion::convert_call (cli_set_lanelet_route_, req);
84165}
85166
86167} // namespace default_ad_api
0 commit comments