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

refactor(map_loader)!: prefix package and namespace with autoware #8927

Open
wants to merge 9 commits into
base: main
Choose a base branch
from
Open
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: 1 addition & 0 deletions .github/CODEOWNERS
Validating CODEOWNERS rules …
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,7 @@ localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuu
localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
map/autoware_lanelet2_map_visualizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
map/autoware_map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
map/autoware_map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
map/autoware_map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -364,7 +364,7 @@ void TrafficLightPublishPanel::onTimer()
void TrafficLightPublishPanel::onVectorMap(const LaneletMapBin::ConstSharedPtr msg)
{
if (received_vector_map_) return;
// NOTE: examples from map_loader/lanelet2_map_visualization_node.cpp
// NOTE: examples from autoware_lanelet2_map_visualizer/lanelet2_map_visualization_node.cpp
lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map);
lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map);
Expand Down
8 changes: 4 additions & 4 deletions launch/tier4_map_launch/launch/map.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
<push-ros-namespace namespace="map"/>

<node_container pkg="rclcpp_components" exec="$(var container_type)" name="map_container" namespace="" output="both">
<composable_node pkg="map_loader" plugin="PointCloudMapLoaderNode" name="pointcloud_map_loader">
<composable_node pkg="autoware_map_loader" plugin="autoware::map_loader::PointCloudMapLoaderNode" name="pointcloud_map_loader">
<param from="$(var pointcloud_map_loader_param_path)"/>
<param name="pcd_paths_or_directory" value="[$(var pointcloud_map_path)]"/>
<param name="pcd_metadata_path" value="$(var pointcloud_map_metadata_path)"/>
Expand All @@ -35,14 +35,14 @@
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
</composable_node>

<composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader">
<composable_node pkg="autoware_map_loader" plugin="autoware::map_loader::Lanelet2MapLoaderNode" name="lanelet2_map_loader">
<param from="$(var lanelet2_map_loader_param_path)"/>
<param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
<remap from="output/lanelet2_map" to="vector_map"/>
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
</composable_node>

<composable_node pkg="map_loader" plugin="Lanelet2MapVisualizationNode" name="lanelet2_map_visualization">
<composable_node pkg="autoware_lanelet2_map_visualizer" plugin="autoware::lanelet2_map_visualizer::Lanelet2MapVisualizationNode" name="lanelet2_map_visualization">
<remap from="input/lanelet2_map" to="vector_map"/>
<remap from="output/lanelet2_map_marker" to="vector_map_marker"/>
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
Expand All @@ -54,7 +54,7 @@
</composable_node>
</node_container>

<node pkg="map_loader" exec="map_hash_generator" name="map_hash_generator">
<node pkg="autoware_map_loader" exec="map_hash_generator" name="map_hash_generator">
<param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
<param name="pointcloud_map_path" value="$(var pointcloud_map_path)"/>
</node>
Expand Down
2 changes: 1 addition & 1 deletion launch/tier4_map_launch/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,9 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<exec_depend>autoware_map_loader</exec_depend>
<exec_depend>autoware_map_projection_loader</exec_depend>
<exec_depend>autoware_map_tf_generator</exec_depend>
<exec_depend>map_loader</exec_depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
18 changes: 18 additions & 0 deletions map/autoware_lanelet2_map_visualizer/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
cmake_minimum_required(VERSION 3.14)
project(autoware_lanelet2_map_visualizer)

find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_add_library(lanelet2_map_visualization_node SHARED
src/lanelet2_map_visualization_node.cpp
)

rclcpp_components_register_node(lanelet2_map_visualization_node
PLUGIN "autoware::lanelet2_map_visualizer::Lanelet2MapVisualizationNode"
EXECUTABLE lanelet2_map_visualization
)

ament_auto_package(INSTALL_TO_SHARE
launch
)
21 changes: 21 additions & 0 deletions map/autoware_lanelet2_map_visualizer/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
# autoware_lanelet2_map_visualizer package

This package provides the features of visualizing the lanelet2 maps.

## lanelet2_map_visualization

### Feature

lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray.

### How to Run

`ros2 run autoware_lanelet2_map_visualizer lanelet2_map_visualization`

### Subscribed Topics

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

### Published Topics

- ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<launch>
<arg name="lanelet2_map_topic" default="vector_map"/>
<arg name="lanelet2_map_marker_topic" default="vector_map_marker"/>

<node pkg="autoware_lanelet2_map_visualizer" exec="lanelet2_map_visualization" name="lanelet2_map_visualization" output="both">
<remap from="input/lanelet2_map" to="$(var lanelet2_map_topic)"/>
<remap from="output/lanelet2_map_marker" to="$(var lanelet2_map_marker_topic)"/>
</node>
</launch>
36 changes: 36 additions & 0 deletions map/autoware_lanelet2_map_visualizer/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>autoware_lanelet2_map_visualizer</name>
<version>0.1.0</version>
<description>The autoware_lanelet2_map_visualizer package</description>
<maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
<maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
<maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
<maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
<maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
<maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
<maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
<maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>

<license>Apache License 2.0</license>
<author email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</author>
<author email="koji.minoda@tier4.jp">Koji Minoda</author>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_lanelet2_extension</depend>
<depend>autoware_map_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tier4_map_msgs</depend>
<depend>visualization_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
*
*/

#include "map_loader/lanelet2_map_visualization_node.hpp"
#include "lanelet2_map_visualization_node.hpp"

#include <autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
Expand All @@ -49,7 +49,7 @@
#include <memory>
#include <vector>

namespace
namespace autoware::lanelet2_map_visualizer
{
void insert_marker_array(
visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2)
Expand All @@ -57,14 +57,13 @@
a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
}

void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a)
{
cl->r = static_cast<float>(r);
cl->g = static_cast<float>(g);
cl->b = static_cast<float>(b);
cl->a = static_cast<float>(a);
}

Check warning on line 66 in map/autoware_lanelet2_map_visualizer/src/lanelet2_map_visualization_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Excess Number of Function Arguments

set_color has 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
} // namespace

Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options)
: Node("lanelet2_map_visualization", options)
Expand All @@ -81,239 +80,240 @@
"output/lanelet2_map_marker", rclcpp::QoS{1}.transient_local());
}

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

lanelet::utils::conversion::fromBinMsg(*msg, viz_lanelet_map);
RCLCPP_INFO(this->get_logger(), "Map is loaded\n");

// get lanelets etc to visualize
lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map);
lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets);
lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets);
lanelet::ConstLanelets crosswalk_lanelets =
lanelet::utils::query::crosswalkLanelets(all_lanelets);
lanelet::ConstLanelets bicycle_lane_lanelets =
lanelet::utils::query::bicycleLaneLanelets(all_lanelets);
lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map);
lanelet::ConstLineStrings3d pedestrian_polygon_markings =
lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map);
lanelet::ConstLineStrings3d pedestrian_line_markings =
lanelet::utils::query::getAllPedestrianLineMarkings(viz_lanelet_map);
lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets);
std::vector<lanelet::ConstLineString3d> stop_lines =
lanelet::utils::query::stopLinesLanelets(road_lanelets);
std::vector<lanelet::AutowareTrafficLightConstPtr> aw_tl_reg_elems =
lanelet::utils::query::autowareTrafficLights(all_lanelets);
std::vector<lanelet::DetectionAreaConstPtr> da_reg_elems =
lanelet::utils::query::detectionAreas(all_lanelets);
std::vector<lanelet::NoStoppingAreaConstPtr> no_reg_elems =
lanelet::utils::query::noStoppingAreas(all_lanelets);
std::vector<lanelet::SpeedBumpConstPtr> sb_reg_elems =
lanelet::utils::query::speedBumps(all_lanelets);
std::vector<lanelet::CrosswalkConstPtr> cw_reg_elems =
lanelet::utils::query::crosswalks(all_lanelets);
lanelet::ConstLineStrings3d parking_spaces =
lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map);
lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map);
lanelet::ConstPolygons3d obstacle_polygons =
lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map);
lanelet::ConstPolygons3d no_obstacle_segmentation_area =
lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area");
lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out =
lanelet::utils::query::getAllPolygonsByType(
viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out");
lanelet::ConstPolygons3d hatched_road_markings_area =
lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings");
lanelet::ConstPolygons3d intersection_areas =
lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "intersection_area");
std::vector<lanelet::NoParkingAreaConstPtr> no_parking_reg_elems =
lanelet::utils::query::noParkingAreas(all_lanelets);
lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map);
std::vector<lanelet::BusStopAreaConstPtr> bus_stop_reg_elems =
lanelet::utils::query::busStopAreas(all_lanelets);

std_msgs::msg::ColorRGBA cl_road;
std_msgs::msg::ColorRGBA cl_shoulder;
std_msgs::msg::ColorRGBA cl_cross;
std_msgs::msg::ColorRGBA cl_partitions;
std_msgs::msg::ColorRGBA cl_pedestrian_markings;
std_msgs::msg::ColorRGBA cl_ll_borders;
std_msgs::msg::ColorRGBA cl_shoulder_borders;
std_msgs::msg::ColorRGBA cl_stoplines;
std_msgs::msg::ColorRGBA cl_trafficlights;
std_msgs::msg::ColorRGBA cl_detection_areas;
std_msgs::msg::ColorRGBA cl_speed_bumps;
std_msgs::msg::ColorRGBA cl_crosswalks;
std_msgs::msg::ColorRGBA cl_parking_lots;
std_msgs::msg::ColorRGBA cl_parking_spaces;
std_msgs::msg::ColorRGBA cl_lanelet_id;
std_msgs::msg::ColorRGBA cl_obstacle_polygons;
std_msgs::msg::ColorRGBA cl_no_stopping_areas;
std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area;
std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area_for_run_out;
std_msgs::msg::ColorRGBA cl_hatched_road_markings_area;
std_msgs::msg::ColorRGBA cl_hatched_road_markings_line;
std_msgs::msg::ColorRGBA cl_no_parking_areas;
std_msgs::msg::ColorRGBA cl_curbstones;
std_msgs::msg::ColorRGBA cl_intersection_area;
std_msgs::msg::ColorRGBA cl_bus_stop_area;
std_msgs::msg::ColorRGBA cl_bicycle_lane;
set_color(&cl_road, 0.27, 0.27, 0.27, 0.999);
set_color(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
set_color(&cl_cross, 0.27, 0.3, 0.27, 0.5);
set_color(&cl_partitions, 0.25, 0.25, 0.25, 0.999);
set_color(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999);
set_color(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999);
set_color(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999);
set_color(&cl_stoplines, 0.5, 0.5, 0.5, 0.999);
set_color(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8);
set_color(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5);
set_color(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5);
set_color(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5);
set_color(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5);
set_color(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5);
set_color(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2);
set_color(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3);
set_color(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999);
set_color(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5);
set_color(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5);
set_color(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5);
set_color(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999);
set_color(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5);
set_color(&cl_curbstones, 0.1, 0.1, 0.2, 0.999);
set_color(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5);
set_color(&cl_bus_stop_area, 0.863, 0.863, 0.863, 0.5);
set_color(&cl_bicycle_lane, 0.0, 0.3843, 0.6274, 0.5);

visualization_msgs::msg::MarkerArray map_marker_array;

insert_marker_array(
&map_marker_array,
lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5));
insert_marker_array(
&map_marker_array,
lanelet::visualization::lineStringsAsMarkerArray(partitions, "partitions", cl_partitions, 0.1));
insert_marker_array(
&map_marker_array,
lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_"));
insert_marker_array(
&map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets));
insert_marker_array(
&map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray(
"crosswalk_lanelets", crosswalk_lanelets, cl_cross));
insert_marker_array(
&map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray(
pedestrian_polygon_markings, cl_pedestrian_markings));

insert_marker_array(
&map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray(
pedestrian_line_markings, cl_pedestrian_markings));
insert_marker_array(
&map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray(
"walkway_lanelets", walkway_lanelets, cl_cross));
insert_marker_array(
&map_marker_array,
lanelet::visualization::obstaclePolygonsAsMarkerArray(obstacle_polygons, cl_obstacle_polygons));
insert_marker_array(
&map_marker_array,
lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas));
insert_marker_array(
&map_marker_array,
lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas));
insert_marker_array(
&map_marker_array,
lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps));
insert_marker_array(
&map_marker_array,
lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks));
insert_marker_array(
&map_marker_array,
lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots));
insert_marker_array(
&map_marker_array,
lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces));
insert_marker_array(
&map_marker_array,
lanelet::visualization::laneletsBoundaryAsMarkerArray(
shoulder_lanelets, cl_shoulder_borders, viz_lanelets_centerline_, "shoulder_"));
insert_marker_array(
&map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray(
road_lanelets, cl_ll_borders, viz_lanelets_centerline_));
insert_marker_array(
&map_marker_array,
lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights));
insert_marker_array(
&map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker(
road_lanelets, cl_trafficlights));
insert_marker_array(
&map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker(
crosswalk_lanelets, cl_trafficlights));
insert_marker_array(
&map_marker_array,
lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights));
insert_marker_array(
&map_marker_array,
lanelet::visualization::generateLaneletIdMarker(shoulder_lanelets, cl_lanelet_id));
insert_marker_array(
&map_marker_array,
lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id));
insert_marker_array(
&map_marker_array, lanelet::visualization::generateLaneletIdMarker(
crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id"));
insert_marker_array(
&map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray(
"shoulder_road_lanelets", shoulder_lanelets, cl_shoulder));
insert_marker_array(
&map_marker_array,
lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road));
insert_marker_array(
&map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray(
no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area));
insert_marker_array(
&map_marker_array,
lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray(
no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out));

insert_marker_array(
&map_marker_array,
lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray(
hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line));

insert_marker_array(
&map_marker_array,
lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas));

insert_marker_array(
&map_marker_array,
lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2));

insert_marker_array(
&map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray(
intersection_areas, cl_intersection_area));

insert_marker_array(
&map_marker_array,
lanelet::visualization::busStopAreasAsMarkerArray(bus_stop_reg_elems, cl_bus_stop_area));

insert_marker_array(
&map_marker_array,
lanelet::visualization::laneletDirectionAsMarkerArray(bicycle_lane_lanelets, "bicycle_lane_"));
insert_marker_array(
&map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray(
bicycle_lane_lanelets, cl_ll_borders /* use ll_border color */,
viz_lanelets_centerline_, "bicycle_lane_"));
insert_marker_array(
&map_marker_array, lanelet::visualization::generateLaneletIdMarker(
bicycle_lane_lanelets, cl_lanelet_id /* use lanelet_id color */));
insert_marker_array(
&map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray(
"bicycle_lane_lanelets", bicycle_lane_lanelets, cl_bicycle_lane));

pub_marker_->publish(map_marker_array);
}

Check warning on line 315 in map/autoware_lanelet2_map_visualizer/src/lanelet2_map_visualization_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method

Lanelet2MapVisualizationNode::on_map_bin has 219 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
} // namespace autoware::lanelet2_map_visualizer

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapVisualizationNode)
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lanelet2_map_visualizer::Lanelet2MapVisualizationNode)

Check warning on line 319 in map/autoware_lanelet2_map_visualizer/src/lanelet2_map_visualization_node.cpp

View check run for this annotation

Codecov / codecov/patch

map/autoware_lanelet2_map_visualizer/src/lanelet2_map_visualization_node.cpp#L319

Added line #L319 was not covered by tests
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_
#define MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_
#ifndef LANELET2_MAP_VISUALIZATION_NODE_HPP_
#define LANELET2_MAP_VISUALIZATION_NODE_HPP_

#include <rclcpp/rclcpp.hpp>

Expand All @@ -23,6 +23,8 @@
#include <string>
#include <vector>

namespace autoware::lanelet2_map_visualizer
{
class Lanelet2MapVisualizationNode : public rclcpp::Node
{
public:
Expand All @@ -36,5 +38,6 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node

void on_map_bin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg);
};
} // namespace autoware::lanelet2_map_visualizer

#endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_
#endif // LANELET2_MAP_VISUALIZATION_NODE_HPP_
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.14)
project(map_loader)
project(autoware_map_loader)

find_package(autoware_cmake REQUIRED)
autoware_package()
Expand All @@ -23,26 +23,17 @@ target_include_directories(pointcloud_map_loader_node
)

rclcpp_components_register_node(pointcloud_map_loader_node
PLUGIN "PointCloudMapLoaderNode"
EXECUTABLE pointcloud_map_loader
PLUGIN "autoware::map_loader::PointCloudMapLoaderNode"
EXECUTABLE autoware_pointcloud_map_loader
)

ament_auto_add_library(lanelet2_map_loader_node SHARED
src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
)

rclcpp_components_register_node(lanelet2_map_loader_node
PLUGIN "Lanelet2MapLoaderNode"
EXECUTABLE lanelet2_map_loader
)

ament_auto_add_library(lanelet2_map_visualization_node SHARED
src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp
)

rclcpp_components_register_node(lanelet2_map_visualization_node
PLUGIN "Lanelet2MapVisualizationNode"
EXECUTABLE lanelet2_map_visualization
PLUGIN "autoware::map_loader::Lanelet2MapLoaderNode"
EXECUTABLE autoware_lanelet2_map_loader
)

if(BUILD_TESTING)
Expand Down
28 changes: 4 additions & 24 deletions map/map_loader/README.md → map/autoware_map_loader/README.md
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# map_loader package
# autoware_map_loader package

This package provides the features of loading various maps.

Expand Down Expand Up @@ -111,7 +111,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co

### Parameters

{{ json_to_markdown("map/map_loader/schema/pointcloud_map_loader.schema.json") }}
{{ json_to_markdown("map/autoware_map_loader/schema/pointcloud_map_loader.schema.json") }}

### Interfaces

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

### How to run

`ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm`
`ros2 run autoware_map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm`

### Subscribed Topics

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

### Parameters

{{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }}
{{ json_to_markdown("map/autoware_map_loader/schema/lanelet2_map_loader.schema.json") }}

`use_waypoints` decides how to handle a centerline.
This flag enables to use the `overwriteLaneletsCenterlineWithWaypoints` function instead of `overwriteLaneletsCenterline`. Please see [the document of the autoware_lanelet2_extension package](https://github.com/autowarefoundation/autoware_lanelet2_extension/blob/main/autoware_lanelet2_extension/docs/lanelet2_format_extension.md#centerline) in detail.

---

## lanelet2_map_visualization

### Feature

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_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map

### Published Topics

- ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
#define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
#ifndef AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
#define AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_

#include <autoware_lanelet2_extension/version.hpp>
#include <component_interface_specs/map.hpp>
Expand All @@ -28,6 +28,8 @@
#include <memory>
#include <string>

namespace autoware::map_loader
{
class Lanelet2MapLoaderNode : public rclcpp::Node
{
public:
Expand All @@ -51,5 +53,6 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
rclcpp::Publisher<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr pub_map_bin_;
};
} // namespace autoware::map_loader

#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
#endif // AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
15 changes: 15 additions & 0 deletions map/autoware_map_loader/launch/lanelet2_map_loader.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
<launch>
<arg name="lanelet2_map_loader_param_path" default="$(find-pkg-share autoware_map_loader)/config/lanelet2_map_loader.param.yaml"/>
<arg name="lanelet2_map_path"/>
<arg name="lanelet2_map_topic" default="vector_map"/>
<arg name="lanelet2_map_marker_topic" default="vector_map_marker"/>

<node pkg="autoware_map_loader" exec="map_hash_generator" name="map_hash_generator" output="both">
<param from="$(var lanelet2_map_loader_param_path)" allow_substs="true"/>
</node>

<node pkg="autoware_map_loader" exec="lanelet2_map_loader" name="lanelet2_map_loader" output="both">
<remap from="output/lanelet2_map" to="$(var lanelet2_map_topic)"/>
<param from="$(var lanelet2_map_loader_param_path)" allow_substs="true"/>
</node>
</launch>
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
<launch>
<arg name="pointcloud_map_path"/>
<arg name="pointcloud_map_metadata_path"/>
<arg name="pointcloud_map_loader_param_path" default="$(find-pkg-share map_loader)/config/pointcloud_map_loader.param.yaml"/>
<arg name="pointcloud_map_loader_param_path" default="$(find-pkg-share autoware_map_loader)/config/pointcloud_map_loader.param.yaml"/>

<node pkg="map_loader" exec="pointcloud_map_loader" name="pointcloud_map_loader" output="both">
<node pkg="autoware_map_loader" exec="pointcloud_map_loader" name="pointcloud_map_loader" output="both">
<remap from="output/pointcloud_map" to="/map/pointcloud_map"/>
<remap from="service/get_partial_pcd_map" to="/map/get_partial_pointcloud_map"/>
<remap from="service/get_selected_pcd_map" to="/map/get_selected_pointcloud_map"/>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>map_loader</name>
<name>autoware_map_loader</name>
<version>0.1.0</version>
<description>The map_loader package</description>
<description>The autoware_map_loader package</description>
<maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
<maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
<maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
Expand Down
Loading
Loading