Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat!: replace autoware_auto_msgs with autoware_msgs for map modules #7244

Merged
merged 1 commit into from
Jun 4, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion map/map_height_fitter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>geometry_msgs</depend>
<depend>lanelet2_extension</depend>
Expand Down
10 changes: 5 additions & 5 deletions map/map_height_fitter/src/map_height_fitter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <lanelet2_extension/utility/query.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <autoware_map_msgs/srv/get_partial_point_cloud_map.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
Expand All @@ -38,7 +38,7 @@ struct MapHeightFitter::Impl

explicit Impl(rclcpp::Node * node);
void on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
void on_vector_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg);
bool get_partial_point_cloud_map(const Point & point);
double get_ground_height(const Point & point) const;
std::optional<Point> fit(const Point & position, const std::string & frame);
Expand All @@ -59,7 +59,7 @@ struct MapHeightFitter::Impl

// for fitting by vector_map_loader
lanelet::LaneletMapPtr vector_map_;
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_vector_map_;
rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr sub_vector_map_;
};

MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), node_(node)
Expand Down Expand Up @@ -95,7 +95,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n

} else if (fit_target_ == "vector_map") {
const auto durable_qos = rclcpp::QoS(1).transient_local();
sub_vector_map_ = node_->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
sub_vector_map_ = node_->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
"~/vector_map", durable_qos,
std::bind(&MapHeightFitter::Impl::on_vector_map, this, std::placeholders::_1));

Expand Down Expand Up @@ -163,7 +163,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
}

void MapHeightFitter::Impl::on_vector_map(
const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg)
const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg)
{
vector_map_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(*msg, vector_map_);
Expand Down
8 changes: 4 additions & 4 deletions map/map_loader/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co

### Feature

lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message.
lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message.
The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`.
Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types.

Expand All @@ -144,7 +144,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie

### Published Topics

- ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map
- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map

### Parameters

Expand All @@ -156,15 +156,15 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie

### Feature

lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray.
lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray.

### How to Run

`ros2 run map_loader lanelet2_map_visualization`

### Subscribed Topics

- ~input/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : binary data of Lanelet2 Map
- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map

### Published Topics

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,15 +19,15 @@
#include <component_interface_utils/rclcpp.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 <tier4_map_msgs/msg/map_projector_info.hpp>

#include <lanelet2_projection/UTM.h>

#include <memory>
#include <string>

using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_map_msgs::msg::LaneletMapBin;
using tier4_map_msgs::msg::MapProjectorInfo;

class Lanelet2MapLoaderNode : public rclcpp::Node
Expand All @@ -38,7 +38,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
static lanelet::LaneletMapPtr load_map(
const std::string & lanelet2_filename,
const tier4_map_msgs::msg::MapProjectorInfo & projector_info);
static HADMapBin create_map_bin_msg(
static LaneletMapBin create_map_bin_msg(
const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename,
const rclcpp::Time & now);

Expand All @@ -48,7 +48,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg);

component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
rclcpp::Publisher<LaneletMapBin>::SharedPtr pub_map_bin_;
};

#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
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 <visualization_msgs/msg/marker_array.hpp>

#include <string>
Expand All @@ -29,12 +29,12 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node
explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options);

private:
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_map_bin_;
rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr sub_map_bin_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_marker_;

bool viz_lanelets_centerline_;

void onMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg);
};

#endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_
1 change: 0 additions & 1 deletion map/map_loader/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>component_interface_specs</depend>
<depend>component_interface_utils</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(

// create publisher and publish
pub_map_bin_ =
create_publisher<HADMapBin>("output/lanelet2_map", rclcpp::QoS{1}.transient_local());
create_publisher<LaneletMapBin>("output/lanelet2_map", rclcpp::QoS{1}.transient_local());
pub_map_bin_->publish(map_bin_msg);
RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published.");
}
Expand Down Expand Up @@ -141,18 +141,18 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
return nullptr;
}

HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg(
LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg(
const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now)
{
std::string format_version{}, map_version{};
lanelet::io_handlers::AutowareOsmParser::parseVersions(
lanelet2_filename, &format_version, &map_version);

HADMapBin map_bin_msg;
LaneletMapBin map_bin_msg;
map_bin_msg.header.stamp = now;
map_bin_msg.header.frame_id = "map";
map_bin_msg.format_version = format_version;
map_bin_msg.map_version = map_version;
map_bin_msg.version_map_format = format_version;
map_bin_msg.version_map = map_version;
lanelet::utils::conversion::toBinMsg(map, &map_bin_msg);

return map_bin_msg;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
#include <lanelet2_extension/visualization/visualization.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 <sensor_msgs/msg/point_cloud2.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

Expand Down Expand Up @@ -73,7 +73,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt

viz_lanelets_centerline_ = true;

sub_map_bin_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
sub_map_bin_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
"input/lanelet2_map", rclcpp::QoS{1}.transient_local(),
std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1));

Expand All @@ -82,7 +82,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt
}

void Lanelet2MapVisualizationNode::onMapBin(
const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg)
const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg)
{
lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap);

Expand Down
6 changes: 3 additions & 3 deletions map/map_tf_generator/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,9 @@ The following are the supported methods to calculate the position of the `viewer

#### vector_map_tf_generator

| Name | Type | Description |
| ----------------- | -------------------------------------------- | ------------------------------------------------------------- |
| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Subscribe vector map to calculate position of `viewer` frames |
| Name | Type | Description |
| ----------------- | --------------------------------------- | ------------------------------------------------------------- |
| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Subscribe vector map to calculate position of `viewer` frames |

### Output

Expand Down
8 changes: 4 additions & 4 deletions map/map_tf_generator/src/vector_map_tf_generator_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
#include <lanelet2_extension/utility/message_conversion.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 <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/Point.h>
Expand All @@ -34,7 +34,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node
map_frame_ = declare_parameter<std::string>("map_frame");
viewer_frame_ = declare_parameter<std::string>("viewer_frame");

sub_ = create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
sub_ = create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
"vector_map", rclcpp::QoS{1}.transient_local(),
std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1));

Expand All @@ -44,12 +44,12 @@ class VectorMapTFGeneratorNode : public rclcpp::Node
private:
std::string map_frame_;
std::string viewer_frame_;
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_;
rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr sub_;

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;
std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;

void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg)
void onVectorMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg)
{
lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_);
Expand Down
Loading