18
18
19
19
#include < memory>
20
20
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
+
21
48
namespace default_ad_api
22
49
{
23
50
@@ -30,20 +57,25 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing",
30
57
adaptor.init_sub (sub_state_, this , &RoutingNode::on_state);
31
58
adaptor.init_sub (sub_route_, this , &RoutingNode::on_route);
32
59
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_);
33
62
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 );
38
67
39
68
adaptor.init_cli (cli_operation_mode_, group_cli_);
40
69
adaptor.init_sub (sub_operation_mode_, this , &RoutingNode::on_operation_mode);
70
+
71
+ is_auto_mode_ = false ;
72
+ state_.state = State::Message::UNKNOWN;
41
73
}
42
74
43
75
void RoutingNode::change_stop_mode ()
44
76
{
45
77
using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request;
46
- if (is_auto_mode ) {
78
+ if (is_auto_mode_ ) {
47
79
const auto req = std::make_shared<OperationModeRequest>();
48
80
req->mode = OperationModeRequest::STOP;
49
81
cli_operation_mode_->async_send_request (req);
@@ -52,12 +84,19 @@ void RoutingNode::change_stop_mode()
52
84
53
85
void RoutingNode::on_operation_mode (const OperationModeState::Message::ConstSharedPtr msg)
54
86
{
55
- is_auto_mode = msg->mode == OperationModeState::Message::AUTONOMOUS;
87
+ is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS;
56
88
}
57
89
58
90
void RoutingNode::on_state (const State::Message::ConstSharedPtr msg)
59
91
{
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));
61
100
62
101
// Change operation mode to stop when the vehicle arrives.
63
102
if (msg->state == State::Message::ARRIVED) {
@@ -80,7 +119,51 @@ void RoutingNode::on_clear_route(
80
119
const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res)
81
120
{
82
121
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);
84
167
}
85
168
86
169
} // namespace default_ad_api
0 commit comments