diff --git a/common/autoware_ad_api_msgs/README.md b/common/autoware_ad_api_msgs/README.md
index 58ccb8bbf2d2b..a596e8c5ba8b4 100644
--- a/common/autoware_ad_api_msgs/README.md
+++ b/common/autoware_ad_api_msgs/README.md
@@ -25,7 +25,15 @@ The version of AD API follows [Semantic Versioning][semver] in order to provide
The routing service support two formats. One uses pose and the other uses map dependent data directly.
The body part of the route message is optional, since the route does not exist when it is cleared by the service.
+[See also routing API][api-routing].
+
+## Localization
+
+The initialization state does not reflect localization errors. Use diagnostics for that purpose.
+[See also localization API][api-localization].
[semver]: https://semver.org/
+[api-localization]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/localization/
+[api-routing]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/
diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml
index 71525748d5f2d..e0fdacdc7101a 100644
--- a/launch/tier4_localization_launch/launch/util/util.launch.xml
+++ b/launch/tier4_localization_launch/launch/util/util.launch.xml
@@ -4,6 +4,7 @@
+
@@ -12,9 +13,18 @@
-
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
index c263e2627678e..608d98c7e4ddc 100644
--- a/launch/tier4_simulator_launch/launch/simulator.launch.xml
+++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -4,6 +4,7 @@
+
@@ -92,6 +93,15 @@
+
+
+
+
+
+
+
+
+
diff --git a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml b/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml
index bfbb6beeaf054..7a16c35390490 100644
--- a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml
+++ b/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml
@@ -25,9 +25,9 @@
"/map/vector_map",
"/map/pointcloud_map",
"/perception/obstacle_segmentation/pointcloud",
+ "/perception/object_recognition/objects",
"/initialpose3d",
"/localization/pose_twist_fusion_filter/pose",
- "/perception/object_recognition/objects",
"/planning/mission_planning/route",
"/planning/scenario_planning/trajectory",
"/control/trajectory_follower/control_cmd",
diff --git a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml
index b20ffec096e03..51fbe99dd9bc3 100644
--- a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml
+++ b/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml
@@ -24,7 +24,7 @@
names: [
"/map/vector_map",
"/perception/object_recognition/objects",
- "/initialpose2d",
+ "/initialpose3d",
"/planning/mission_planning/route",
"/planning/scenario_planning/trajectory",
"/control/trajectory_follower/control_cmd",
@@ -48,7 +48,7 @@
warn_rate: 5.0
type: "autoware_auto_perception_msgs/msg/PredictedObjects"
- /initialpose2d:
+ /initialpose3d:
module: "localization"
timeout: 0.0
warn_rate: 0.0
diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml
index 9ec05bae8a864..69509cd1c3a95 100644
--- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml
+++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml
@@ -5,7 +5,7 @@
-
+
diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp
index 114883e33da58..654095641c7f4 100644
--- a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp
+++ b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp
@@ -69,7 +69,7 @@ void InitialPoseButtonPanel::onInitialize()
std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1));
client_ = raw_node->create_client(
- "/localization/util/initialize_pose");
+ "/localization/initialize");
}
void InitialPoseButtonPanel::callbackPoseCov(
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
index aff97be9e2fbc..38e3f8c617327 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -341,14 +341,12 @@ void NDTScanMatcher::serviceNDTAlign(
if (ndt_ptr_->getInputTarget() == nullptr) {
res->success = false;
- res->seq = req->seq;
RCLCPP_WARN(get_logger(), "No InputTarget");
return;
}
if (ndt_ptr_->getInputSource() == nullptr) {
res->success = false;
- res->seq = req->seq;
RCLCPP_WARN(get_logger(), "No InputSource");
return;
}
@@ -360,7 +358,6 @@ void NDTScanMatcher::serviceNDTAlign(
res->pose_with_covariance = alignUsingMonteCarlo(ndt_ptr_, mapTF_initial_pose_msg);
key_value_stdmap_["state"] = "Sleeping";
res->success = true;
- res->seq = req->seq;
res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance;
}
diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt
index 32a1525463eaa..2808a8de31894 100644
--- a/localization/pose_initializer/CMakeLists.txt
+++ b/localization/pose_initializer/CMakeLists.txt
@@ -4,21 +4,25 @@ project(pose_initializer)
find_package(autoware_cmake REQUIRED)
autoware_package()
-find_package(PCL REQUIRED)
-
-ament_auto_add_executable(pose_initializer
- src/pose_initializer_node.cpp
- src/pose_initializer_core.cpp
+find_package(PCL REQUIRED COMPONENTS common)
+
+ament_auto_add_executable(pose_initializer_node
+ src/pose_initializer/pose_initializer_node.cpp
+ src/pose_initializer/pose_initializer_core.cpp
+ src/pose_initializer/gnss_module.cpp
+ src/pose_initializer/ndt_module.cpp
+ src/pose_initializer/stop_check_module.cpp
)
-
-# fmt often fails to automatically link so manually specify here
-target_link_libraries(pose_initializer ${PCL_LIBRARIES} fmt)
+ament_auto_add_executable(map_height_fitter
+ src/map_height_fitter/map_height_fitter_node.cpp
+ src/map_height_fitter/map_height_fitter_core.cpp
+)
+target_link_libraries(map_height_fitter ${PCL_LIBRARIES})
if(BUILD_TESTING)
function(add_testcase filepath)
get_filename_component(filename ${filepath} NAME)
string(REGEX REPLACE ".cpp" "" test_name ${filename})
-
ament_add_gmock(${test_name} ${filepath})
target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(${test_name} fmt)
@@ -27,7 +31,6 @@ if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
-
add_testcase(test/test_copy_vector_to_array.cpp)
endif()
diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md
index 8fceb433efa58..c9e39058e6604 100644
--- a/localization/pose_initializer/README.md
+++ b/localization/pose_initializer/README.md
@@ -1,24 +1,4 @@
# pose_initializer
-## Purpose
-
-`pose_initializer` is the package to send an initial pose to `ekf_localizer`.
-It receives roughly estimated initial pose from GNSS/user.
-Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service.
-Finally, it publishes the initial pose to `ekf_localizer`.
-
-## Input / Output
-
-### Input topics
-
-| Name | Type | Description |
-| ------------------------------------ | --------------------------------------------- | ---------------------- |
-| `/initialpose` | geometry_msgs::msg::PoseWithCovarianceStamped | initial pose from rviz |
-| `/sensing/gnss/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | pose from gnss |
-| `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map |
-
-### Output topics
-
-| Name | Type | Description |
-| ---------------- | --------------------------------------------- | --------------------------- |
-| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose |
+- [pose_initializer](./docs/pose_initializer.md)
+- [map_height_fitter](./docs/map_height_fitter.md)
diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml
index a64c9de8dcde7..e94f86c8d6c7e 100644
--- a/localization/pose_initializer/config/pose_initializer.param.yaml
+++ b/localization/pose_initializer/config/pose_initializer.param.yaml
@@ -1,17 +1,6 @@
/**:
ros__parameters:
- # from initialpose (Rviz's 2DPoseEstimate)
- initialpose_particle_covariance:
- [
- 4.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 4.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
- ]
-
# from gnss
gnss_particle_covariance:
[
@@ -23,17 +12,6 @@
0.0, 0.0, 0.0, 0.0, 0.0, 10.0,
]
- # from service
- service_particle_covariance:
- [
- 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 10.0,
- ]
-
# output
output_pose_covariance:
[
diff --git a/localization/pose_initializer/docs/map_height_fitter.md b/localization/pose_initializer/docs/map_height_fitter.md
new file mode 100644
index 0000000000000..ee7fdfb44264d
--- /dev/null
+++ b/localization/pose_initializer/docs/map_height_fitter.md
@@ -0,0 +1,22 @@
+# map_height_fitter
+
+## Purpose
+
+This node provides a service to fit a pose height to a map.
+Use this service as preprocessing for `pose_initializer` when using a initial poses with inaccurate height such as RViz and GNSS.
+This service replaces the Z value of the input pose with the lowest point of the map point cloud within a cylinder of XY-radius.
+If no point is found in this range, returns the input pose without changes.
+
+## Interfaces
+
+### Services
+
+| Name | Type | Description |
+| ----------------------------------- | ------------------------------------------------------- | -------------------- |
+| `/localization/util/fit_map_height` | tier4_localization_msgs::srv::PoseWithCovarianceStamped | pose fitting service |
+
+### Subscriptions
+
+| Name | Type | Description |
+| --------------------- | ----------------------------- | -------------- |
+| `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map |
diff --git a/localization/pose_initializer/docs/pose_initializer.md b/localization/pose_initializer/docs/pose_initializer.md
new file mode 100644
index 0000000000000..08d27ae83b51d
--- /dev/null
+++ b/localization/pose_initializer/docs/pose_initializer.md
@@ -0,0 +1,46 @@
+# pose_initializer
+
+## Purpose
+
+`pose_initializer` is the package to send an initial pose to `ekf_localizer`.
+It receives roughly estimated initial pose from GNSS/user.
+Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service.
+Finally, it publishes the initial pose to `ekf_localizer`.
+
+## Interfaces
+
+### Parameters
+
+| Name | Type | Description |
+| --------------------- | ---- | ---------------------------------------------------------------------------------------- |
+| `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. |
+| `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. |
+| `stop_check_duration` | bool | The duration used for the stop check above. |
+| `gnss_enabled` | bool | If true, use the GNSS pose when no pose is specified. |
+| `gnss_pose_timeout` | bool | The duration that the GNSS pose is valid. |
+
+### Services
+
+| Name | Type | Description |
+| -------------------------- | ------------------------------------------------- | --------------------- |
+| `/localization/initialize` | autoware_ad_api_msgs::srv::InitializeLocalization | initial pose from api |
+
+### Clients
+
+| Name | Type | Description |
+| -------------------------------------------- | ------------------------------------------------------- | ----------------------- |
+| `/localization/pose_estimator/ndt_align_srv` | tier4_localization_msgs::srv::PoseWithCovarianceStamped | pose estimation service |
+
+### Subscriptions
+
+| Name | Type | Description |
+| ----------------------------------------------------------- | --------------------------------------------- | -------------------- |
+| `/sensing/gnss/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | pose from gnss |
+| `/sensing/vehicle_velocity_converter/twist_with_covariance` | geometry_msgs::msg::TwistStamped | twist for stop check |
+
+### Publications
+
+| Name | Type | Description |
+| ------------------------------------ | ---------------------------------------------------------- | --------------------------- |
+| `/localization/initialization_state` | autoware_ad_api_msgs::msg::LocalizationInitializationState | pose initialization state |
+| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose |
diff --git a/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp
deleted file mode 100644
index a78c2040dcd77..0000000000000
--- a/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp
+++ /dev/null
@@ -1,100 +0,0 @@
-// Copyright 2020 Autoware Foundation
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
-#define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
-
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-
-#include
-#include
-
-class PoseInitializer : public rclcpp::Node
-{
-public:
- PoseInitializer();
-
-private:
- void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr);
- void serviceInitializePose(
- const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
- tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
- void serviceInitializePoseAuto(
- const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr req,
- tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr res);
- void callbackInitialPose(
- geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr);
- void callbackGNSSPoseCov(
- geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr);
- void callbackPoseInitializationRequest(
- const tier4_localization_msgs::msg::PoseInitializationRequest::ConstSharedPtr
- request_msg_ptr); // NOLINT
-
- bool getHeight(
- const geometry_msgs::msg::PoseWithCovarianceStamped & input_pose_msg,
- const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr output_pose_msg_ptr);
- bool callAlignServiceAndPublishResult(
- const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
-
- rclcpp::Subscription::SharedPtr gnss_pose_sub_;
- rclcpp::Subscription::SharedPtr map_points_sub_;
-
- // TODO(Takagi, Isamu): deprecated
- rclcpp::Subscription::SharedPtr initial_pose_sub_;
- rclcpp::Subscription::SharedPtr
- pose_initialization_request_sub_;
-
- rclcpp::Publisher::SharedPtr initial_pose_pub_;
-
- rclcpp::Client::SharedPtr ndt_client_;
-
- rclcpp::CallbackGroup::SharedPtr initialize_pose_service_group_;
- rclcpp::Service::SharedPtr
- initialize_pose_service_;
- rclcpp::Service::SharedPtr
- initialize_pose_auto_service_;
-
- tf2::BufferCore tf2_buffer_;
- tf2_ros::TransformListener tf2_listener_;
-
- pcl::PointCloud::Ptr map_ptr_;
- std::string map_frame_;
-
- // With the currently available facilities for calling a service, there is no
- // easy way of detecting whether an answer was received within a reasonable
- // amount of time. So, as a sanity check, we check whether a response for the
- // previous request was received when a new request is sent.
- uint32_t request_id_ = 0;
- uint32_t response_id_ = 0;
-
- bool enable_gnss_callback_;
- std::array initialpose_particle_covariance_;
- std::array gnss_particle_covariance_;
- std::array service_particle_covariance_;
- std::array output_pose_covariance_;
-};
-
-#endif // POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml
index 0645f0cbfca53..eacd638d85d22 100644
--- a/localization/pose_initializer/launch/pose_initializer.launch.xml
+++ b/localization/pose_initializer/launch/pose_initializer.launch.xml
@@ -1,16 +1,26 @@
+
+
+
-
-
-
-
+
+
+
-
-
-
+
+
+
-
+
+
+
+
+
+
+
+
+
diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml
index 4ed7f4ae65465..1f478850e5bb2 100644
--- a/localization/pose_initializer/package.xml
+++ b/localization/pose_initializer/package.xml
@@ -5,25 +5,27 @@
0.1.0
The pose_initializer package
Yamato Ando
+ Takagi, Isamu
Apache License 2.0
Yamato Ando
+ Takagi, Isamu
ament_cmake
autoware_cmake
- fmt
+ component_interface_specs
+ component_interface_utils
geometry_msgs
libpcl-all-dev
+ motion_utils
pcl_conversions
rclcpp
+ rclcpp_components
sensor_msgs
- std_msgs
tf2
tf2_geometry_msgs
tf2_ros
- tier4_api_utils
- tier4_external_api_msgs
tier4_localization_msgs
ament_cmake_cppcheck
diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp
new file mode 100644
index 0000000000000..b70c0eac5ddbd
--- /dev/null
+++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp
@@ -0,0 +1,91 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "map_height_fitter_core.hpp"
+
+#include
+
+#ifdef ROS_DISTRO_GALACTIC
+#include
+#else
+#include
+#endif
+
+#include
+
+MapHeightFitter::MapHeightFitter() : Node("map_height_fitter"), tf2_listener_(tf2_buffer_)
+{
+ const auto durable_qos = rclcpp::QoS(1).transient_local();
+ using std::placeholders::_1;
+ using std::placeholders::_2;
+
+ sub_map_ = create_subscription(
+ "pointcloud_map", durable_qos, std::bind(&MapHeightFitter::on_map, this, _1));
+ srv_fit_ = create_service(
+ "fit_map_height", std::bind(&MapHeightFitter::on_fit, this, _1, _2));
+}
+
+void MapHeightFitter::on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
+{
+ map_frame_ = msg->header.frame_id;
+ map_cloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud);
+ pcl::fromROSMsg(*msg, *map_cloud_);
+}
+
+double MapHeightFitter::get_ground_height(const tf2::Vector3 & point) const
+{
+ constexpr double radius = 1.0 * 1.0;
+ const double x = point.getX();
+ const double y = point.getY();
+
+ double height = INFINITY;
+ for (const auto & p : map_cloud_->points) {
+ const double dx = x - p.x;
+ const double dy = y - p.y;
+ const double sd = (dx * dx) + (dy * dy);
+ if (sd < radius) {
+ height = std::min(height, static_cast(p.z));
+ }
+ }
+ return std::isfinite(height) ? height : point.getZ();
+}
+
+void MapHeightFitter::on_fit(
+ const RequestHeightFitting::Request::SharedPtr req,
+ const RequestHeightFitting::Response::SharedPtr res) const
+{
+ const auto & position = req->pose_with_covariance.pose.pose.position;
+ tf2::Vector3 point(position.x, position.y, position.z);
+ std::string req_frame = req->pose_with_covariance.header.frame_id;
+ res->success = false;
+
+ if (map_cloud_) {
+ try {
+ const auto stamped = tf2_buffer_.lookupTransform(map_frame_, req_frame, tf2::TimePointZero);
+ tf2::Transform transform{tf2::Quaternion{}, tf2::Vector3{}};
+ tf2::fromMsg(stamped.transform, transform);
+ point = transform * point;
+ point.setZ(get_ground_height(point));
+ point = transform.inverse() * point;
+ res->success = true;
+ } catch (tf2::TransformException & exception) {
+ RCLCPP_WARN_STREAM(get_logger(), "failed to lookup transform: " << exception.what());
+ }
+ }
+
+ res->pose_with_covariance = req->pose_with_covariance;
+ res->pose_with_covariance.pose.pose.position.x = point.getX();
+ res->pose_with_covariance.pose.pose.position.y = point.getY();
+ res->pose_with_covariance.pose.pose.position.z = point.getZ();
+}
diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp
new file mode 100644
index 0000000000000..8845b77a78095
--- /dev/null
+++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp
@@ -0,0 +1,52 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_CORE_HPP_
+#define MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_CORE_HPP_
+
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+
+class MapHeightFitter : public rclcpp::Node
+{
+public:
+ MapHeightFitter();
+
+private:
+ using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+ using RequestHeightFitting = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+ tf2::BufferCore tf2_buffer_;
+ tf2_ros::TransformListener tf2_listener_;
+ std::string map_frame_;
+ pcl::PointCloud::Ptr map_cloud_;
+ rclcpp::Subscription::SharedPtr sub_map_;
+ rclcpp::Service::SharedPtr srv_fit_;
+
+ void on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
+ void on_fit(
+ const RequestHeightFitting::Request::SharedPtr req,
+ const RequestHeightFitting::Response::SharedPtr res) const;
+ double get_ground_height(const tf2::Vector3 & point) const;
+};
+
+#endif // MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_CORE_HPP_
diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp
new file mode 100644
index 0000000000000..296a50e0cd3ab
--- /dev/null
+++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp
@@ -0,0 +1,28 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "map_height_fitter_core.hpp"
+
+#include
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::executors::SingleThreadedExecutor executor;
+ auto node = std::make_shared();
+ executor.add_node(node);
+ executor.spin();
+ executor.remove_node(node);
+ rclcpp::shutdown();
+}
diff --git a/localization/pose_initializer/include/pose_initializer/copy_vector_to_array.hpp b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp
similarity index 63%
rename from localization/pose_initializer/include/pose_initializer/copy_vector_to_array.hpp
rename to localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp
index 2b8b31e3bc651..1274092ae4993 100644
--- a/localization/pose_initializer/include/pose_initializer/copy_vector_to_array.hpp
+++ b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp
@@ -19,19 +19,30 @@
#include
#include
+#include
#include
template
-void CopyVectorToArray(const std::vector & vector, std::array & array)
+void copy_vector_to_array(const std::vector & vector, std::array & array)
{
if (N != vector.size()) {
// throws the error to prevent causing an anonymous bug
// such as only partial array is initialized
- throw std::invalid_argument(fmt::format(
- "Vector size (which is {}) is different from the copy size (which is {})", vector.size(), N));
+ const auto v = std::to_string(vector.size());
+ const auto n = std::to_string(N);
+ throw std::invalid_argument(
+ "Vector size (which is " + v + ") is different from the copy size (which is " + n + ")");
}
-
std::copy_n(vector.begin(), N, array.begin());
}
+template
+std::array get_covariance_parameter(NodeT * node, const std::string & name)
+{
+ const auto parameter = node->template declare_parameter>(name);
+ std::array covariance;
+ copy_vector_to_array(parameter, covariance);
+ return covariance;
+}
+
#endif // POSE_INITIALIZER__COPY_VECTOR_TO_ARRAY_HPP_
diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.cpp b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp
new file mode 100644
index 0000000000000..e06cec1c2c24e
--- /dev/null
+++ b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp
@@ -0,0 +1,52 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "gnss_module.hpp"
+
+#include
+#include
+
+#include
+
+GnssModule::GnssModule(rclcpp::Node * node)
+{
+ cli_map_fit_ = node->create_client("fit_map_height");
+ sub_gnss_pose_ = node->create_subscription(
+ "gnss_pose_cov", 1, [this](PoseWithCovarianceStamped::ConstSharedPtr msg) { pose_ = msg; });
+
+ clock_ = node->get_clock();
+ timeout_ = node->declare_parameter("gnss_pose_timeout");
+}
+
+geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose() const
+{
+ using Initialize = localization_interface::Initialize;
+
+ if (!pose_) {
+ throw component_interface_utils::ServiceException(
+ Initialize::Service::Response::ERROR_GNSS, "The GNSS pose has not arrived.");
+ }
+
+ const auto elapsed = rclcpp::Time(pose_->header.stamp) - clock_->now();
+ if (timeout_ < elapsed.seconds()) {
+ throw component_interface_utils::ServiceException(
+ Initialize::Service::Response::ERROR_GNSS, "The GNSS pose is out of date.");
+ }
+
+ const auto req = std::make_shared();
+ req->pose_with_covariance = *pose_;
+
+ auto future = cli_map_fit_->async_send_request(req);
+ return future.get()->pose_with_covariance;
+}
diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp
new file mode 100644
index 0000000000000..24d28a64a72a9
--- /dev/null
+++ b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp
@@ -0,0 +1,41 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef POSE_INITIALIZER__GNSS_MODULE_HPP_
+#define POSE_INITIALIZER__GNSS_MODULE_HPP_
+
+#include
+
+#include
+#include
+
+class GnssModule
+{
+private:
+ using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+ using RequestHeightFitting = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+
+public:
+ explicit GnssModule(rclcpp::Node * node);
+ PoseWithCovarianceStamped get_pose() const;
+
+private:
+ rclcpp::Clock::SharedPtr clock_;
+ rclcpp::Client::SharedPtr cli_map_fit_;
+ rclcpp::Subscription::SharedPtr sub_gnss_pose_;
+ PoseWithCovarianceStamped::ConstSharedPtr pose_;
+ double timeout_;
+};
+
+#endif // POSE_INITIALIZER__GNSS_MODULE_HPP_
diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp
new file mode 100644
index 0000000000000..801a08f83aab8
--- /dev/null
+++ b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp
@@ -0,0 +1,51 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ndt_module.hpp"
+
+#include
+#include
+
+#include
+
+using ServiceException = component_interface_utils::ServiceException;
+using Initialize = localization_interface::Initialize;
+using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+
+NdtModule::NdtModule(rclcpp::Node * node) : logger_(node->get_logger())
+{
+ cli_align_ = node->create_client("ndt_align");
+}
+
+PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped & pose)
+{
+ const auto req = std::make_shared();
+ req->pose_with_covariance = pose;
+
+ if (!cli_align_->service_is_ready()) {
+ throw component_interface_utils::ServiceUnready("NDT align server is not ready.");
+ }
+
+ RCLCPP_INFO(logger_, "Call NDT align server.");
+ const auto res = cli_align_->async_send_request(req).get();
+ if (!res->success) {
+ RCLCPP_INFO(logger_, "NDT align server failed.");
+ throw ServiceException(
+ Initialize::Service::Response::ERROR_ESTIMATION, "NDT align server failed.");
+ }
+ RCLCPP_INFO(logger_, "NDT align server succeeded.");
+
+ // Overwrite the covariance.
+ return res->pose_with_covariance;
+}
diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.hpp b/localization/pose_initializer/src/pose_initializer/ndt_module.hpp
new file mode 100644
index 0000000000000..a6cdea9879e34
--- /dev/null
+++ b/localization/pose_initializer/src/pose_initializer/ndt_module.hpp
@@ -0,0 +1,38 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef POSE_INITIALIZER__NDT_MODULE_HPP_
+#define POSE_INITIALIZER__NDT_MODULE_HPP_
+
+#include
+
+#include
+#include
+
+class NdtModule
+{
+private:
+ using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+ using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+
+public:
+ explicit NdtModule(rclcpp::Node * node);
+ PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose);
+
+private:
+ rclcpp::Logger logger_;
+ rclcpp::Client::SharedPtr cli_align_;
+};
+
+#endif // POSE_INITIALIZER__NDT_MODULE_HPP_
diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp
new file mode 100644
index 0000000000000..15bb07d235e94
--- /dev/null
+++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp
@@ -0,0 +1,96 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "pose_initializer_core.hpp"
+
+#include "copy_vector_to_array.hpp"
+#include "gnss_module.hpp"
+#include "ndt_module.hpp"
+#include "stop_check_module.hpp"
+
+#include
+#include
+
+PoseInitializer::PoseInitializer() : Node("pose_initializer")
+{
+ const auto node = component_interface_utils::NodeAdaptor(this);
+ group_srv_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+ node.init_pub(pub_state_);
+ node.init_srv(srv_initialize_, this, &PoseInitializer::on_initialize, group_srv_);
+ pub_reset_ = create_publisher("pose_reset", 1);
+
+ output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance");
+ gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance");
+
+ if (declare_parameter("gnss_enabled")) {
+ gnss_ = std::make_unique(this);
+ }
+ if (declare_parameter("ndt_enabled")) {
+ ndt_ = std::make_unique(this);
+ }
+ if (declare_parameter("stop_check_enabled")) {
+ // Add 1.0 sec margin for twist buffer.
+ stop_check_duration_ = declare_parameter("stop_check_duration");
+ stop_check_ = std::make_unique(this, stop_check_duration_ + 1.0);
+ }
+ change_state(State::Message::UNINITIALIZED);
+}
+
+PoseInitializer::~PoseInitializer()
+{
+ // to delete gnss module
+}
+
+void PoseInitializer::change_state(State::Message::_state_type state)
+{
+ state_.stamp = now();
+ state_.state = state;
+ pub_state_->publish(state_);
+}
+
+void PoseInitializer::on_initialize(
+ const Initialize::Service::Request::SharedPtr req,
+ const Initialize::Service::Response::SharedPtr res)
+{
+ // NOTE: This function is not executed during initialization because mutually exclusive.
+ if (stop_check_ && !stop_check_->isVehicleStopped(stop_check_duration_)) {
+ throw ServiceException(
+ Initialize::Service::Response::ERROR_UNSAFE, "The vehicle is not stopped.");
+ }
+ try {
+ change_state(State::Message::INITIALIZING);
+ auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front();
+ if (ndt_) {
+ pose = ndt_->align_pose(pose);
+ }
+ pose.pose.covariance = output_pose_covariance_;
+ pub_reset_->publish(pose);
+ res->status.success = true;
+ change_state(State::Message::INITIALIZED);
+ } catch (const ServiceException & error) {
+ res->status = error.status();
+ change_state(State::Message::UNINITIALIZED);
+ }
+}
+
+geometry_msgs::msg::PoseWithCovarianceStamped PoseInitializer::get_gnss_pose()
+{
+ if (gnss_) {
+ PoseWithCovarianceStamped pose = gnss_->get_pose();
+ pose.pose.covariance = gnss_particle_covariance_;
+ return pose;
+ }
+ throw ServiceException(
+ Initialize::Service::Response::ERROR_GNSS_SUPPORT, "GNSS is not supported.");
+}
diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp
new file mode 100644
index 0000000000000..a66d9d1612218
--- /dev/null
+++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp
@@ -0,0 +1,60 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
+#define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
+
+#include
+#include
+#include
+
+#include
+
+#include
+
+class StopCheckModule;
+class NdtModule;
+class GnssModule;
+
+class PoseInitializer : public rclcpp::Node
+{
+public:
+ PoseInitializer();
+ ~PoseInitializer();
+
+private:
+ using ServiceException = component_interface_utils::ServiceException;
+ using Initialize = localization_interface::Initialize;
+ using State = localization_interface::InitializationState;
+ using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+
+ rclcpp::CallbackGroup::SharedPtr group_srv_;
+ rclcpp::Publisher::SharedPtr pub_reset_;
+ component_interface_utils::Publisher::SharedPtr pub_state_;
+ component_interface_utils::Service::SharedPtr srv_initialize_;
+ State::Message state_;
+ std::array output_pose_covariance_;
+ std::array gnss_particle_covariance_;
+ std::unique_ptr gnss_;
+ std::unique_ptr ndt_;
+ std::unique_ptr stop_check_;
+ double stop_check_duration_;
+ void change_state(State::Message::_state_type state);
+ void on_initialize(
+ const Initialize::Service::Request::SharedPtr req,
+ const Initialize::Service::Response::SharedPtr res);
+ PoseWithCovarianceStamped get_gnss_pose();
+};
+
+#endif // POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
diff --git a/localization/pose_initializer/src/pose_initializer_node.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp
similarity index 91%
rename from localization/pose_initializer/src/pose_initializer_node.cpp
rename to localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp
index a46969c7711a1..c5b31c4e37ccd 100644
--- a/localization/pose_initializer/src/pose_initializer_node.cpp
+++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp
@@ -12,9 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "pose_initializer/pose_initializer_core.hpp"
-
-#include
+#include "pose_initializer_core.hpp"
#include
diff --git a/localization/pose_initializer/src/pose_initializer/stop_check_module.cpp b/localization/pose_initializer/src/pose_initializer/stop_check_module.cpp
new file mode 100644
index 0000000000000..683b161e14165
--- /dev/null
+++ b/localization/pose_initializer/src/pose_initializer/stop_check_module.cpp
@@ -0,0 +1,30 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "stop_check_module.hpp"
+
+StopCheckModule::StopCheckModule(rclcpp::Node * node, double buffer_duration)
+: VehicleStopCheckerBase(node, buffer_duration)
+{
+ sub_twist_ = node->create_subscription(
+ "stop_check_twist", 1, std::bind(&StopCheckModule::on_twist, this, std::placeholders::_1));
+}
+
+void StopCheckModule::on_twist(TwistWithCovarianceStamped::ConstSharedPtr msg)
+{
+ TwistStamped twist;
+ twist.header = msg->header;
+ twist.twist = msg->twist.twist;
+ addTwist(twist);
+}
diff --git a/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp b/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp
new file mode 100644
index 0000000000000..86c68b4a2dbb4
--- /dev/null
+++ b/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp
@@ -0,0 +1,36 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef POSE_INITIALIZER__STOP_CHECK_MODULE_HPP_
+#define POSE_INITIALIZER__STOP_CHECK_MODULE_HPP_
+
+#include
+#include
+
+#include
+#include
+
+class StopCheckModule : public motion_utils::VehicleStopCheckerBase
+{
+public:
+ StopCheckModule(rclcpp::Node * node, double buffer_duration);
+
+private:
+ using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped;
+ using TwistStamped = geometry_msgs::msg::TwistStamped;
+ rclcpp::Subscription::SharedPtr sub_twist_;
+ void on_twist(TwistWithCovarianceStamped::ConstSharedPtr msg);
+};
+
+#endif // POSE_INITIALIZER__STOP_CHECK_MODULE_HPP_
diff --git a/localization/pose_initializer/src/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer_core.cpp
deleted file mode 100644
index ae5deeb74c683..0000000000000
--- a/localization/pose_initializer/src/pose_initializer_core.cpp
+++ /dev/null
@@ -1,246 +0,0 @@
-// Copyright 2020 Autoware Foundation
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "pose_initializer/pose_initializer_core.hpp"
-
-#include "pose_initializer/copy_vector_to_array.hpp"
-
-#include
-#ifdef ROS_DISTRO_GALACTIC
-#include
-#else
-#include
-#endif
-
-#include
-#include
-#include
-#include
-#include
-
-double getGroundHeight(const pcl::PointCloud::Ptr pcdmap, const tf2::Vector3 & point)
-{
- constexpr double radius = 1.0 * 1.0;
- const double x = point.getX();
- const double y = point.getY();
-
- double height = INFINITY;
- for (const auto & p : pcdmap->points) {
- const double dx = x - p.x;
- const double dy = y - p.y;
- const double sd = (dx * dx) + (dy * dy);
- if (sd < radius) {
- height = std::min(height, static_cast(p.z));
- }
- }
- return std::isfinite(height) ? height : point.getZ();
-}
-
-PoseInitializer::PoseInitializer()
-: Node("pose_initializer"), tf2_listener_(tf2_buffer_), map_frame_("map")
-{
- enable_gnss_callback_ = this->declare_parameter("enable_gnss_callback", true);
-
- const std::vector initialpose_particle_covariance =
- this->declare_parameter>("initialpose_particle_covariance");
- CopyVectorToArray(initialpose_particle_covariance, initialpose_particle_covariance_);
-
- const std::vector gnss_particle_covariance =
- this->declare_parameter>("gnss_particle_covariance");
- CopyVectorToArray(gnss_particle_covariance, gnss_particle_covariance_);
-
- const std::vector service_particle_covariance =
- this->declare_parameter>("service_particle_covariance");
- CopyVectorToArray(service_particle_covariance, service_particle_covariance_);
-
- const std::vector output_pose_covariance =
- this->declare_parameter>("output_pose_covariance");
- CopyVectorToArray(output_pose_covariance, output_pose_covariance_);
-
- // We can't use _1 because pcl leaks an alias to boost::placeholders::_1, so it would be ambiguous
- initial_pose_sub_ = this->create_subscription(
- "initialpose", 10,
- std::bind(&PoseInitializer::callbackInitialPose, this, std::placeholders::_1));
- map_points_sub_ = this->create_subscription(
- "pointcloud_map", rclcpp::QoS{1}.transient_local(),
- std::bind(&PoseInitializer::callbackMapPoints, this, std::placeholders::_1));
- gnss_pose_sub_ = this->create_subscription(
- "gnss_pose_cov", 1,
- std::bind(&PoseInitializer::callbackGNSSPoseCov, this, std::placeholders::_1));
- pose_initialization_request_sub_ =
- this->create_subscription(
- "pose_initialization_request", rclcpp::QoS{1}.transient_local(),
- std::bind(&PoseInitializer::callbackPoseInitializationRequest, this, std::placeholders::_1));
-
- initial_pose_pub_ =
- this->create_publisher("initialpose3d", 10);
-
- initialize_pose_service_group_ =
- create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
- ndt_client_ = this->create_client(
- "ndt_align_srv", rmw_qos_profile_services_default, initialize_pose_service_group_);
- while (!ndt_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
- RCLCPP_INFO(get_logger(), "Waiting for service...");
- }
-
- initialize_pose_service_ =
- this->create_service(
- "service/initialize_pose", std::bind(
- &PoseInitializer::serviceInitializePose, this,
- std::placeholders::_1, std::placeholders::_2));
-
- initialize_pose_auto_service_ =
- this->create_service(
- "service/initialize_pose_auto", std::bind(
- &PoseInitializer::serviceInitializePoseAuto, this,
- std::placeholders::_1, std::placeholders::_2));
-}
-
-void PoseInitializer::callbackMapPoints(
- sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
-{
- std::string map_frame_ = map_points_msg_ptr->header.frame_id;
- map_ptr_ = pcl::PointCloud::Ptr(new pcl::PointCloud);
- pcl::fromROSMsg(*map_points_msg_ptr, *map_ptr_);
-}
-
-void PoseInitializer::serviceInitializePose(
- const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
- tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res)
-{
- enable_gnss_callback_ = false; // get only first topic
-
- auto add_height_pose_msg_ptr = std::make_shared();
- getHeight(req->pose_with_covariance, add_height_pose_msg_ptr);
-
- add_height_pose_msg_ptr->pose.covariance = service_particle_covariance_;
-
- res->success = callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
-}
-
-void PoseInitializer::callbackInitialPose(
- geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr)
-{
- enable_gnss_callback_ = false; // get only first topic
-
- auto add_height_pose_msg_ptr = std::make_shared();
- getHeight(*pose_cov_msg_ptr, add_height_pose_msg_ptr);
-
- add_height_pose_msg_ptr->pose.covariance = initialpose_particle_covariance_;
-
- callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
-}
-
-// NOTE Still not usable callback
-void PoseInitializer::callbackGNSSPoseCov(
- geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr)
-{
- if (!enable_gnss_callback_) {
- return;
- }
-
- // TODO(YamatoAndo) check service is available
-
- auto add_height_pose_msg_ptr = std::make_shared();
- getHeight(*pose_cov_msg_ptr, add_height_pose_msg_ptr);
-
- add_height_pose_msg_ptr->pose.covariance = gnss_particle_covariance_;
-
- callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
-}
-
-void PoseInitializer::serviceInitializePoseAuto(
- const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr req,
- tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr res)
-{
- (void)req;
-
- RCLCPP_INFO(this->get_logger(), "Called Pose Initialize Service");
- enable_gnss_callback_ = true;
- res->status = tier4_api_utils::response_success();
-}
-
-void PoseInitializer::callbackPoseInitializationRequest(
- const tier4_localization_msgs::msg::PoseInitializationRequest::ConstSharedPtr request_msg_ptr)
-{
- RCLCPP_INFO(this->get_logger(), "Called Pose Initialize");
- enable_gnss_callback_ = request_msg_ptr->data;
-}
-
-bool PoseInitializer::getHeight(
- const geometry_msgs::msg::PoseWithCovarianceStamped & input_pose_msg,
- const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr output_pose_msg_ptr)
-{
- std::string fixed_frame = input_pose_msg.header.frame_id;
- tf2::Vector3 point(
- input_pose_msg.pose.pose.position.x, input_pose_msg.pose.pose.position.y,
- input_pose_msg.pose.pose.position.z);
-
- if (map_ptr_) {
- tf2::Transform transform{tf2::Quaternion{}, tf2::Vector3{}};
- try {
- const auto stamped = tf2_buffer_.lookupTransform(map_frame_, fixed_frame, tf2::TimePointZero);
- tf2::fromMsg(stamped.transform, transform);
- } catch (tf2::TransformException & exception) {
- RCLCPP_WARN_STREAM(get_logger(), "failed to lookup transform: " << exception.what());
- }
-
- point = transform * point;
- point.setZ(getGroundHeight(map_ptr_, point));
- point = transform.inverse() * point;
- }
-
- *output_pose_msg_ptr = input_pose_msg;
- output_pose_msg_ptr->pose.pose.position.x = point.getX();
- output_pose_msg_ptr->pose.pose.position.y = point.getY();
- output_pose_msg_ptr->pose.pose.position.z = point.getZ();
-
- return true;
-}
-
-bool PoseInitializer::callAlignServiceAndPublishResult(
- const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr input_pose_msg)
-{
- if (request_id_ != response_id_) {
- RCLCPP_ERROR(get_logger(), "Did not receive response for previous NDT Align Server call");
- return false;
- }
- auto req = std::make_shared();
- req->pose_with_covariance = *input_pose_msg;
- req->seq = ++request_id_;
-
- RCLCPP_INFO(get_logger(), "call NDT Align Server");
- auto result = ndt_client_->async_send_request(req).get();
-
- if (!result->success) {
- RCLCPP_INFO(get_logger(), "failed NDT Align Server");
- response_id_ = result->seq;
- return false;
- }
-
- RCLCPP_INFO(get_logger(), "called NDT Align Server");
- response_id_ = result->seq;
- // NOTE temporary cov
- geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov = result->pose_with_covariance;
- pose_with_cov.pose.covariance[0] = 1.0;
- pose_with_cov.pose.covariance[1 * 6 + 1] = 1.0;
- pose_with_cov.pose.covariance[2 * 6 + 2] = 0.01;
- pose_with_cov.pose.covariance[3 * 6 + 3] = 0.01;
- pose_with_cov.pose.covariance[4 * 6 + 4] = 0.01;
- pose_with_cov.pose.covariance[5 * 6 + 5] = 0.2;
- initial_pose_pub_->publish(pose_with_cov);
- enable_gnss_callback_ = false;
-
- return true;
-}
diff --git a/localization/pose_initializer/test/test_copy_vector_to_array.cpp b/localization/pose_initializer/test/test_copy_vector_to_array.cpp
index aa0dd87bc82f6..65b1d7e7d2711 100644
--- a/localization/pose_initializer/test/test_copy_vector_to_array.cpp
+++ b/localization/pose_initializer/test/test_copy_vector_to_array.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "pose_initializer/copy_vector_to_array.hpp"
+#include "../src/pose_initializer/copy_vector_to_array.hpp"
#include
@@ -20,7 +20,7 @@ TEST(CopyVectorToArray, CopyAllElements)
{
const std::vector vector{0, 1, 2, 3, 4};
std::array array;
- CopyVectorToArray(vector, array);
+ copy_vector_to_array(vector, array);
EXPECT_THAT(array, testing::ElementsAre(0, 1, 2, 3, 4));
}
@@ -29,7 +29,7 @@ TEST(CopyVectorToArray, CopyZeroElements)
const std::vector vector{};
// just confirm that this works
std::array array;
- CopyVectorToArray(vector, array);
+ copy_vector_to_array(vector, array);
}
TEST(CopyVectorToArray, ThrowsInvalidArgumentIfMoreElementsExpected)
@@ -37,7 +37,7 @@ TEST(CopyVectorToArray, ThrowsInvalidArgumentIfMoreElementsExpected)
auto f = [] {
const std::vector vector{0, 1, 2, 3, 4};
std::array array;
- CopyVectorToArray(vector, array);
+ copy_vector_to_array(vector, array);
};
EXPECT_THROW(
diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md
index c12d13cb0177e..d70aca794dc07 100644
--- a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md
+++ b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md
@@ -17,7 +17,7 @@ The purpose of this simulator is for the integration test of planning and contro
### input
-- /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose
+- input/initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose
- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle
- input/manual_ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : manual target command to drive a vehicle (used when control_mode_request = Manual)
- input/gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command.
@@ -40,17 +40,17 @@ The purpose of this simulator is for the integration test of planning and contro
### Common Parameters
-| Name | Type | Description | Default value |
-| :-------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------ | :------------------- |
-| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" |
-| origin_frame_id | string | set to the frame_id in output tf | "odom" |
-| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" |
-| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true |
-| pos_noise_stddev | double | Standard deviation for position noise | 0.01 |
-| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 |
-| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 |
-| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 |
-| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 |
+| Name | Type | Description | Default value |
+| :-------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------------- |
+| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" |
+| origin_frame_id | string | set to the frame_id in output tf | "odom" |
+| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `input/initialpose` topic is published. | "INITIAL_POSE_TOPIC" |
+| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true |
+| pos_noise_stddev | double | Standard deviation for position noise | 0.01 |
+| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 |
+| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 |
+| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 |
+| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 |
### Vehicle Model Parameters
diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
index ae96587af474e..43a0fe25888c7 100644
--- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
+++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
@@ -53,6 +53,7 @@ def launch_setup(context, *args, **kwargs):
},
],
remappings=[
+ ("input/initialpose", "/initialpose3d"),
("input/ackermann_control_command", "/control/command/control_cmd"),
("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
("input/gear_command", "/control/command/gear_cmd"),
@@ -70,7 +71,6 @@ def launch_setup(context, *args, **kwargs):
("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
("output/control_mode_report", "/vehicle/status/control_mode"),
- ("/initialpose", "/initialpose"),
],
)
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
index 2d2d55d4fb294..ad013e1846f8a 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -86,7 +86,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
using std::placeholders::_2;
sub_init_pose_ = create_subscription(
- "/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1));
+ "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1));
sub_ackermann_cmd_ = create_subscription(
"input/ackermann_control_command", QoS{1},
[this](const AckermannControlCommand::SharedPtr msg) { current_ackermann_cmd_ = *msg; });
diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
index fea6cd16db9d0..9e770bd52bbb1 100644
--- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
+++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
@@ -52,8 +52,9 @@ class PubSubNode : public rclcpp::Node
[this](const Odometry::SharedPtr msg) { current_odom_ = msg; });
pub_ackermann_command_ =
create_publisher("input/ackermann_control_command", rclcpp::QoS{1});
- pub_initialpose_ = create_publisher("/initialpose", rclcpp::QoS{1});
- pub_gear_cmd_ = create_publisher("/input/gear_command", rclcpp::QoS{1});
+ pub_initialpose_ =
+ create_publisher("input/initialpose", rclcpp::QoS{1});
+ pub_gear_cmd_ = create_publisher("input/gear_command", rclcpp::QoS{1});
}
rclcpp::Subscription::SharedPtr current_odom_sub_;
diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml
index bfbb6beeaf054..9113e8a1f1206 100644
--- a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml
+++ b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml
@@ -25,9 +25,9 @@
"/map/vector_map",
"/map/pointcloud_map",
"/perception/obstacle_segmentation/pointcloud",
+ "/perception/object_recognition/objects",
"/initialpose3d",
"/localization/pose_twist_fusion_filter/pose",
- "/perception/object_recognition/objects",
"/planning/mission_planning/route",
"/planning/scenario_planning/trajectory",
"/control/trajectory_follower/control_cmd",
@@ -59,18 +59,6 @@
type: "sensor_msgs/msg/PointCloud2"
best_effort: True
- /initialpose3d:
- module: "localization"
- timeout: 0.0
- warn_rate: 0.0
- type: "geometry_msgs/msg/PoseWithCovarianceStamped"
-
- /localization/pose_twist_fusion_filter/pose:
- module: "localization"
- timeout: 1.0
- warn_rate: 5.0
- type: "geometry_msgs/msg/PoseStamped"
-
# Can be both with feature array or without
# In prediction.launch.xml this is set to without
/perception/object_recognition/objects:
@@ -83,6 +71,18 @@
# to be defined for a topic. By default this is set to not use have features.
# type: ["tier4_perception_msgs/msg/DynamicObjectArray", "tier4_perception_msgs/msg/DynamicObjectWithFeatureArray"]
+ /initialpose3d:
+ module: "localization"
+ timeout: 0.0
+ warn_rate: 0.0
+ type: "geometry_msgs/msg/PoseWithCovarianceStamped"
+
+ /localization/pose_twist_fusion_filter/pose:
+ module: "localization"
+ timeout: 1.0
+ warn_rate: 5.0
+ type: "geometry_msgs/msg/PoseStamped"
+
/planning/mission_planning/route:
module: "planning"
timeout: 0.0
diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml
index b20ffec096e03..51fbe99dd9bc3 100644
--- a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml
+++ b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml
@@ -24,7 +24,7 @@
names: [
"/map/vector_map",
"/perception/object_recognition/objects",
- "/initialpose2d",
+ "/initialpose3d",
"/planning/mission_planning/route",
"/planning/scenario_planning/trajectory",
"/control/trajectory_follower/control_cmd",
@@ -48,7 +48,7 @@
warn_rate: 5.0
type: "autoware_auto_perception_msgs/msg/PredictedObjects"
- /initialpose2d:
+ /initialpose3d:
module: "localization"
timeout: 0.0
warn_rate: 0.0