Skip to content

Commit

Permalink
Merge branch 'main' into ci/add_cpp_check
Browse files Browse the repository at this point in the history
  • Loading branch information
kminoda authored Jun 4, 2024
2 parents b8829a4 + b7f18a1 commit 3384a55
Show file tree
Hide file tree
Showing 29 changed files with 137 additions and 157 deletions.
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
4 changes: 2 additions & 2 deletions simulator/dummy_perception_publisher/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,14 @@ autoware_package()
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/InitialState.msg"
"msg/Object.msg"
DEPENDENCIES autoware_auto_perception_msgs tier4_perception_msgs geometry_msgs std_msgs unique_identifier_msgs
DEPENDENCIES autoware_perception_msgs tier4_perception_msgs geometry_msgs std_msgs unique_identifier_msgs
)

# See ndt_omp package for documentation on why PCL is special
find_package(PCL REQUIRED COMPONENTS common filters)

set(${PROJECT_NAME}_DEPENDENCIES
autoware_auto_perception_msgs
autoware_perception_msgs
tier4_perception_msgs
pcl_conversions
rclcpp
Expand Down
2 changes: 1 addition & 1 deletion simulator/dummy_perception_publisher/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ This node publishes the result of the dummy detection with the type of perceptio
| ----------------------------------- | -------------------------------------------------------- | ----------------------- |
| `output/dynamic_object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | dummy detection objects |
| `output/points_raw` | `sensor_msgs::msg::PointCloud2` | point cloud of objects |
| `output/debug/ground_truth_objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | ground truth objects |
| `output/debug/ground_truth_objects` | `autoware_perception_msgs::msg::TrackedObjects` | ground truth objects |

## Parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/tracked_objects.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>

Expand Down Expand Up @@ -59,7 +59,7 @@ struct ObjectInfo
geometry_msgs::msg::PoseWithCovariance pose_covariance_;
// convert to TrackedObject
// (todo) currently need object input to get id and header information, but it should be removed
autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject(
autoware_perception_msgs::msg::TrackedObject toTrackedObject(
const dummy_perception_publisher::msg::Object & object) const;
};

Expand Down Expand Up @@ -114,7 +114,7 @@ class DummyPerceptionPublisherNode : public rclcpp::Node
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
rclcpp::Publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
detected_object_with_feature_pub_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
rclcpp::Publisher<autoware_perception_msgs::msg::TrackedObjects>::SharedPtr
ground_truth_objects_pub_;
rclcpp::Subscription<dummy_perception_publisher::msg::Object>::SharedPtr object_sub_;
rclcpp::TimerBase::SharedPtr timer_;
Expand Down
4 changes: 2 additions & 2 deletions simulator/dummy_perception_publisher/msg/Object.msg
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
std_msgs/Header header
unique_identifier_msgs/UUID id
dummy_perception_publisher/InitialState initial_state
autoware_auto_perception_msgs/ObjectClassification classification
autoware_auto_perception_msgs/Shape shape
autoware_perception_msgs/ObjectClassification classification
autoware_perception_msgs/Shape shape
float32 max_velocity
float32 min_velocity

Expand Down
2 changes: 1 addition & 1 deletion simulator/dummy_perception_publisher/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>geometry_msgs</depend>
<depend>libpcl-all-dev</depend>
<depend>pcl_conversions</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_perception_msgs/msg/predicted_objects.hpp>

#include <memory>
#include <utility>
Expand All @@ -24,23 +24,21 @@ class EmptyObjectsPublisher : public rclcpp::Node
public:
EmptyObjectsPublisher() : Node("empty_objects_publisher")
{
empty_objects_pub_ =
this->create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>(
"~/output/objects", 1);
empty_objects_pub_ = this->create_publisher<autoware_perception_msgs::msg::PredictedObjects>(
"~/output/objects", 1);

using std::chrono_literals::operator""ms;
timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&EmptyObjectsPublisher::timerCallback, this));
}

private:
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr
empty_objects_pub_;
rclcpp::Publisher<autoware_perception_msgs::msg::PredictedObjects>::SharedPtr empty_objects_pub_;
rclcpp::TimerBase::SharedPtr timer_;

void timerCallback()
{
autoware_auto_perception_msgs::msg::PredictedObjects empty_objects;
autoware_perception_msgs::msg::PredictedObjects empty_objects;
empty_objects.header.frame_id = "map";
empty_objects.header.stamp = this->now();
empty_objects_pub_->publish(empty_objects);
Expand Down
Loading

0 comments on commit 3384a55

Please sign in to comment.