Skip to content

Commit 405ec70

Browse files
YamatoAndopre-commit-ci[bot]mitsudome-r
authored
feat(pose_initializer): add a method type to the /localization/initialize service. (#7048)
* change message type of InitializeLocalization in component_interface_specs Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * change message type of InitializeLocalizaion in pose_initializer Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * modify readme Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * style(pre-commit): autofix * delete redundant line Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * fix typo Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * Update localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> * add description in readme Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * style(pre-commit): autofix * add bash Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> --------- Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
1 parent b6c548f commit 405ec70

File tree

10 files changed

+241
-28
lines changed

10 files changed

+241
-28
lines changed

common/component_interface_specs/include/component_interface_specs/localization.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -18,16 +18,16 @@
1818
#include <rclcpp/qos.hpp>
1919

2020
#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
21-
#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
2221
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
2322
#include <nav_msgs/msg/odometry.hpp>
23+
#include <tier4_localization_msgs/srv/initialize_localization.hpp>
2424

2525
namespace localization_interface
2626
{
2727

2828
struct Initialize
2929
{
30-
using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization;
30+
using Service = tier4_localization_msgs::srv::InitializeLocalization;
3131
static constexpr char name[] = "/localization/initialize";
3232
};
3333

common/component_interface_specs/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
<depend>rclcpp</depend>
2222
<depend>rosidl_runtime_cpp</depend>
2323
<depend>tier4_control_msgs</depend>
24+
<depend>tier4_localization_msgs</depend>
2425
<depend>tier4_map_msgs</depend>
2526
<depend>tier4_planning_msgs</depend>
2627
<depend>tier4_system_msgs</depend>

localization/pose_initializer/README.md

+86-3
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,9 @@ This node depends on the map height fitter library.
1717

1818
### Services
1919

20-
| Name | Type | Description |
21-
| -------------------------- | --------------------------------------------------- | --------------------- |
22-
| `/localization/initialize` | autoware_adapi_v1_msgs::srv::InitializeLocalization | initial pose from api |
20+
| Name | Type | Description |
21+
| -------------------------- | ---------------------------------------------------- | --------------------- |
22+
| `/localization/initialize` | tier4_localization_msgs::srv::InitializeLocalization | initial pose from api |
2323

2424
### Clients
2525

@@ -46,3 +46,86 @@ This node depends on the map height fitter library.
4646
This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md).
4747

4848
<img src="../../system/default_ad_api/document/images/localization.drawio.svg" alt="drawing" width="800"/>
49+
50+
## Initialize pose via CLI
51+
52+
### Using the GNSS estimated position
53+
54+
```bash
55+
ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization
56+
```
57+
58+
The GNSS estimated position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position.
59+
60+
### Using the input position
61+
62+
```bash
63+
ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization "
64+
pose_with_covariance:
65+
- header:
66+
frame_id: map
67+
pose:
68+
pose:
69+
position:
70+
x: 89571.1640625
71+
y: 42301.1875
72+
z: -3.1565165519714355
73+
orientation:
74+
x: 0.0
75+
y: 0.0
76+
z: 0.28072773940524687
77+
w: 0.9597874433062874
78+
covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467]
79+
method: 0
80+
"
81+
```
82+
83+
The input position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position.
84+
85+
### Direct initial position set
86+
87+
```bash
88+
ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization "
89+
pose_with_covariance:
90+
- header:
91+
frame_id: map
92+
pose:
93+
pose:
94+
position:
95+
x: 89571.1640625
96+
y: 42301.1875
97+
z: -3.1565165519714355
98+
orientation:
99+
x: 0.0
100+
y: 0.0
101+
z: 0.28072773940524687
102+
w: 0.9597874433062874
103+
covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467]
104+
method: 1
105+
"
106+
```
107+
108+
The initial position is set directly by the input position without going through localization algorithm.
109+
110+
### Via ros2 topic pub
111+
112+
```bash
113+
ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
114+
header:
115+
frame_id: map
116+
pose:
117+
pose:
118+
position:
119+
x: 89571.1640625
120+
y: 42301.1875
121+
z: 0.0
122+
orientation:
123+
x: 0.0
124+
y: 0.0
125+
z: 0.28072773940524687
126+
w: 0.9597874433062874
127+
"
128+
```
129+
130+
It behaves the same as "initialpose (from rviz)".
131+
The position.z and the covariance will be overwritten by [ad_api_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/default_ad_api_helpers/ad_api_adaptors), so there is no need to input them.

localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp

+50-21
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include "yabloc_module.hpp"
2424

2525
#include <memory>
26+
#include <sstream>
2627
#include <vector>
2728

2829
PoseInitializer::PoseInitializer() : Node("pose_initializer")
@@ -79,7 +80,7 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer")
7980
initial_pose.orientation.z = initial_pose_array[5];
8081
initial_pose.orientation.w = initial_pose_array[6];
8182

82-
set_user_defined_initial_pose(initial_pose);
83+
set_user_defined_initial_pose(initial_pose, true);
8384
}
8485
}
8586
}
@@ -114,11 +115,12 @@ void PoseInitializer::change_node_trigger(bool flag, bool need_spin)
114115
}
115116
}
116117

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)
118120
{
119121
try {
120122
change_state(State::Message::INITIALIZING);
121-
change_node_trigger(false, true);
123+
change_node_trigger(false, need_spin);
122124

123125
PoseWithCovarianceStamped pose;
124126
pose.header.frame_id = "map";
@@ -127,7 +129,7 @@ void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Po
127129
pose.pose.covariance = output_pose_covariance_;
128130
pub_reset_->publish(pose);
129131

130-
change_node_trigger(true, true);
132+
change_node_trigger(true, need_spin);
131133
change_state(State::Message::INITIALIZED);
132134

133135
RCLCPP_INFO(get_logger(), "Set user defined initial pose");
@@ -147,25 +149,52 @@ void PoseInitializer::on_initialize(
147149
Initialize::Service::Response::ERROR_UNSAFE, "The vehicle is not stopped.");
148150
}
149151
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;
163184

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+
}
167192
} 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;
169198
change_state(State::Message::UNINITIALIZED);
170199
}
171200
}

localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,8 @@ class PoseInitializer : public rclcpp::Node
6060
double stop_check_duration_;
6161

6262
void change_node_trigger(bool flag, bool need_spin = false);
63-
void set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose);
63+
void set_user_defined_initial_pose(
64+
const geometry_msgs::msg::Pose initial_pose, bool need_spin = false);
6465
void change_state(State::Message::_state_type state);
6566
void on_initialize(
6667
const Initialize::Service::Request::SharedPtr req,

system/default_ad_api/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
1818
src/vehicle.cpp
1919
src/vehicle_info.cpp
2020
src/vehicle_door.cpp
21+
src/utils/localization_conversion.cpp
2122
src/utils/route_conversion.cpp
2223
src/compatibility/autoware_state.cpp
2324
)

system/default_ad_api/src/localization.cpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,8 @@
1414

1515
#include "localization.hpp"
1616

17+
#include "utils/localization_conversion.hpp"
18+
1719
namespace default_ad_api
1820
{
1921

@@ -23,7 +25,15 @@ LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options)
2325
const auto adaptor = component_interface_utils::NodeAdaptor(this);
2426
group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
2527
adaptor.relay_message(pub_state_, sub_state_);
26-
adaptor.relay_service(cli_initialize_, srv_initialize_, group_cli_);
28+
adaptor.init_cli(cli_initialize_);
29+
adaptor.init_srv(srv_initialize_, this, &LocalizationNode::on_initialize, group_cli_);
30+
}
31+
32+
void LocalizationNode::on_initialize(
33+
const autoware_ad_api::localization::Initialize::Service::Request::SharedPtr req,
34+
const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res)
35+
{
36+
res->status = localization_conversion::convert_call(cli_initialize_, req);
2737
}
2838

2939
} // namespace default_ad_api

system/default_ad_api/src/localization.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,10 @@ class LocalizationNode : public rclcpp::Node
3636
Pub<autoware_ad_api::localization::InitializationState> pub_state_;
3737
Cli<localization_interface::Initialize> cli_initialize_;
3838
Sub<localization_interface::InitializationState> sub_state_;
39+
40+
void on_initialize(
41+
const autoware_ad_api::localization::Initialize::Service::Request::SharedPtr req,
42+
const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res);
3943
};
4044

4145
} // namespace default_ad_api
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "localization_conversion.hpp"
16+
17+
#include <memory>
18+
19+
namespace default_ad_api::localization_conversion
20+
{
21+
22+
InternalInitializeRequest convert_request(const ExternalInitializeRequest & external)
23+
{
24+
auto internal = std::make_shared<InternalInitializeRequest::element_type>();
25+
internal->pose_with_covariance = external->pose;
26+
internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::AUTO;
27+
return internal;
28+
}
29+
30+
ExternalResponse convert_response(const InternalResponse & internal)
31+
{
32+
// TODO(Takagi, Isamu): check error code correspondence
33+
ExternalResponse external;
34+
external.success = internal.success;
35+
external.code = internal.code;
36+
external.message = internal.message;
37+
return external;
38+
}
39+
40+
} // namespace default_ad_api::localization_conversion
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef UTILS__LOCALIZATION_CONVERSION_HPP_
16+
#define UTILS__LOCALIZATION_CONVERSION_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
21+
#include <tier4_localization_msgs/srv/initialize_localization.hpp>
22+
23+
namespace default_ad_api::localization_conversion
24+
{
25+
26+
using ExternalInitializeRequest =
27+
autoware_adapi_v1_msgs::srv::InitializeLocalization::Request::SharedPtr;
28+
using InternalInitializeRequest =
29+
tier4_localization_msgs::srv::InitializeLocalization::Request::SharedPtr;
30+
InternalInitializeRequest convert_request(const ExternalInitializeRequest & external);
31+
32+
using ExternalResponse = autoware_adapi_v1_msgs::msg::ResponseStatus;
33+
using InternalResponse = autoware_common_msgs::msg::ResponseStatus;
34+
ExternalResponse convert_response(const InternalResponse & internal);
35+
36+
template <class ClientT, class RequestT>
37+
ExternalResponse convert_call(ClientT & client, RequestT & req)
38+
{
39+
return convert_response(client->call(convert_request(req))->status);
40+
}
41+
42+
} // namespace default_ad_api::localization_conversion
43+
44+
#endif // UTILS__LOCALIZATION_CONVERSION_HPP_

0 commit comments

Comments
 (0)