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,19 @@ 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+ // TODO(Takagi, Isamu): Add adapi initializing state.
93+ // Represent initializing state by not publishing the topic for now.
94+ if (msg->state == State::Message::INITIALIZING) {
95+ return ;
96+ }
97+
98+ state_ = *msg;
99+ pub_state_->publish (conversion::convert_state (*msg));
61100
62101 // Change operation mode to stop when the vehicle arrives.
63102 if (msg->state == State::Message::ARRIVED) {
@@ -80,7 +119,51 @@ void RoutingNode::on_clear_route(
80119 const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res)
81120{
82121 change_stop_mode ();
83- *res = *cli_clear_route_->call (req);
122+ res->status = conversion::convert_call (cli_clear_route_, req);
123+ }
124+
125+ void RoutingNode::on_set_route_points (
126+ const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req,
127+ const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res)
128+ {
129+ if (state_.state != State::Message::UNSET) {
130+ res->
status = route_already_set<autoware_ad_api::routing::SetRoutePoints>();
131+ return ;
132+ }
133+ res->status = conversion::convert_call (cli_set_waypoint_route_, req);
134+ }
135+
136+ void RoutingNode::on_set_route (
137+ const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req,
138+ const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res)
139+ {
140+ if (state_.state != State::Message::UNSET) {
141+ res->
status = route_already_set<autoware_ad_api::routing::SetRoute>();
142+ return ;
143+ }
144+ res->status = conversion::convert_call (cli_set_lanelet_route_, req);
145+ }
146+
147+ void RoutingNode::on_change_route_points (
148+ const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req,
149+ const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res)
150+ {
151+ if (state_.state != State::Message::SET) {
152+ res->
status = route_is_not_set<autoware_ad_api::routing::SetRoutePoints>();
153+ return ;
154+ }
155+ res->status = conversion::convert_call (cli_set_waypoint_route_, req);
156+ }
157+
158+ void RoutingNode::on_change_route (
159+ const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req,
160+ const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res)
161+ {
162+ if (state_.state != State::Message::SET) {
163+ res->
status = route_is_not_set<autoware_ad_api::routing::SetRoute>();
164+ return ;
165+ }
166+ res->status = conversion::convert_call (cli_set_lanelet_route_, req);
84167}
85168
86169} // namespace default_ad_api
0 commit comments