23
23
#include " yabloc_module.hpp"
24
24
25
25
#include < memory>
26
+ #include < sstream>
26
27
#include < vector>
27
28
28
29
PoseInitializer::PoseInitializer () : Node(" pose_initializer" )
@@ -79,7 +80,7 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer")
79
80
initial_pose.orientation .z = initial_pose_array[5 ];
80
81
initial_pose.orientation .w = initial_pose_array[6 ];
81
82
82
- set_user_defined_initial_pose (initial_pose);
83
+ set_user_defined_initial_pose (initial_pose, true );
83
84
}
84
85
}
85
86
}
@@ -114,11 +115,12 @@ void PoseInitializer::change_node_trigger(bool flag, bool need_spin)
114
115
}
115
116
}
116
117
117
- void PoseInitializer::set_user_defined_initial_pose (const geometry_msgs::msg::Pose initial_pose)
118
+ void PoseInitializer::set_user_defined_initial_pose (
119
+ const geometry_msgs::msg::Pose initial_pose, bool need_spin)
118
120
{
119
121
try {
120
122
change_state (State::Message::INITIALIZING);
121
- change_node_trigger (false , true );
123
+ change_node_trigger (false , need_spin );
122
124
123
125
PoseWithCovarianceStamped pose;
124
126
pose.header .frame_id = " map" ;
@@ -127,7 +129,7 @@ void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Po
127
129
pose.pose .covariance = output_pose_covariance_;
128
130
pub_reset_->publish (pose);
129
131
130
- change_node_trigger (true , true );
132
+ change_node_trigger (true , need_spin );
131
133
change_state (State::Message::INITIALIZED);
132
134
133
135
RCLCPP_INFO (get_logger (), " Set user defined initial pose" );
@@ -147,25 +149,52 @@ void PoseInitializer::on_initialize(
147
149
Initialize::Service::Response::ERROR_UNSAFE, " The vehicle is not stopped." );
148
150
}
149
151
try {
150
- change_state (State::Message::INITIALIZING);
151
- change_node_trigger (false , false );
152
-
153
- auto pose = req->pose .empty () ? get_gnss_pose () : req->pose .front ();
154
- if (ndt_) {
155
- pose = ndt_->align_pose (pose);
156
- } else if (yabloc_) {
157
- // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more
158
- // accuracy pose.
159
- pose = yabloc_->align_pose (pose);
160
- }
161
- pose.pose .covariance = output_pose_covariance_;
162
- pub_reset_->publish (pose);
152
+ if (req->method == Initialize::Service::Request::AUTO) {
153
+ change_state (State::Message::INITIALIZING);
154
+ change_node_trigger (false , false );
155
+
156
+ auto pose =
157
+ req->pose_with_covariance .empty () ? get_gnss_pose () : req->pose_with_covariance .front ();
158
+ if (ndt_) {
159
+ pose = ndt_->align_pose (pose);
160
+ } else if (yabloc_) {
161
+ // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more
162
+ // accuracy pose.
163
+ pose = yabloc_->align_pose (pose);
164
+ }
165
+ pose.pose .covariance = output_pose_covariance_;
166
+ pub_reset_->publish (pose);
167
+
168
+ change_node_trigger (true , false );
169
+ res->status .success = true ;
170
+ change_state (State::Message::INITIALIZED);
171
+
172
+ } else if (req->method == Initialize::Service::Request::DIRECT) {
173
+ if (req->pose_with_covariance .empty ()) {
174
+ std::stringstream message;
175
+ message << " No input pose_with_covariance. If you want to use DIRECT method, please input "
176
+ " pose_with_covariance." ;
177
+ RCLCPP_ERROR (get_logger (), message.str ().c_str ());
178
+ throw ServiceException (
179
+ autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str ());
180
+ }
181
+ auto pose = req->pose_with_covariance .front ().pose .pose ;
182
+ set_user_defined_initial_pose (pose, false );
183
+ res->status .success = true ;
163
184
164
- change_node_trigger (true , false );
165
- res->status .success = true ;
166
- change_state (State::Message::INITIALIZED);
185
+ } else {
186
+ std::stringstream message;
187
+ message << " Unknown method type (=" << std::to_string (req->method ) << " )" ;
188
+ RCLCPP_ERROR (get_logger (), message.str ().c_str ());
189
+ throw ServiceException (
190
+ autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str ());
191
+ }
167
192
} catch (const ServiceException & error) {
168
- res->status = error.status ();
193
+ autoware_adapi_v1_msgs::msg::ResponseStatus respose_status;
194
+ respose_status = error.status ();
195
+ res->status .success = respose_status.success ;
196
+ res->status .code = respose_status.code ;
197
+ res->status .message = respose_status.message ;
169
198
change_state (State::Message::UNINITIALIZED);
170
199
}
171
200
}
0 commit comments