Skip to content

Commit

Permalink
feat: port lanelet2_map_preprocessor to ROS2 (tier4#847)
Browse files Browse the repository at this point in the history 
Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>
  • Loading branch information
@kenji-miyake @boyali
kenji-miyake authored and boyali committed Sep 28, 2022
1 parent 9d106c6 commit ee8551f
Show file tree
Hide file tree
Showing 8 changed files with 104 additions and 119 deletions.
67 changes: 20 additions & 47 deletions map/util/lanelet2_map_preprocessor/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,56 +1,29 @@
cmake_minimum_required(VERSION 3.0.2)
cmake_minimum_required(VERSION 3.5)
project(lanelet2_map_preprocessor)

add_compile_options(-std=c++14)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
endif()

find_package(catkin REQUIRED COMPONENTS
lanelet2_extension
pcl_ros
roscpp
)

catkin_package(
)
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

include_directories(
include
${catkin_INCLUDE_DIRS}
SYSTEM
${PCL_INCLUDE_DIRS}
)

add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp)
add_dependencies(fix_z_value_by_pcd ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(fix_z_value_by_pcd ${catkin_LIBRARIES})

add_executable(transform_maps src/transform_maps.cpp)
add_dependencies(transform_maps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(transform_maps ${catkin_LIBRARIES})

add_executable(merge_close_lines src/merge_close_lines.cpp)
add_dependencies(merge_close_lines ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(merge_close_lines ${catkin_LIBRARIES})
ament_auto_add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp)
ament_auto_add_executable(transform_maps src/transform_maps.cpp)
ament_auto_add_executable(merge_close_lines src/merge_close_lines.cpp)
ament_auto_add_executable(merge_close_points src/merge_close_points.cpp)
ament_auto_add_executable(remove_unreferenced_geometry src/remove_unreferenced_geometry.cpp)
ament_auto_add_executable(fix_lane_change_tags src/fix_lane_change_tags.cpp)

add_executable(merge_close_points src/merge_close_points.cpp)
add_dependencies(merge_close_points ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(merge_close_points ${catkin_LIBRARIES})

add_executable(remove_unreferenced_geometry src/remove_unreferenced_geometry.cpp)
add_dependencies(remove_unreferenced_geometry ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(remove_unreferenced_geometry ${catkin_LIBRARIES})

add_executable(fix_lane_change_tags src/fix_lane_change_tags.cpp)
add_dependencies(fix_lane_change_tags ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(fix_lane_change_tags ${catkin_LIBRARIES})

## Install executables and/or libraries
install(
TARGETS
fix_z_value_by_pcd
transform_maps
merge_close_lines
merge_close_points
remove_unreferenced_geometry
fix_lane_change_tags
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
ament_auto_package()
Empty file removed map/util/lanelet2_map_preprocessor/COLCON_IGNORE
View file
Empty file.
18 changes: 10 additions & 8 deletions map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,12 @@
#include <lanelet2_extension/io/autoware_osm_parser.hpp>
#include <lanelet2_extension/projection/mgrs_projector.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <rclcpp/rclcpp.hpp>

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/primitives/LaneletSequence.h>
#include <lanelet2_io/Io.h>
#include <ros/ros.h>

#include <iostream>
#include <unordered_set>
Expand All @@ -45,7 +45,7 @@ bool loadLaneletMap(
lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);

for (const auto & error : errors) {
ROS_ERROR_STREAM(error);
RCLCPP_ERROR_STREAM(rclcpp::get_logger("fix_lane_change_tags"), error);
}
if (!errors.empty()) {
return false;
Expand Down Expand Up @@ -92,21 +92,21 @@ void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr)

int main(int argc, char * argv[])
{
ros::init(argc, argv, "merge_lines");
ros::NodeHandle pnh("~");
rclcpp::init(argc, argv);
rclcpp::Node node("fix_lane_change_tags");

if (!pnh.hasParam("llt_map_path")) {
if (!node.has_parameter("llt_map_path")) {
printUsage();
return EXIT_FAILURE;
}
if (!pnh.hasParam("output_path")) {
if (!node.has_parameter("output_path")) {
printUsage();
return EXIT_FAILURE;
}

std::string llt_map_path, output_path;
pnh.getParam("llt_map_path", llt_map_path);
pnh.getParam("output_path", output_path);
node.get_parameter("llt_map_path", llt_map_path);
node.get_parameter("output_path", output_path);

lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap);
lanelet::projection::MGRSProjector projector;
Expand All @@ -118,5 +118,7 @@ int main(int argc, char * argv[])
fixTags(llt_map_ptr);
lanelet::write(output_path, *llt_map_ptr, projector);

rclcpp::shutdown();

return 0;
}
22 changes: 12 additions & 10 deletions map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,13 @@
#include <lanelet2_extension/io/autoware_osm_parser.hpp>
#include <lanelet2_extension/projection/mgrs_projector.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <rclcpp/rclcpp.hpp>

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_io/Io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_types.h>
#include <ros/ros.h>

#include <iostream>
#include <unordered_set>
Expand All @@ -44,7 +44,7 @@ bool loadLaneletMap(
lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);

for (const auto & error : errors) {
ROS_ERROR_STREAM(error);
RCLCPP_ERROR_STREAM(rclcpp::get_logger("fix_z_value_by_pcd"), error);
}
if (!errors.empty()) {
return false;
Expand Down Expand Up @@ -148,26 +148,26 @@ void adjustHeight(

int main(int argc, char * argv[])
{
ros::init(argc, argv, "lanelet_map_height_adjuster");
ros::NodeHandle pnh("~");
rclcpp::init(argc, argv);
rclcpp::Node node("fix_z_value_by_pcd");

if (!pnh.hasParam("llt_map_path")) {
if (!node.has_parameter("llt_map_path")) {
printUsage();
return EXIT_FAILURE;
}
if (!pnh.hasParam("pcd_map_path")) {
if (!node.has_parameter("pcd_map_path")) {
printUsage();
return EXIT_FAILURE;
}
if (!pnh.hasParam("output_path")) {
if (!node.has_parameter("output_path")) {
printUsage();
return EXIT_FAILURE;
}

std::string llt_map_path, pcd_map_path, output_path;
pnh.getParam("llt_map_path", llt_map_path);
pnh.getParam("pcd_map_path", pcd_map_path);
pnh.getParam("output_path", output_path);
node.get_parameter("llt_map_path", llt_map_path);
node.get_parameter("pcd_map_path", pcd_map_path);
node.get_parameter("output_path", output_path);

lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap);
lanelet::projection::MGRSProjector projector;
Expand All @@ -184,5 +184,7 @@ int main(int argc, char * argv[])
adjustHeight(pcd_map_ptr, llt_map_ptr);
lanelet::write(output_path, *llt_map_ptr, projector);

rclcpp::shutdown();

return 0;
}
28 changes: 15 additions & 13 deletions map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,12 @@
#include <lanelet2_extension/io/autoware_osm_parser.hpp>
#include <lanelet2_extension/projection/mgrs_projector.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <rclcpp/rclcpp.hpp>

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/primitives/LaneletSequence.h>
#include <lanelet2_io/Io.h>
#include <ros/ros.h>

#include <iostream>
#include <unordered_set>
Expand All @@ -45,7 +45,7 @@ bool loadLaneletMap(
lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);

for (const auto & error : errors) {
ROS_ERROR_STREAM(error);
RCLCPP_ERROR_STREAM(rclcpp::get_logger("merge_close_lines"), error);
}
if (!errors.empty()) {
return false;
Expand All @@ -62,7 +62,7 @@ bool exists(std::unordered_set<lanelet::Id> & set, lanelet::Id element)
lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr)
{
lanelet::LineStrings3d lines;
for (const lanelet::LineString3d line : lanelet_map_ptr->lineStringLayer) {
for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) {
lines.push_back(line);
}
return lines;
Expand All @@ -76,7 +76,7 @@ lanelet::ConstPoint3d get3DPointFrom2DArcLength(
return lanelet::Point3d();
}
auto prev_pt = line.front();
for (int i = 1; i < line.size(); i++) {
for (size_t i = 1; i < line.size(); i++) {
const auto & pt = line[i];
double distance2d = lanelet::geometry::distance2d(to2D(prev_pt), to2D(pt));
if (accumulated_distance2d + distance2d >= s) {
Expand All @@ -89,7 +89,7 @@ lanelet::ConstPoint3d get3DPointFrom2DArcLength(
accumulated_distance2d += distance2d;
prev_pt = pt;
}
ROS_ERROR("interpolation failed");
RCLCPP_ERROR(rclcpp::get_logger("merge_close_lines"), "interpolation failed");
return lanelet::ConstPoint3d();
}

Expand Down Expand Up @@ -155,7 +155,7 @@ void copyData(lanelet::LineString3d & dst, lanelet::LineString3d & src)
{
lanelet::Points3d points;
dst.clear();
for (lanelet::Point3d pt : src) {
for (lanelet::Point3d & pt : src) {
dst.push_back(pt);
}
}
Expand All @@ -175,7 +175,7 @@ void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr)
line_i.setId(line_j.id());
std::cout << line_j << " " << line_i << std::endl;
// lanelet_map_ptr->add(merged_line);
for (lanelet::Point3d pt : merged_line) {
for (lanelet::Point3d & pt : merged_line) {
lanelet_map_ptr->add(pt);
}
break;
Expand All @@ -186,21 +186,21 @@ void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr)

int main(int argc, char * argv[])
{
ros::init(argc, argv, "merge_lines");
ros::NodeHandle pnh("~");
rclcpp::init(argc, argv);
rclcpp::Node node("merge_close_lines");

if (!pnh.hasParam("llt_map_path")) {
if (!node.has_parameter("llt_map_path")) {
printUsage();
return EXIT_FAILURE;
}
if (!pnh.hasParam("output_path")) {
if (!node.has_parameter("output_path")) {
printUsage();
return EXIT_FAILURE;
}

std::string llt_map_path, output_path;
pnh.getParam("llt_map_path", llt_map_path);
pnh.getParam("output_path", output_path);
node.get_parameter("llt_map_path", llt_map_path);
node.get_parameter("output_path", output_path);

lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap);
lanelet::projection::MGRSProjector projector;
Expand All @@ -212,5 +212,7 @@ int main(int argc, char * argv[])
mergeLines(llt_map_ptr);
lanelet::write(output_path, *llt_map_ptr, projector);

rclcpp::shutdown();

return 0;
}
20 changes: 11 additions & 9 deletions map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,11 @@
#include <lanelet2_extension/io/autoware_osm_parser.hpp>
#include <lanelet2_extension/projection/mgrs_projector.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <rclcpp/rclcpp.hpp>

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_io/Io.h>
#include <ros/ros.h>

#include <iostream>
#include <unordered_set>
Expand All @@ -41,7 +41,7 @@ bool loadLaneletMap(
lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);

for (const auto & error : errors) {
ROS_ERROR_STREAM(error);
RCLCPP_ERROR_STREAM(rclcpp::get_logger("merge_close_points"), error);
}
if (!errors.empty()) {
return false;
Expand All @@ -58,7 +58,7 @@ bool exists(std::unordered_set<lanelet::Id> & set, lanelet::Id element)
lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr)
{
lanelet::Points3d points;
for (const lanelet::Point3d pt : lanelet_map_ptr->pointLayer) {
for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) {
points.push_back(pt);
}
return points;
Expand Down Expand Up @@ -106,21 +106,21 @@ void mergePoints(lanelet::LaneletMapPtr & lanelet_map_ptr)

int main(int argc, char * argv[])
{
ros::init(argc, argv, "merge_lines");
ros::NodeHandle pnh("~");
rclcpp::init(argc, argv);
rclcpp::Node node("merge_close_points");

if (!pnh.hasParam("llt_map_path")) {
if (!node.has_parameter("llt_map_path")) {
printUsage();
return EXIT_FAILURE;
}
if (!pnh.hasParam("output_path")) {
if (!node.has_parameter("output_path")) {
printUsage();
return EXIT_FAILURE;
}

std::string llt_map_path, pcd_map_path, output_path;
pnh.getParam("llt_map_path", llt_map_path);
pnh.getParam("output_path", output_path);
node.get_parameter("llt_map_path", llt_map_path);
node.get_parameter("output_path", output_path);

lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap);
lanelet::projection::MGRSProjector projector;
Expand All @@ -132,5 +132,7 @@ int main(int argc, char * argv[])
mergePoints(llt_map_ptr);
lanelet::write(output_path, *llt_map_ptr, projector);

rclcpp::shutdown();

return 0;
}
Loading

0 comments on commit ee8551f

Please sign in to comment.