From 44fb0fd3b0da4cf39da19216cd5dada4dbcecbb0 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Fri, 22 Sep 2023 17:56:06 +0900 Subject: [PATCH 1/2] feat(ar_tag_based_localizer): split the package `ar_tag_based_localizer` (#5043) * Fix package name Signed-off-by: Shintaro SAKODA * Removed utils Signed-off-by: Shintaro SAKODA * Renamed tag_tf_caster to landmark_tf_caster Signed-off-by: Shintaro SAKODA * Updated node_diagram Signed-off-by: Shintaro SAKODA * Fixed documents Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Fixed the directory name Signed-off-by: Shintaro SAKODA * Fixed to split packages Signed-off-by: Shintaro SAKODA * Removed unused package dependency Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Fixed directory structure Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Fixed ArTagDetector to ArTagBasedLocalizer Signed-off-by: Shintaro SAKODA --------- Signed-off-by: Shintaro SAKODA Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ar_tag_based_localizer.launch.xml | 4 +- launch/tier4_localization_launch/package.xml | 2 +- .../include/ar_tag_based_localizer/utils.hpp | 66 ---------- .../launch/tag_tf_caster.launch.xml | 12 -- .../ar_tag_based_localizer/src/utils.cpp | 116 ----------------- .../landmark_based_localizer/README.md | 123 ++++++++++++++++++ .../ar_tag_based_localizer/CMakeLists.txt | 16 --- .../ar_tag_based_localizer/README.md | 109 +--------------- .../config/ar_tag_based_localizer.param.yaml | 0 .../doc_image/ar_tag_image.png | Bin .../doc_image/sample_result.png | Bin .../doc_image/sample_result_in_awsim.png | Bin .../ar_tag_based_localizer_core.hpp | 1 + .../launch/ar_tag_based_localizer.launch.xml | 0 .../ar_tag_based_localizer/package.xml | 2 - .../src/ar_tag_based_localizer_core.cpp | 48 ++++++- .../src/ar_tag_based_localizer_node.cpp | 0 .../lanelet2_data_structure.drawio.svg | 6 +- .../doc_image/node_diagram.drawio.svg | 18 +-- .../doc_image/principle.png | Bin .../landmark_tf_caster/CMakeLists.txt | 30 +++++ .../config/landmark_tf_caster.param.yaml} | 0 .../landmark_tf_caster_core.hpp} | 10 +- .../launch/landmark_tf_caster.launch.xml | 12 ++ .../landmark_tf_caster/package.xml | 25 ++++ .../src/landmark_tf_caster_core.cpp} | 10 +- .../src/landmark_tf_caster_node.cpp} | 4 +- 27 files changed, 267 insertions(+), 347 deletions(-) delete mode 100644 localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp delete mode 100644 localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml delete mode 100644 localization/ar_tag_based_localizer/src/utils.cpp create mode 100644 localization/landmark_based_localizer/README.md rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/CMakeLists.txt (74%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/README.md (52%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml (100%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/doc_image/ar_tag_image.png (100%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/doc_image/sample_result.png (100%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png (100%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp (98%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml (100%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/package.xml (92%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp (89%) rename localization/{ => landmark_based_localizer}/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp (100%) rename localization/{ar_tag_based_localizer => landmark_based_localizer}/doc_image/lanelet2_data_structure.drawio.svg (91%) rename localization/{ar_tag_based_localizer => landmark_based_localizer}/doc_image/node_diagram.drawio.svg (85%) rename localization/{ar_tag_based_localizer => landmark_based_localizer}/doc_image/principle.png (100%) create mode 100644 localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt rename localization/{ar_tag_based_localizer/config/tag_tf_caster.param.yaml => landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml} (100%) rename localization/{ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp => landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp} (84%) create mode 100644 localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml create mode 100644 localization/landmark_based_localizer/landmark_tf_caster/package.xml rename localization/{ar_tag_based_localizer/src/tag_tf_caster_core.cpp => landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp} (93%) rename localization/{ar_tag_based_localizer/src/tag_tf_caster_node.cpp => landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp} (85%) diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 642edb4f68ab8..a767d2d253cff 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -14,9 +14,9 @@ - + - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index f00752d1c14fc..7991ffb0d8adf 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -14,13 +14,13 @@ ament_cmake_auto autoware_cmake - ar_tag_based_localizer automatic_pose_initializer eagleye_fix2pose eagleye_gnss_converter eagleye_rt ekf_localizer gyro_odometer + landmark_based_localizer ndt_scan_matcher pointcloud_preprocessor pose_initializer diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp deleted file mode 100644 index 06401dad8fdd1..0000000000000 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2023 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#ifndef AR_TAG_BASED_LOCALIZER__UTILS_HPP_ -#define AR_TAG_BASED_LOCALIZER__UTILS_HPP_ - -#include - -#include -#include - -/** - * @brief ros_camera_info_to_aruco_cam_params gets the camera intrinsics from a CameraInfo message - * and copies them to aruco's data structure - * @param cam_info - * @param use_rectified_parameters if true, the intrinsics are taken from cam_info.P and the - * distortion parameters are set to 0. Otherwise, cam_info.K and cam_info.D are taken. - * @return - */ -aruco::CameraParameters ros_camera_info_to_aruco_cam_params( - const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters); - -tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); - -#endif // AR_TAG_BASED_LOCALIZER__UTILS_HPP_ diff --git a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml b/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml deleted file mode 100644 index 6a9c3dcd17e2e..0000000000000 --- a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/localization/ar_tag_based_localizer/src/utils.cpp b/localization/ar_tag_based_localizer/src/utils.cpp deleted file mode 100644 index 9f582830280d9..0000000000000 --- a/localization/ar_tag_based_localizer/src/utils.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2023 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#include "ar_tag_based_localizer/utils.hpp" - -#include -#include - -aruco::CameraParameters ros_camera_info_to_aruco_cam_params( - const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters) -{ - cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - cv::Mat distortion_coeff(4, 1, CV_64FC1); - cv::Size size(static_cast(cam_info.width), static_cast(cam_info.height)); - - if (use_rectified_parameters) { - camera_matrix.setTo(0); - camera_matrix.at(0, 0) = cam_info.p[0]; - camera_matrix.at(0, 1) = cam_info.p[1]; - camera_matrix.at(0, 2) = cam_info.p[2]; - camera_matrix.at(0, 3) = cam_info.p[3]; - camera_matrix.at(1, 0) = cam_info.p[4]; - camera_matrix.at(1, 1) = cam_info.p[5]; - camera_matrix.at(1, 2) = cam_info.p[6]; - camera_matrix.at(1, 3) = cam_info.p[7]; - camera_matrix.at(2, 0) = cam_info.p[8]; - camera_matrix.at(2, 1) = cam_info.p[9]; - camera_matrix.at(2, 2) = cam_info.p[10]; - camera_matrix.at(2, 3) = cam_info.p[11]; - - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } - } else { - cv::Mat camera_matrix_from_k(3, 3, CV_64FC1, 0.0); - for (int i = 0; i < 9; ++i) { - camera_matrix_from_k.at(i % 3, i - (i % 3) * 3) = cam_info.k[i]; - } - camera_matrix_from_k.copyTo(camera_matrix(cv::Rect(0, 0, 3, 3))); - - if (cam_info.d.size() == 4) { - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = cam_info.d[i]; - } - } else { - RCLCPP_WARN( - rclcpp::get_logger("ar_tag_based_localizer"), - "length of camera_info D vector is not 4, assuming zero distortion..."); - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } - } - } - - return {camera_matrix, distortion_coeff, size}; -} - -tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker) -{ - cv::Mat rot(3, 3, CV_64FC1); - cv::Mat r_vec64; - marker.Rvec.convertTo(r_vec64, CV_64FC1); - cv::Rodrigues(r_vec64, rot); - cv::Mat tran64; - marker.Tvec.convertTo(tran64, CV_64FC1); - - tf2::Matrix3x3 tf_rot( - rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), - rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), - rot.at(2, 2)); - - tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); - - return tf2::Transform(tf_rot, tf_orig); -} diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md new file mode 100644 index 0000000000000..a8ebdc0c8f1e5 --- /dev/null +++ b/localization/landmark_based_localizer/README.md @@ -0,0 +1,123 @@ +# Landmark Based Localizer + +This directory contains packages for landmark-based localization. + +Landmarks are, for example + +- AR tags detected by camera +- Boards characterized by intensity detected by LiDAR + +etc. + +Since these landmarks are easy to detect and estimate pose, the ego pose can be calculated from the pose of the detected landmark if the pose of the landmark is written on the map in advance. + +Currently, landmarks are assumed to be flat. + +The following figure shows the principle of localization in the case of `ar_tag_based_localizer`. + +![principle](./doc_image/principle.png) + +This calculated ego pose is passed to the EKF, where it is fused with the twist information and used to estimate a more accurate ego pose. + +## Node diagram + +![node diagram](./doc_image/node_diagram.drawio.svg) + +### `landmark_tf_caster` node + +The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`. + +The `landmark_tf_caster` node publishes the TF from the map to the landmark. + +- Translation : The center of the four vertices of the landmark +- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. + +Users can define landmarks as Lanelet2 4-vertex polygons. +In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the landmark in that case is difficult to calculate. +So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the landmark will not publish tf_static. + +### Landmark based localizer packages + +- ar_tag_based_localizer +- etc. + +## Inputs / Outputs + +### `landmark_tf_caster` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :------------------------------------------- | :--------------- | +| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | + +#### Output + +| Name | Type | Description | +| :---------- | :------------------------------------- | :----------------- | +| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | + +## Map specifications + +For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_tf_caster` can interpret. + +The four vertices of a landmark are defined counterclockwise. + +The order of the four vertices is defined as follows. In the coordinate system of a landmark, + +- the x-axis is parallel to the vector from the first vertex to the second vertex +- the y-axis is parallel to the vector from the second vertex to the third vertex + +![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) + +### Example of `lanelet2_map.osm` + +The values provided below are placeholders. +Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. + +For example, when using the AR tag, it would look like this. + +```xml +... + + + + + + + + + + + + + + + + + + + + + + + + + + +... + + + + + + + + + + + + +... + +``` diff --git a/localization/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt similarity index 74% rename from localization/ar_tag_based_localizer/CMakeLists.txt rename to localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt index 89337b9bb966c..15aecd1f8ded9 100644 --- a/localization/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt @@ -14,13 +14,9 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) -# -# ar_tag_based_localizer -# ament_auto_add_executable(ar_tag_based_localizer src/ar_tag_based_localizer_node.cpp src/ar_tag_based_localizer_core.cpp - src/utils.cpp ) target_include_directories(ar_tag_based_localizer SYSTEM PUBLIC @@ -28,18 +24,6 @@ target_include_directories(ar_tag_based_localizer ) target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) -# -# tag_tf_caster -# -ament_auto_add_executable(tag_tf_caster - src/tag_tf_caster_node.cpp - src/tag_tf_caster_core.cpp -) -target_include_directories(tag_tf_caster - SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} -) - ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md similarity index 52% rename from localization/ar_tag_based_localizer/README.md rename to localization/landmark_based_localizer/ar_tag_based_localizer/README.md index 81bc11a7cbc9b..8c51febc6f60c 100644 --- a/localization/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -1,17 +1,12 @@ # AR Tag Based Localizer -**ArTagBasedLocalizer** is a vision-based localization package. +**ArTagBasedLocalizer** is a vision-based localization node. -This package uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. +This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. -This package includes two nodes. - -- `ar_tag_based_localizer` : A node that detects AR-Tags from camera images and publishes the pose of the ego vehicle. -- `tag_tf_caster` : A node that publishes the pose of the AR-Tags applied in Lanelet2 as `tf_static`. - ## Inputs / Outputs ### `ar_tag_based_localizer` node @@ -32,23 +27,9 @@ This package includes two nodes. | `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | | `tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | -### `tag_tf_caster` node - -#### Input - -| Name | Type | Description | -| :--------------------- | :------------------------------------------- | :--------------- | -| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | - -#### Output - -| Name | Type | Description | -| :---------- | :------------------------------------- | :----------------- | -| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | - ## How to launch -When launching Autoware, specify `artag` for `pose_source`. +When launching Autoware, set `artag` for `pose_source`. ```bash ros2 launch autoware_launch ... \ @@ -87,88 +68,6 @@ The pull request video below should also be helpful. -## Architecture - -![node diagram](./doc_image/node_diagram.drawio.svg) - ## Principle -### `ar_tag_based_localizer` node - -![principle](./doc_image/principle.png) - -### `tag_tf_caster` node - -The definitions of the tags written to the map are introduced in the next section. See `Map Specifications`. - -The `tag_tf_caster` node publishes the TF from the map to the tag. - -- Translation : The center of the four vertices of the tag -- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. - -Users can define tags as Lanelet2 4-vertex polygons. -In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the tag in that case is difficult to calculate. -So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the tag will not publish tf_static. - -## Map specifications - -For this package to function correctly, the pose of the AR-Tags must be specified in the Lanelet2 map format that Autoware can interpret. - -The four vertices of AR-Tag are defined counterclockwise. - -The order of the four vertices is defined as follows. In the coordinate system of AR-Tag, - -- the x-axis is parallel to the vector from the first vertex to the second vertex -- the y-axis is parallel to the vector from the second vertex to the third vertex - -![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) - -### example of `lanelet2_map.osm` - -The values provided below are placeholders. -Ensure to input the correct coordinates corresponding to the actual location where the AR-Tag is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. - -```xml -... - - - - - - - - - - - - - - - - - - - - - - - - - - -... - - - - - - - - - - - - -... - -``` +![principle](../doc_image/principle.png) diff --git a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml similarity index 100% rename from localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml rename to localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml diff --git a/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/ar_tag_image.png rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/sample_result.png rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp similarity index 98% rename from localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp index 65c1283a82728..6af9fefae23f7 100644 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -72,6 +72,7 @@ class ArTagBasedLocalizer : public rclcpp::Node void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); void publish_pose_as_base_link( const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id); + static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); // Parameters float marker_size_{}; diff --git a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml similarity index 100% rename from localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml rename to localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml diff --git a/localization/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml similarity index 92% rename from localization/ar_tag_based_localizer/package.xml rename to localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 4527d2ad9b086..e68fd21ed340a 100644 --- a/localization/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -12,11 +12,9 @@ autoware_cmake aruco - autoware_auto_mapping_msgs cv_bridge geometry_msgs image_transport - lanelet2_extension rclcpp sensor_msgs tf2_eigen diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp similarity index 89% rename from localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp index cf0236c1a1e48..f664ef6378a69 100644 --- a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -44,12 +44,12 @@ #include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" -#include "ar_tag_based_localizer/utils.hpp" - #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -208,7 +208,30 @@ void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & return; } - cam_param_ = ros_camera_info_to_aruco_cam_params(msg, true); + cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); + cv::Mat distortion_coeff(4, 1, CV_64FC1); + cv::Size size(static_cast(msg.width), static_cast(msg.height)); + + camera_matrix.setTo(0); + camera_matrix.at(0, 0) = msg.p[0]; + camera_matrix.at(0, 1) = msg.p[1]; + camera_matrix.at(0, 2) = msg.p[2]; + camera_matrix.at(0, 3) = msg.p[3]; + camera_matrix.at(1, 0) = msg.p[4]; + camera_matrix.at(1, 1) = msg.p[5]; + camera_matrix.at(1, 2) = msg.p[6]; + camera_matrix.at(1, 3) = msg.p[7]; + camera_matrix.at(2, 0) = msg.p[8]; + camera_matrix.at(2, 1) = msg.p[9]; + camera_matrix.at(2, 2) = msg.p[10]; + camera_matrix.at(2, 3) = msg.p[11]; + + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = 0; + } + + cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size); + cam_info_received_ = true; } @@ -324,3 +347,22 @@ void ArTagBasedLocalizer::publish_pose_as_base_link( pose_pub_->publish(pose_with_covariance_stamped); } + +tf2::Transform ArTagBasedLocalizer::aruco_marker_to_tf2(const aruco::Marker & marker) +{ + cv::Mat rot(3, 3, CV_64FC1); + cv::Mat r_vec64; + marker.Rvec.convertTo(r_vec64, CV_64FC1); + cv::Rodrigues(r_vec64, rot); + cv::Mat tran64; + marker.Tvec.convertTo(tran64, CV_64FC1); + + tf2::Matrix3x3 tf_rot( + rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), + rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), + rot.at(2, 2)); + + tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + + return tf2::Transform(tf_rot, tf_orig); +} diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp similarity index 100% rename from localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp diff --git a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg b/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg similarity index 91% rename from localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg rename to localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg index bc8ba297558bb..5895d8ef65073 100644 --- a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg +++ b/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg @@ -6,7 +6,7 @@ width="336px" height="336px" viewBox="-0.5 -0.5 336 336" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">5ZhNj5swEIZ/DVJ7wzYBcsymu+2lUtVU6tmCCVg1ODLOhuyvr1kMBDsROdBk1eQQ4ddfzDPj8SQeWRf1V0l3+XeRAvewn9Ye+eJhHC1i/d0Ix1Ygy6gVMsnSVkKDsGFvYETfqHuWQjUaqITgiu3GYiLKEhI10qiU4jAethV8vOuOZuAIm4RyV/3NUpW3aoyjQf8GLMu7nVG4bHsK2g02llQ5TcXhRCLPHllLIVT7VNRr4A27jks77+VCb/9iEkp1zYRg0c54pXxvjDMvpo6dtVLsyxSaCb5Hng45U7DZ0aTpPWj3ai1XBdctpB/NciAV1BffCfWW6ggBUYCSRz2kmxAaOCY6UAfrMLDux+SnnDuRGv9m/doDAv1gKFwgEjpEkINET9GxBtM4aLVrA3DL6gbhLHyCMR8cu3zwGTx4DjqRQ8cNmLvSIfiOdGKHDvnYdJB/QzpLh07wsejYJ+uWdBb+dCaGMl0115duJZxWFUvOJV9IndtrEsCJgYszBnaaBE4Vex0vf85qs8MPwfTGPd/YOpq+xa0Se5mAmXR6a1nrBP7EQorKDJSz0LsPequvcwty3FI7ftERp8aeqJQUf2AtuJBaKUXZRPSWcW5JlLOsbNypHQRaf2ril+lKY2U6Cpam/NJxGN/MW1EqUyuhcKZ0EVkHInIPBDkTL2SOA4Ed8sfHId/Xuscu5G8Hnvz/mQhbmZ7YpeO1qcheKPh3mSi4wi33vD2xXVvcsPJauFX7jHBO04enz9j7Zy5okQUtcqCFLrNwBmRuKf/2OPkVLS3uIbldgnV/Jqx+6vYvmn16kdrWz4/jh3jymkNoHjfo5vC/Spt/hz+nyPNf</diagram></mxfile>" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">5ZhNb6MwEIZ/DdLuDWwC9Nhm2+5hV1ophz1beAJWDY6Mk5D++jXFfNmJkgNNqk0OEbz+wPPMeDzg4WVRv0qyyX8LCtxDPq09/MNDCCeJ/m+EQytEKGyFTDLaSsEgrNg7GNE36pZRqCYdlRBcsc1UTEVZQqomGpFS7Kfd1oJPn7ohGTjCKiXcVf8yqvJWTVA86D+BZXn35CB6aFsK0nU2llQ5oWI/kvCzh5dSCNVeFfUSeMOu49KOeznR2i9MQqkuGRAu2hE7wrfGOLMwdeislWJbUmgG+B5+2udMwWpD0qZ1r92rtVwVXN8F+tJdgFnTDqSCeiSZBb2CKEDJg+7StUYGjomOoIO1H1j3ffIx504kxr9ZP/eAQF8YCieIRA6RwEGih+hYg/M4SLVpA3DN6gbhLHzCKR+UuHzQETxoDjqxQ8cNmJvSweiGdBKHDv7adAL/inQeHDrh16Jj76xr0ln45zMxlPSxOb70XcpJVbH0wuQLdHKguQBGBi6OGNhpEjhRbDfNi8esNk/4I5heSc83sbamb3GrxFamYAaNTy1rntA/M5EiMgPlTPThg97qy9wSOG6pHb/o8FJTT1RKijdYCi6kVkpRNhG9ZpxbEuEsKxt3ao+B1p+aYGW60ng0DQWjlJ/aDtOTeS1KZWqlIJopXcTWhojdDYGPxAueY0Mgh/zhfsj3te6hC/nrgcf/fyZCVqbHdul4aSqyJwo/LxOFF7jllqcnsmuLK1ZeC7dqnxHOOH14eo99/OaCFlvQYgda5DKLZkDmlvLv95NfgweLe4Svl2Dd14RfpKQFkW/fXqS29fv9+CE5e8wFwTxu0LfDd5U2/w4fp/DzPw==</diagram></mxfile>" > @@ -138,12 +138,12 @@
- AR Tag(Front) + Landmark(Front)
- AR Tag(Front) + Landmark(Front) diff --git a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg similarity index 85% rename from localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg rename to localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg index e41ca0ad019ce..120a1f2aa3391 100644 --- a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg +++ b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg @@ -6,20 +6,20 @@ width="721px" height="331px" viewBox="-0.5 -0.5 721 331" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1ZlLc5swEIB/jY/1WAiwfaydOD2k00yTmbYnRgYBmmDECPnVX1/JCDBIjomNkykHBy0SSN+u9qEM4Hy1e2Aoi7/TACcDaxTsBvBuYFnTiSt+pWBfCGygBBEjQSECteCZ/MVKOFLSNQlw3ujIKU04yZpCn6Yp9nlDhhij22a3kCbNr2Yowprg2UeJLv1FAh4X0ok1ruXfMIni8svAnRZPVqjsrFaSxyig2yMRvB/AOaOUF3er3Rwnkl3JpRi3OPG0mhjDKe8ywCoGbFCyVmtT8+L7crGMrtMAy/6jAZyFNOVKIcBS7TlNKDt0hqPDJeT6RNTcNphxvDsSqYk9YLrCnO1FF/XUchQkZSVQNbc18soi4iPcrpIhpeWoenMNQtwoFmYutsbl60/RfkGR+J2hXPCwhM1CINf6SIVpCCbsHDuSJEes5vN7ZyHmM8s5o6/46InrT/AyvC1d6Dbp2jrdqQGu3QNcR4P7g8eS3ujrmtOtWNUR3Az5r2I/5e9lu1jM5ya2E2sJXffGlvt5bOHYsKMXfEty7m0Jjz2fbhAjKPWxRhQHwsupJmU8phFNUXJfS2cCCdv/VsgPjT+yMRw7Zftud/z0bq9aJ6FyxCKsRFDJ5DzexMxwgjjZNL2xiZoa+kSJ+OxJ04f2pPmKnK6Zj9WoFvtqGp3UUfr5pjrISgaYt/CnNH2Lt9ULbzD+GN42dBq8rXHL0ItJaby1F1mjpuIsCG+muIlJcRnN8bu2UanHHeEHNQ4d1ZJ6BOq+1qFs7I8V2qvuCzpN3V+0/5Q6voyGQFzFoGttZNy0EQjfayNlRxqGOb5W/cDoRhHzOIq8pYz/XnJl1D8Vf/Rs4HD1E5ns9g6y9NAEHENscvqITcAEFb+Gl7NUUf5TWLrt/BR8JEujf/LRCjPkkTSk14SXfsL5R4UXLSo43cK5IU41XwQmTicfdIF/saFBfecV1sm1w6muC7v31KqzoU61lT6iFCeY6ytGeVaU6yHZyT0/yzAj4otY7l7xWlHf46daNBM+I0rFI1/gOAiqcloaaYDyuPIcuagjSBq90EwIoBAUOVj1946sRGm3SMhS+nlfIvACwsR0qFzvIkAcSb+fD/NN1I//mLSszZLZQduBmPyHPb1eLbZehMk6IfRyLvTvdzHGii/oapllgXNbL9EZgTE3lwFeYPBRzv+vgr4d2gGYGuzpZgHJ1o9MevNotiG6lNr7BLsxpjGJcmreCmUin0HBxcnM5xxZtK3H6I1uZz2uialMDd9dcp05uQDNVOdcpmOu3KpizVi5GQ3zfNg+mUL1fwAybmWul2ZMbvskxRkP+8qZRLM+By+61/9MgPf/AA==</diagram></mxfile>" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1VnLjpswFP2aLBsFG0iybF7TxVQdaSq1XSEHDFgDGBnn1a+vHQwE8ExIQiYqm+BjA/Y51/fhDOA83j8xlIbfqYejARh5+wFcDAAwgA3Ej0QOOWIDMwcCRjw1qAJeyV+swJFCN8TDWW0gpzTiJK2DLk0S7PIahhiju/own0b1r6YowC3g1UVRG/1FPB7m6ASMK/wbJkFYfNmwp3lPjIrBaiVZiDy6O4HgcgDnjFKe38X7OY4keQUv+XOrd3rLiTGc8C4PKCG2KNqotal58UOxWEY3iYfl+NEAznyacCWIAVR7TiPKjoPh6HgJvD0RNbctZhzvTyA1sSdMY8zZQQxRvcBSJCkrgaq5qygvsfCEblthSKkclG+uiBA3igs9L2aLl2eUeDFibwKdoUzwAWyxXkOu9ZkK0xCcsHPckSg64Wo+X1orMZ9Zxhl9wyc9tjvBa/++7MJxnV2zza4x0rBr9sCu1WL3Bw8lfaOvG053Ylkn7KbIfRMbKruU3NVqPteROwFraNt3Nl37ceTCsWZPr/iOZNzZER46Lt0iRlDi4hal2BN+TjUp4yENaIKiZYXOBCfs8Ftxfmz8kY3h2Crai/1p7+KgWu+yyhELsIKgwuQ8PuSZ4Qhxsq37Yx1r6tEXSsRnK+Nv6APNSf0VGd0wF6unGtyX0+gkR+Hp63KQWIaYj+hPaPIR36AXvo3x5/BtQqvGNxg3DD2fVIvv1ovAqC4cgPBuwk10wqU0wxdto0LHPeFHGYeWakkdDXVfaSgbh1NBe9U+Z6eu/VX7T8nxZTQ0xJU/dKuNjOs2AuGlNlIMpL6f4VvlN7RuFDGHo8BZywzAiW6M++8FoHY+cLz6CU1mcwcBTWyyNLHJ6iM2GTpS8Zt/PZcqzD+ES7uZoRqfyaXWP7koxgw5JPHpLeGln3D+WeGlFRWsbuFcE6fqLzImVicfdIV/MaFGvvOCdXLtcNrWwuw9tepsqNPWSkUxhSPM2ytGWZoX7D7Zyz0/SzEj4otY7l7xWlHh45cKmgmfESSiyxV0HIGyoJZG6qEsLD1HJgoJkgQ/aSoAKIA8Byt/FyQOxFIispZ+3pUUOB5hYjpUrnflIY6k38+G2Tbox39MGtYGZHbQdCA6/2FOb5fFbFdhsk7wnYwL/d0uxljya3S1zKLAua+X6EyBNjePVKXvCC5clPH/q65vxnfDmGqM6m5RyWyfnPTm1kxNiCkkfIDxaHOZSHk2J0apSGqQd3VG85iDi6b1aF3S/azH1nEq88OL664zxxdGPd85l+7oy7eyYtOWb91C9QPTpuYRILw2bbKbxynWeNhX4iSa1XF4Prz6UwEu/wE=</diagram></mxfile>" > - - + + - AR Tag Based - Localizer + Landmark Based + Localizer - + - Other Autoware - packages + Other Autoware + packages @@ -74,7 +74,7 @@ - /tag_tf_caster + /landmark_tf_caster diff --git a/localization/ar_tag_based_localizer/doc_image/principle.png b/localization/landmark_based_localizer/doc_image/principle.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/principle.png rename to localization/landmark_based_localizer/doc_image/principle.png diff --git a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt new file mode 100644 index 0000000000000..47899d716e411 --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(landmark_tf_caster) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +ament_auto_add_executable(landmark_tf_caster + src/landmark_tf_caster_node.cpp + src/landmark_tf_caster_core.cpp +) +target_include_directories(landmark_tf_caster + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml b/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml similarity index 100% rename from localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml rename to localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp similarity index 84% rename from localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp rename to localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp index 5b10fe5b1dc0f..0eb6706a0b5b0 100644 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ -#define AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ +#ifndef LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ +#define LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ #include @@ -26,10 +26,10 @@ #include -class TagTfCaster : public rclcpp::Node +class LandmarkTfCaster : public rclcpp::Node { public: - TagTfCaster(); + LandmarkTfCaster(); private: void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); @@ -46,4 +46,4 @@ class TagTfCaster : public rclcpp::Node rclcpp::Subscription::SharedPtr map_bin_sub_; }; -#endif // AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ +#endif // LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ diff --git a/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml new file mode 100644 index 0000000000000..eeaba555cb6f5 --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_tf_caster/package.xml new file mode 100644 index 0000000000000..9740e26e2d04a --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/package.xml @@ -0,0 +1,25 @@ + + + + landmark_tf_caster + 0.0.0 + The landmark_tf_caster package + Shintaro Sakoda + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake + autoware_cmake + + autoware_auto_mapping_msgs + geometry_msgs + lanelet2_extension + rclcpp + tf2_eigen + tf2_geometry_msgs + tf2_ros + + + ament_cmake + + diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp similarity index 93% rename from localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp rename to localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp index 427bf513058d4..801e036415326 100644 --- a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" +#include "landmark_tf_caster/landmark_tf_caster_core.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" @@ -26,7 +26,7 @@ #include #endif -TagTfCaster::TagTfCaster() : Node("tag_tf_caster") +LandmarkTfCaster::LandmarkTfCaster() : Node("landmark_tf_caster") { // Parameters volume_threshold_ = this->declare_parameter("volume_threshold", 1e-5); @@ -38,11 +38,11 @@ TagTfCaster::TagTfCaster() : Node("tag_tf_caster") // Subscribers map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&TagTfCaster::map_bin_callback, this, std::placeholders::_1)); + std::bind(&LandmarkTfCaster::map_bin_callback, this, std::placeholders::_1)); RCLCPP_INFO(this->get_logger(), "finish setup"); } -void TagTfCaster::map_bin_callback( +void LandmarkTfCaster::map_bin_callback( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) { RCLCPP_INFO_STREAM(this->get_logger(), "msg->format_version: " << msg->format_version); @@ -60,7 +60,7 @@ void TagTfCaster::map_bin_callback( } } -void TagTfCaster::publish_tf(const lanelet::Polygon3d & poly) +void LandmarkTfCaster::publish_tf(const lanelet::Polygon3d & poly) { // Get marker_id const std::string marker_id = poly.attributeOr("marker_id", "none"); diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp similarity index 85% rename from localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp rename to localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp index ce83c572d6a6e..4c34e593e7552 100644 --- a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" +#include "landmark_tf_caster/landmark_tf_caster_core.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); } From b58c009ce5875b73cf385884f3eafe75b983c8a9 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 <129915538+TaikiYamada4@users.noreply.github.com> Date: Fri, 22 Sep 2023 18:02:39 +0900 Subject: [PATCH 2/2] docs(ndt_scan_matcher): update readme (#5074) Updated README by adding ndt_base_frame in the parameters list Signed-off-by: TaikiYamada4 --- localization/ndt_scan_matcher/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index fcbe4804ca7d2..c5a14d94ebed3 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -57,6 +57,7 @@ One optional function is regularization. Please see the regularization chapter i | Name | Type | Description | | --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- | | `base_frame` | string | Vehicle reference frame | +| `ndt_base_frame` | string | NDT reference frame | | `input_sensor_points_queue_size` | int | Subscriber queue size | | `trans_epsilon` | double | The maximum difference between two consecutive transformations in order to consider convergence | | `step_size` | double | The newton line search maximum step length |