Skip to content

Commit

Permalink
feat!: replace autoware_auto_msgs with autoware_msgs for localization…
Browse files Browse the repository at this point in the history
… modules (#7243)

Signed-off-by: Ryohsuke Mitsudome <[email protected]>
Co-authored-by: Cynthia Liu <[email protected]>
Co-authored-by: NorahXiong <[email protected]>
Co-authored-by: beginningfan <[email protected]>
  • Loading branch information
4 people authored and KhalilSelyan committed Jul 22, 2024
1 parent ca0ccc6 commit d5e59a5
Show file tree
Hide file tree
Showing 24 changed files with 54 additions and 54 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ The positions and orientations of the AR-Tags are assumed to be written in the L

| Name | Type | Description |
| :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 |
| `~/input/lanelet2_map` | `autoware_map_msgs::msg::LaneletMapBin` | Data of lanelet2 |
| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image |
| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info |
| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. |
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options)
Subscribers
*/
using std::placeholders::_1;
map_bin_sub_ = this->create_subscription<HADMapBin>(
map_bin_sub_ = this->create_subscription<LaneletMapBin>(
"~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal),
std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1));

Expand Down Expand Up @@ -140,7 +140,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options)
RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!");
}

void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg)
void ArTagBasedLocalizer::map_bin_callback(const LaneletMapBin::ConstSharedPtr & msg)
{
landmark_manager_.parse_landmarks(msg, "apriltag_16h5");
const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@

class ArTagBasedLocalizer : public rclcpp::Node
{
using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
using Image = sensor_msgs::msg::Image;
using CameraInfo = sensor_msgs::msg::CameraInfo;
using Pose = geometry_msgs::msg::Pose;
Expand All @@ -86,7 +86,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

private:
void map_bin_callback(const HADMapBin::ConstSharedPtr & msg);
void map_bin_callback(const LaneletMapBin::ConstSharedPtr & msg);
void image_callback(const Image::ConstSharedPtr & msg);
void cam_info_callback(const CameraInfo::ConstSharedPtr & msg);
void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg);
Expand All @@ -106,7 +106,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
std::unique_ptr<tf2_ros::TransformListener> tf_listener_;

// Subscribers
rclcpp::Subscription<HADMapBin>::SharedPtr map_bin_sub_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr map_bin_sub_;
rclcpp::Subscription<Image>::SharedPtr image_sub_;
rclcpp::Subscription<CameraInfo>::SharedPtr cam_info_sub_;
rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr ekf_pose_sub_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include <rclcpp/rclcpp.hpp>

#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
#include "autoware_map_msgs/msg/lanelet_map_bin.hpp"
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
Expand All @@ -41,7 +41,7 @@ class LandmarkManager
{
public:
void parse_landmarks(
const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg,
const std::string & target_subtype);

[[nodiscard]] std::vector<landmark_manager::Landmark> get_landmarks() const;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@

<build_depend>eigen</build_depend>

<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>geometry_msgs</depend>
<depend>lanelet2_extension</depend>
<depend>localization_util</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ namespace landmark_manager
{

void LandmarkManager::parse_landmarks(
const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg,
const std::string & target_subtype)
{
std::vector<lanelet::Polygon3d> landmarks =
Expand Down
2 changes: 1 addition & 1 deletion localization/pose_estimator_arbiter/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ For switching rule:

| Name | Type | Description |
| ----------------------------- | ------------------------------------------------------------ | --------------------------------- |
| `/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map |
| `/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map |
| `/input/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | localization final output |
| `/input/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | localization initialization state |

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

Expand All @@ -29,7 +29,7 @@ namespace pose_estimator_arbiter::rule_helper
{
class PoseEstimatorArea
{
using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using HADMapBin = autoware_map_msgs::msg::LaneletMapBin;
using Marker = visualization_msgs::msg::Marker;
using MarkerArray = visualization_msgs::msg::MarkerArray;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ VectorMapBasedRule::VectorMapBasedRule(

// Register callback
shared_data_->vector_map.register_callback(
[this](autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -> void {
[this](autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) -> void {
pose_estimator_area_->init(msg);
});

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@

#include <lanelet2_extension/utility/message_conversion.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>

#include <boost/geometry/geometry.hpp>

Expand Down Expand Up @@ -59,7 +59,7 @@ TEST_F(MockNode, poseEstimatorArea)
lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
lanelet_map->add(create_polygon3d());

using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using HADMapBin = autoware_map_msgs::msg::LaneletMapBin;
using Point = geometry_msgs::msg::Point;
HADMapBin msg;
lanelet::utils::conversion::toBinMsg(lanelet_map, &msg);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@

#include <lanelet2_extension/utility/message_conversion.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>

#include <gtest/gtest.h>
#include <lanelet2_core/LaneletMap.h>
Expand Down Expand Up @@ -71,7 +71,7 @@ TEST_F(MockNode, vectorMapBasedRule)
lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
lanelet_map->add(create_polygon3d());

using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using HADMapBin = autoware_map_msgs::msg::LaneletMapBin;
HADMapBin msg;
lanelet::utils::conversion::toBinMsg(lanelet_map, &msg);

Expand Down
2 changes: 1 addition & 1 deletion localization/pose_estimator_arbiter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
<build_depend>autoware_cmake</build_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>diagnostic_msgs</depend>
<depend>geometry_msgs</depend>
<depend>lanelet2_extension</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ class PoseEstimatorArbiter : public rclcpp::Node
using Image = sensor_msgs::msg::Image;
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using HADMapBin = autoware_map_msgs::msg::LaneletMapBin;
using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
#define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_

#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
Expand Down Expand Up @@ -77,7 +77,7 @@ struct SharedData
using Image = sensor_msgs::msg::Image;
using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using HADMapBin = autoware_map_msgs::msg::LaneletMapBin;
using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;

SharedData() {}
Expand Down
14 changes: 7 additions & 7 deletions localization/yabloc/yabloc_common/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,10 @@ It estimates the height and tilt of the ground from lanelet2.

#### Input

| Name | Type | Description |
| ------------------ | -------------------------------------------- | ------------------- |
| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map |
| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose |
| Name | Type | Description |
| ------------------ | --------------------------------------- | ------------------- |
| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map |
| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose |

#### Output

Expand All @@ -44,9 +44,9 @@ This node extracts the elements related to the road surface markings and yabloc

#### Input

| Name | Type | Description |
| ------------------ | -------------------------------------------- | ----------- |
| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map |
| Name | Type | Description |
| ------------------ | --------------------------------------- | ----------- |
| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map |

#### Output

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include <signal_processing/lowpass_filter_1d.hpp>
#include <yabloc_common/ground_plane.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
Expand All @@ -44,7 +44,7 @@ class GroundServer : public rclcpp::Node
{
public:
using GroundPlane = common::GroundPlane;
using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;

using Pose = geometry_msgs::msg::Pose;
using PoseStamped = geometry_msgs::msg::PoseStamped;
Expand All @@ -65,7 +65,7 @@ class GroundServer : public rclcpp::Node
const int K;

// Subscriber
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<PoseStamped>::SharedPtr sub_pose_stamped_;
rclcpp::Subscription<PoseCovStamped>::SharedPtr sub_initial_pose_;
// Publisher
Expand All @@ -86,7 +86,7 @@ class GroundServer : public rclcpp::Node
std::vector<int> last_indices_;

// Callback
void on_map(const HADMapBin & msg);
void on_map(const LaneletMapBin & msg);
void on_initial_pose(const PoseCovStamped & msg);
void on_pose_stamped(const PoseStamped & msg);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
Expand All @@ -35,7 +35,7 @@ class Ll2Decomposer : public rclcpp::Node
{
public:
using Cloud2 = sensor_msgs::msg::PointCloud2;
using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
using Marker = visualization_msgs::msg::Marker;
using MarkerArray = visualization_msgs::msg::MarkerArray;

Expand All @@ -47,12 +47,12 @@ class Ll2Decomposer : public rclcpp::Node
rclcpp::Publisher<Cloud2>::SharedPtr pub_bounding_box_;
rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;

rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;
std::set<std::string> road_marking_labels_;
std::set<std::string> sign_board_labels_;
std::set<std::string> bounding_box_labels_;

void on_map(const HADMapBin & msg);
void on_map(const LaneletMapBin & msg);

pcl::PointNormal to_point_normal(
const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const;
Expand Down
2 changes: 1 addition & 1 deletion localization/yabloc/yabloc_common/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>

<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>cv_bridge</depend>
<depend>geometry_msgs</depend>
<depend>lanelet2_core</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ GroundServer::GroundServer(const rclcpp::NodeOptions & options)
auto on_pose = std::bind(&GroundServer::on_pose_stamped, this, _1);
auto on_map = std::bind(&GroundServer::on_map, this, _1);

sub_map_ = create_subscription<HADMapBin>("~/input/vector_map", map_qos, on_map);
sub_map_ = create_subscription<LaneletMapBin>("~/input/vector_map", map_qos, on_map);
sub_pose_stamped_ = create_subscription<PoseStamped>("~/input/pose", 10, on_pose);

pub_ground_height_ = create_publisher<Float32>("~/output/height", 10);
Expand Down Expand Up @@ -100,7 +100,7 @@ void GroundServer::on_pose_stamped(const PoseStamped & msg)
}
}

void GroundServer::on_map(const HADMapBin & msg)
void GroundServer::on_map(const LaneletMapBin & msg)
{
lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
lanelet::utils::conversion::fromBinMsg(msg, lanelet_map);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to

// Subscriber
auto cb_map = std::bind(&Ll2Decomposer::on_map, this, _1);
sub_map_ = create_subscription<HADMapBin>("~/input/vector_map", map_qos, cb_map);
sub_map_ = create_subscription<LaneletMapBin>("~/input/vector_map", map_qos, cb_map);

auto load_lanelet2_labels =
[this](const std::string & param_name, std::set<std::string> & labels) -> void {
Expand Down Expand Up @@ -102,7 +102,7 @@ pcl::PointCloud<pcl::PointXYZL> Ll2Decomposer::load_bounding_boxes(
return cloud;
}

void Ll2Decomposer::on_map(const HADMapBin & msg)
void Ll2Decomposer::on_map(const LaneletMapBin & msg)
{
RCLCPP_INFO_STREAM(get_logger(), "subscribed binary vector map");
lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
Expand Down
10 changes: 5 additions & 5 deletions localization/yabloc/yabloc_pose_initializer/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -45,11 +45,11 @@ Converted model URL

#### Input

| Name | Type | Description |
| ------------------- | -------------------------------------------- | ------------------------ |
| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info |
| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image |
| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map |
| Name | Type | Description |
| ------------------- | --------------------------------------- | ------------------------ |
| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info |
| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image |
| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map |

#### Output

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
#include <opencv2/core.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
Expand All @@ -40,7 +40,7 @@ class CameraPoseInitializer : public rclcpp::Node
using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using Image = sensor_msgs::msg::Image;
using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped;

CameraPoseInitializer();
Expand All @@ -52,7 +52,7 @@ class CameraPoseInitializer : public rclcpp::Node
std::unique_ptr<initializer::ProjectorModule> projector_module_{nullptr};

rclcpp::Subscription<PoseCovStamped>::SharedPtr sub_initialpose_;
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<Image>::SharedPtr sub_image_;

rclcpp::Service<RequestPoseAlignment>::SharedPtr align_server_;
Expand All @@ -63,7 +63,7 @@ class CameraPoseInitializer : public rclcpp::Node

std::unique_ptr<SemanticSegmentation> semantic_segmentation_{nullptr};

void on_map(const HADMapBin & msg);
void on_map(const LaneletMapBin & msg);
void on_service(
const RequestPoseAlignment::Request::SharedPtr,
RequestPoseAlignment::Response::SharedPtr request);
Expand Down
Loading

0 comments on commit d5e59a5

Please sign in to comment.