From 6eea0ef0c0f3fe14b8bed83257c80a1744f27f8f Mon Sep 17 00:00:00 2001 From: Ryan Date: Sun, 29 Oct 2023 19:17:31 -0600 Subject: [PATCH 01/14] Port to humble (#24) * Port to ROS 2 Signed-off-by: Ryan Friedman * Get launch file running aside from resources * Update docs * Remove ROS1 cruft Signed-off-by: Ryan Friedman --------- Signed-off-by: Ryan Friedman --- CMakeLists.txt | 82 ++++++++++--------- README.md | 22 ++--- .../{grid_map_geo.h => grid_map_geo.hpp} | 2 +- .../{transform.h => transform.hpp} | 0 launch/load_tif_launch.xml | 14 ++++ package.xml | 25 ++++-- src/CMakeLists.txt | 10 +++ src/grid_map_geo.cpp | 2 +- src/test_tif_loader.cpp | 14 +--- test/CMakeLists.txt | 13 +++ test/test_grid_map_geo.cpp | 7 +- 11 files changed, 110 insertions(+), 81 deletions(-) rename include/grid_map_geo/{grid_map_geo.h => grid_map_geo.hpp} (99%) rename include/grid_map_geo/{transform.h => transform.hpp} (100%) create mode 100644 launch/load_tif_launch.xml create mode 100644 src/CMakeLists.txt create mode 100644 test/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 364aa95..39c5af0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,62 +1,68 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.14.4) project(grid_map_geo) +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +# Set policy for 3.16 for CMP0076 defaulting to ON +cmake_policy(VERSION 3.16) + +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies -find_package(GDAL REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3) - +find_package(GDAL 3 REQUIRED) find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(grid_map_core REQUIRED) find_package(grid_map_ros REQUIRED) -find_package(grid_map_msgs REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(Eigen3 REQUIRED) +add_library(${PROJECT_NAME}) +add_subdirectory(src) -include_directories( - include +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" ) -add_library(${PROJECT_NAME} - src/grid_map_geo.cpp -) -ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs geometry_msgs grid_map_core grid_map_ros grid_map_msgs) -target_include_directories(${PROJECT_NAME} PUBLIC "${Eigen3_INCLUDE_DIRS}") -target_link_libraries(${PROJECT_NAME} ${GDAL_LIBRARY}) +# Reverse compatability for GDAL<3.5 +# https://gdal.org/development/cmake.html +if(NOT TARGET GDAL::GDAL) + add_library(GDAL::GDAL ALIAS ${GDAL_LIBRARY}) +endif() -add_executable(test_tif_loader - src/test_tif_loader.cpp -) -ament_target_dependencies(test_tif_loader rclcpp std_msgs) -target_include_directories(test_tif_loader PUBLIC "${Eigen3_INCLUDE_DIRS}") -target_link_libraries(test_tif_loader ${PROJECT_NAME} ${GDAL_LIBRARY}) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen GDAL::GDAL grid_map_core::grid_map_core) + +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_dependencies(GDAL) -install(TARGETS - test_tif_loader - DESTINATION lib/${PROJECT_NAME}) +install( + DIRECTORY include/ + DESTINATION include +) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +include(CTest) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + add_subdirectory(test) endif() ament_package() diff --git a/README.md b/README.md index 8a5ac56..c265942 100644 --- a/README.md +++ b/README.md @@ -10,30 +10,22 @@ This package provides a georeferenced extension to the elevation map [grid_map]( Affiliation: [ETH Zurich, Autonomous Systems Lab](https://asl.ethz.ch/)
** ## Setup Install the dependencies. This package depends on gdal, to read georeferenced images and GeoTIFF files. -``` -apt install libgdal-dev -``` -Configure the catkin workspace -``` -catkin config --extend "/opt/ros/noetic" -catkin config --merge-devel -``` -Pull in dependencies using rosinstall / rosdep +Pull in dependencies using rosdep ``` -wstool init src src/grid_map_geo/dependencies.rosinstall -wstool update -t src -j4 +source /opt/ros/humble/setup.bash rosdep update -rosdep install --from-paths src --ignore-src -y --rosdistro noetic +# Assuming the package is cloned in the src folder of a ROS workspace... +rosdep install --from-paths src --ignore-src -y ``` Build the package ``` -catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False -catkin build -j$(nproc) -l$(nproc) grid_map_geo +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False --packages-up-to grid_map_geo ``` ## Running the package The default launch file can be run as the following command. ``` -roslaunch grid_map_geo load_tif.launch +source install/setup.bash +ros2 launch grid_map_geo load_tif_launch.xml ``` diff --git a/include/grid_map_geo/grid_map_geo.h b/include/grid_map_geo/grid_map_geo.hpp similarity index 99% rename from include/grid_map_geo/grid_map_geo.h rename to include/grid_map_geo/grid_map_geo.hpp index 6484ffd..c2b962c 100644 --- a/include/grid_map_geo/grid_map_geo.h +++ b/include/grid_map_geo/grid_map_geo.hpp @@ -34,7 +34,7 @@ #ifndef GRID_MAP_GEO_H #define GRID_MAP_GEO_H -#include "grid_map_geo/transform.h" +#include "grid_map_geo/transform.hpp" #include #include diff --git a/include/grid_map_geo/transform.h b/include/grid_map_geo/transform.hpp similarity index 100% rename from include/grid_map_geo/transform.h rename to include/grid_map_geo/transform.hpp diff --git a/launch/load_tif_launch.xml b/launch/load_tif_launch.xml new file mode 100644 index 0000000..749a34d --- /dev/null +++ b/launch/load_tif_launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 3d04411..823838d 100644 --- a/package.xml +++ b/package.xml @@ -2,18 +2,25 @@ grid_map_geo - 0.0.0 - TODO: Package description - jaeyoung - TODO: License declaration + 0.0.1 + Georeferenced grid map + Jaeyoung Lim + Ryan Friedman + Jaeyoung Lim + BSD-3 - ament_cmake + https://github.com/Jaeyoung-Lim/grid_map_geo + https://github.com/Jaeyoung-Lim/grid_map_geo/issues - ament_lint_auto - ament_lint_common - rclcpp + ament_cmake + eigen grid_map_core + grid_map_ros + libgdal-dev + rclcpp + ros2launch + ament_cmake - + \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..9352845 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,10 @@ +target_sources(${PROJECT_NAME} PRIVATE grid_map_geo.cpp) + + +find_package(rclcpp REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +add_executable(test_tif_loader) +target_sources(test_tif_loader PRIVATE test_tif_loader.cpp) +target_link_libraries(test_tif_loader PRIVATE ${PROJECT_NAME} rclcpp::rclcpp ${grid_map_msgs_TARGETS}) +ament_target_dependencies(${PROJECT_NAME} grid_map_ros) \ No newline at end of file diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp index 8b01ec2..355908e 100644 --- a/src/grid_map_geo.cpp +++ b/src/grid_map_geo.cpp @@ -37,7 +37,7 @@ * @author Jaeyoung Lim */ -#include "grid_map_geo/grid_map_geo.h" +#include "grid_map_geo/grid_map_geo.hpp" #include #include diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp index c42f3b8..1c8636d 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -37,7 +37,7 @@ * @author Jaeyoung Lim */ -#include "grid_map_geo/grid_map_geo.h" +#include "grid_map_geo/grid_map_geo.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -52,11 +52,8 @@ class MapPublisher : public rclcpp::Node { MapPublisher() : Node("map_publisher") { original_map_pub_ = this->create_publisher("elevation_map", 1); - this->declare_parameter("tif_path", "."); - this->declare_parameter("tif_color_path", "."); - - std::string file_path = this->get_parameter("tif_path").as_string(); - std::string color_path = this->get_parameter("tif_color_path").as_string(); + std::string file_path = this->declare_parameter("tif_path", "."); + std::string color_path = this->declare_parameter("tif_color_path", "."); map_ = std::make_shared(); map_->Load(file_path, false, color_path); @@ -68,13 +65,8 @@ class MapPublisher : public rclcpp::Node { original_map_pub_->publish(std::move(msg)); } rclcpp::TimerBase::SharedPtr timer_; - size_t count_{0}; rclcpp::Publisher::SharedPtr original_map_pub_; std::shared_ptr map_; - // void MapPublishOnce(ros::Publisher &pub, grid_map::GridMap &map) { - // map.setTimestamp(ros::Time::now().toNSec()); - // pub.publish(message); - // } }; int main(int argc, char **argv) { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..08aad80 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,13 @@ +# Add gtest based cpp test target and link libraries +find_package(ament_cmake_gtest) +ament_add_gtest(${PROJECT_NAME}-test + main.cpp + test_grid_map_geo.cpp +) + + +target_link_libraries(${PROJECT_NAME}-test + ${PROJECT_NAME} + # ${YAML_CPP_LIBRARIES} + # ${GDAL_LIBRARY} +) diff --git a/test/test_grid_map_geo.cpp b/test/test_grid_map_geo.cpp index 05bad62..cb1c005 100644 --- a/test/test_grid_map_geo.cpp +++ b/test/test_grid_map_geo.cpp @@ -1,15 +1,10 @@ -#include "grid_map_geo/transform.h" +#include "grid_map_geo/transform.hpp" #include #include TEST(GridMapTest, geoTransform) { - // Depending on Gdal versions, lon lat order are reversed -#if GDAL_VERSION_MAJOR > 2 Eigen::Vector3d berghaus_wgs84(46.7209147, 9.9219592, 488.0); -#else - Eigen::Vector3d berghaus_wgs84(9.9219592, 46.7209147, 488.0); -#endif Eigen::Vector3d berghaus_lv03(789823.96735451114, 177416.47911055354, 440.3752994351089); Eigen::Vector3d tranformed_lv03 = transformCoordinates(ESPG::WGS84, ESPG::CH1903_LV03, berghaus_wgs84); EXPECT_NEAR(tranformed_lv03(0), berghaus_lv03(0), 0.0001); From cdddb2ee97a9b17791cc0b59b37a51cb7f29eb78 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 21 Oct 2023 19:25:40 -0500 Subject: [PATCH 02/14] Port to ROS 2 Signed-off-by: Ryan Friedman --- src/test_tif_loader.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp index 1c8636d..8e05208 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -61,7 +61,9 @@ class MapPublisher : public rclcpp::Node { } private: void timer_callback() { - auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap()); + const auto now = node->now(); + map.setTimestamp(now.nanoseconds()); + const auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap()); original_map_pub_->publish(std::move(msg)); } rclcpp::TimerBase::SharedPtr timer_; From afaeac930bf975313e72979fc8cc5820aa12442a Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sun, 22 Oct 2023 11:22:50 -0600 Subject: [PATCH 03/14] Get launch file running aside from resources * Update docs * Remove ROS1 cruft Signed-off-by: Ryan Friedman --- src/test_tif_loader.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp index 8e05208..1c8636d 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -61,9 +61,7 @@ class MapPublisher : public rclcpp::Node { } private: void timer_callback() { - const auto now = node->now(); - map.setTimestamp(now.nanoseconds()); - const auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap()); + auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap()); original_map_pub_->publish(std::move(msg)); } rclcpp::TimerBase::SharedPtr timer_; From af1fc2a6ea90e6f7903d00a857686228cc67b8b0 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 24 Oct 2023 23:46:48 +0100 Subject: [PATCH 04/14] ros2: update gdal include path if __APPLE__ Signed-off-by: Rhys Mainwaring --- include/grid_map_geo/grid_map_geo.hpp | 9 +++++++++ include/grid_map_geo/transform.hpp | 7 +++++++ 2 files changed, 16 insertions(+) diff --git a/include/grid_map_geo/grid_map_geo.hpp b/include/grid_map_geo/grid_map_geo.hpp index c2b962c..aef380a 100644 --- a/include/grid_map_geo/grid_map_geo.hpp +++ b/include/grid_map_geo/grid_map_geo.hpp @@ -39,11 +39,20 @@ #include #include +#if __APPLE__ +#include +#include +#include +#include +#include +#else #include #include #include #include #include +#endif + #include struct Location { ESPG espg{ESPG::WGS84}; diff --git a/include/grid_map_geo/transform.hpp b/include/grid_map_geo/transform.hpp index e99531e..d03006e 100644 --- a/include/grid_map_geo/transform.hpp +++ b/include/grid_map_geo/transform.hpp @@ -34,10 +34,17 @@ #ifndef GRID_MAP_GEO_TRANSFORM_H #define GRID_MAP_GEO_TRANSFORM_H +#if __APPLE__ +#include +#include +#include +#include +#else #include #include #include #include +#endif #include From 9f845000e44b869c771f862d58ad0c3f5936312d Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sat, 28 Oct 2023 21:28:58 +0100 Subject: [PATCH 05/14] ros2: consolidate cmake files and install executable Signed-off-by: Rhys Mainwaring --- CMakeLists.txt | 77 ++++++++++++++++++++++++++++++++-------------- src/CMakeLists.txt | 10 ------ 2 files changed, 54 insertions(+), 33 deletions(-) delete mode 100644 src/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 39c5af0..fde3083 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,35 +15,74 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(GDAL 3 REQUIRED) find_package(ament_cmake REQUIRED) +find_package(grid_map_msgs REQUIRED) find_package(grid_map_ros REQUIRED) find_package(grid_map_core REQUIRED) +find_package(rclcpp REQUIRED) + find_package(Eigen3 REQUIRED) +find_package(GDAL 3 REQUIRED) -add_library(${PROJECT_NAME}) -add_subdirectory(src) - -target_include_directories(${PROJECT_NAME} - PUBLIC - "$" - "$" -) # Reverse compatability for GDAL<3.5 # https://gdal.org/development/cmake.html if(NOT TARGET GDAL::GDAL) - add_library(GDAL::GDAL ALIAS ${GDAL_LIBRARY}) + add_library(GDAL::GDAL ALIAS ${GDAL_LIBRARY}) endif() -target_link_libraries(${PROJECT_NAME} Eigen3::Eigen GDAL::GDAL grid_map_core::grid_map_core) +# Build +add_library(${PROJECT_NAME} + src/grid_map_geo.cpp +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" + ${GDAL_INCLUDE_DIR} +) + +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen GDAL::GDAL) +ament_target_dependencies(${PROJECT_NAME} SYSTEM + grid_map_core + grid_map_ros +) + +add_executable(test_tif_loader + src/test_tif_loader.cpp +) + +target_include_directories(test_tif_loader + PRIVATE + include + ${GDAL_INCLUDE_DIR} +) + +target_link_libraries(test_tif_loader + ${PROJECT_NAME} +) + +# Export ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies(GDAL) +# Install install( - DIRECTORY include/ - DESTINATION include + TARGETS + ${PROJECT_NAME} + test_tif_loader + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.hpp" ) install(DIRECTORY @@ -51,18 +90,10 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) -install( - TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME}Targets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - +# Test include(CTest) if(BUILD_TESTING) - add_subdirectory(test) + add_subdirectory(test) endif() ament_package() diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt deleted file mode 100644 index 9352845..0000000 --- a/src/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -target_sources(${PROJECT_NAME} PRIVATE grid_map_geo.cpp) - - -find_package(rclcpp REQUIRED) -find_package(grid_map_msgs REQUIRED) -find_package(grid_map_ros REQUIRED) -add_executable(test_tif_loader) -target_sources(test_tif_loader PRIVATE test_tif_loader.cpp) -target_link_libraries(test_tif_loader PRIVATE ${PROJECT_NAME} rclcpp::rclcpp ${grid_map_msgs_TARGETS}) -ament_target_dependencies(${PROJECT_NAME} grid_map_ros) \ No newline at end of file From 9992aff95e843baa8dca6c694589d32d131540bb Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sat, 28 Oct 2023 21:32:05 +0100 Subject: [PATCH 06/14] ros2: update rviz to rviz2 in launch file Signed-off-by: Rhys Mainwaring --- launch/load_tif_launch.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/load_tif_launch.xml b/launch/load_tif_launch.xml index 749a34d..342e42b 100644 --- a/launch/load_tif_launch.xml +++ b/launch/load_tif_launch.xml @@ -1,5 +1,5 @@ - + @@ -8,7 +8,7 @@ - - + + From caf822b32fff223086613f383a72c963050c4e13 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sat, 28 Oct 2023 21:44:32 +0100 Subject: [PATCH 07/14] ros2: add arguments and rename python launch file Signed-off-by: Rhys Mainwaring --- launch/load_tif.launch.py | 86 +++++++++++++++++++++++++++++++++++++++ launch/run.launch.py | 30 -------------- 2 files changed, 86 insertions(+), 30 deletions(-) create mode 100644 launch/load_tif.launch.py delete mode 100644 launch/run.launch.py diff --git a/launch/load_tif.launch.py b/launch/load_tif.launch.py new file mode 100644 index 0000000..b7abb18 --- /dev/null +++ b/launch/load_tif.launch.py @@ -0,0 +1,86 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + + +def generate_launch_description(): + """Generate a launch description.""" + + pkg_grid_map_geo = get_package_share_directory("grid_map_geo") + + # static transform node + static_transform_publisher = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="world_map", + arguments=[ + "--frame-id", + "world", + "--child-frame-id", + "map", + ], + ) + + # tif loader node + tif_loader = Node( + package="grid_map_geo", + # namespace="grid_map_geo", + executable="test_tif_loader", + name="tif_loader", + parameters=[ + {"tif_path": LaunchConfiguration("tif_path")}, + {"color_path": LaunchConfiguration("color_path")}, + ], + output="screen", + emulate_tty=True, + ) + + # rviz node + rviz = Node( + package="rviz2", + executable="rviz2", + arguments=["-d", os.path.join(pkg_grid_map_geo, "launch", "config.rviz")], + condition=IfCondition(LaunchConfiguration("rviz")), + ) + + default_location = "sargans" + default_tif_file = "sertig.tif" + default_color_file = "sertig_color.tif" + # default_tif_file = "hinwil.tif" + # default_color_file = "hinwil_color.tif" + return LaunchDescription( + [ + DeclareLaunchArgument( + "rviz", default_value="true", description="Open RViz." + ), + DeclareLaunchArgument( + "location", + default_value=default_location, + description="Location.", + ), + DeclareLaunchArgument( + "tif_path", + default_value=os.path.join( + pkg_grid_map_geo, "resources", default_tif_file + ), + description="Full path to the elevation map file.", + ), + DeclareLaunchArgument( + "color_path", + default_value=os.path.join( + pkg_grid_map_geo, "resources", default_color_file + ), + description="Full path to the elevation texture file.", + ), + static_transform_publisher, + tif_loader, + rviz, + ] + ) diff --git a/launch/run.launch.py b/launch/run.launch.py deleted file mode 100644 index 69c2e6c..0000000 --- a/launch/run.launch.py +++ /dev/null @@ -1,30 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import os - -def generate_launch_description(): - package_dir = get_package_share_directory('grid_map_geo') - tif_loader = Node( - package='grid_map_geo', - namespace='grid_map_geo', - executable='test_tif_loader', - name='tif_loader', - output="screen", - emulate_tty=True, - parameters=[ - {"tif_path": "/home/jaeyoung/dev/terrain-models/models/hinwil.tif"}, - {"tif_color_path": "/home/jaeyoung/dev/terrain-models/models/hinwil_color.tif"}, - ] - ) - rviz = Node( - package='rviz2', - namespace='', - executable='rviz2', - name='rviz2', - arguments=['-d', [os.path.join(package_dir, 'launch/config.rviz')]] - ) - return LaunchDescription([ - tif_loader, - rviz -]) From 5894c353f5608ad50a2b921f91ccfafd903df823 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sat, 28 Oct 2023 22:15:12 +0100 Subject: [PATCH 08/14] macOS: link directory workaround for yaml-cpp on aarch64 Signed-off-by: Rhys Mainwaring --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index fde3083..5b8c7b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,10 @@ find_package(rclcpp REQUIRED) find_package(Eigen3 REQUIRED) find_package(GDAL 3 REQUIRED) +# macOS +if(APPLE) + link_directories(/opt/homebrew/opt/yaml-cpp/lib) +endif() # Reverse compatability for GDAL<3.5 # https://gdal.org/development/cmake.html From 57b435079699ff6a3a1e6c3188e1f1c9aceb2708 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sun, 29 Oct 2023 00:14:11 +0100 Subject: [PATCH 09/14] ros2: update launch files to use correct param name for tif_color_path Signed-off-by: Rhys Mainwaring --- launch/load_tif.launch.py | 12 +++++------- launch/load_tif_launch.xml | 4 ++-- src/test_tif_loader.cpp | 3 +++ 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/launch/load_tif.launch.py b/launch/load_tif.launch.py index b7abb18..a60aeec 100644 --- a/launch/load_tif.launch.py +++ b/launch/load_tif.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): name="tif_loader", parameters=[ {"tif_path": LaunchConfiguration("tif_path")}, - {"color_path": LaunchConfiguration("color_path")}, + {"tif_color_path": LaunchConfiguration("tif_color_path")}, ], output="screen", emulate_tty=True, @@ -51,10 +51,8 @@ def generate_launch_description(): ) default_location = "sargans" - default_tif_file = "sertig.tif" - default_color_file = "sertig_color.tif" - # default_tif_file = "hinwil.tif" - # default_color_file = "hinwil_color.tif" + default_tif_file = "sargans.tif" + default_tif_color_file = "sargans_color.tif" return LaunchDescription( [ DeclareLaunchArgument( @@ -73,9 +71,9 @@ def generate_launch_description(): description="Full path to the elevation map file.", ), DeclareLaunchArgument( - "color_path", + "tif_color_path", default_value=os.path.join( - pkg_grid_map_geo, "resources", default_color_file + pkg_grid_map_geo, "resources", default_tif_color_file ), description="Full path to the elevation texture file.", ), diff --git a/launch/load_tif_launch.xml b/launch/load_tif_launch.xml index 342e42b..3f143aa 100644 --- a/launch/load_tif_launch.xml +++ b/launch/load_tif_launch.xml @@ -4,8 +4,8 @@ - - + + diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp index 1c8636d..9132f77 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -55,6 +55,9 @@ class MapPublisher : public rclcpp::Node { std::string file_path = this->declare_parameter("tif_path", "."); std::string color_path = this->declare_parameter("tif_color_path", "."); + RCLCPP_INFO_STREAM(get_logger(), "file_path " << file_path); + RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path); + map_ = std::make_shared(); map_->Load(file_path, false, color_path); timer_ = this->create_wall_timer(5s, std::bind(&MapPublisher::timer_callback, this)); From d00e64549898e2cec0d7f645a99324ec537f53dc Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sun, 29 Oct 2023 08:32:46 +0000 Subject: [PATCH 10/14] ros2: CMakeLists.txt improvements Signed-off-by: Rhys Mainwaring --- CMakeLists.txt | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b8c7b2..17794c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,8 +25,13 @@ find_package(Eigen3 REQUIRED) find_package(GDAL 3 REQUIRED) # macOS -if(APPLE) - link_directories(/opt/homebrew/opt/yaml-cpp/lib) +if(${CMAKE_SYSTEM_NAME} MATCHES Darwin) + if(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm64) + link_directories(/opt/homebrew/opt/yaml-cpp/lib) + endif() + if(${CMAKE_SYSTEM_PROCESSOR} MATCHES x84_64) + link_directories(/usr/local/opt/yaml-cpp/lib) + endif() endif() # Reverse compatability for GDAL<3.5 @@ -44,7 +49,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" - ${GDAL_INCLUDE_DIR} ) target_link_libraries(${PROJECT_NAME} Eigen3::Eigen GDAL::GDAL) @@ -61,7 +65,6 @@ add_executable(test_tif_loader target_include_directories(test_tif_loader PRIVATE include - ${GDAL_INCLUDE_DIR} ) target_link_libraries(test_tif_loader From 8e2042b3c49c0985399000ffa8960b996da59bb0 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sun, 29 Oct 2023 08:38:33 +0000 Subject: [PATCH 11/14] ros2: remove target_include_directories for test_tif_loader Signed-off-by: Rhys Mainwaring --- CMakeLists.txt | 5 ----- 1 file changed, 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 17794c4..cdfd357 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,11 +62,6 @@ add_executable(test_tif_loader src/test_tif_loader.cpp ) -target_include_directories(test_tif_loader - PRIVATE - include -) - target_link_libraries(test_tif_loader ${PROJECT_NAME} ) From f55db93ccb1c8ce8e963a4b86b77580ba48360b9 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 1 Nov 2023 15:55:24 +0000 Subject: [PATCH 12/14] ros2: add dependency on yaml_cpp_vendor Signed-off-by: Rhys Mainwaring --- CMakeLists.txt | 11 ++--------- package.xml | 1 + 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cdfd357..96011c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,15 +24,8 @@ find_package(rclcpp REQUIRED) find_package(Eigen3 REQUIRED) find_package(GDAL 3 REQUIRED) -# macOS -if(${CMAKE_SYSTEM_NAME} MATCHES Darwin) - if(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm64) - link_directories(/opt/homebrew/opt/yaml-cpp/lib) - endif() - if(${CMAKE_SYSTEM_PROCESSOR} MATCHES x84_64) - link_directories(/usr/local/opt/yaml-cpp/lib) - endif() -endif() +find_package(yaml_cpp_vendor REQUIRED) +link_directories(${yaml_cpp_vendor_LIBRARY_DIRS}) # Reverse compatability for GDAL<3.5 # https://gdal.org/development/cmake.html diff --git a/package.xml b/package.xml index 823838d..b996158 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ grid_map_ros libgdal-dev rclcpp + yaml_cpp_vendor ros2launch From 8d533d1fed34e80cf32a50a3f85fecffda2ab141 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 1 Nov 2023 15:55:56 +0000 Subject: [PATCH 13/14] ros2: restore namespace to tif_loader node in launch file Signed-off-by: Rhys Mainwaring --- launch/load_tif.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/load_tif.launch.py b/launch/load_tif.launch.py index a60aeec..e69628d 100644 --- a/launch/load_tif.launch.py +++ b/launch/load_tif.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): # tif loader node tif_loader = Node( package="grid_map_geo", - # namespace="grid_map_geo", + namespace="grid_map_geo", executable="test_tif_loader", name="tif_loader", parameters=[ From 01089c718717268a9bb9054ee5b81f0555969c65 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Fri, 3 Nov 2023 20:53:27 +0000 Subject: [PATCH 14/14] ros2: exclude executable from target exports and fix header install directory Signed-off-by: Rhys Mainwaring --- CMakeLists.txt | 31 +++++++++++++++++++++---------- test/CMakeLists.txt | 11 +++++------ 2 files changed, 26 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 96011c6..89167a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,25 +59,36 @@ target_link_libraries(test_tif_loader ${PROJECT_NAME} ) -# Export -ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) -ament_export_dependencies(GDAL) - # Install +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + install( TARGETS ${PROJECT_NAME} - test_tif_loader - EXPORT ${PROJECT_NAME}Targets + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib/${PROJECT_NAME} LIBRARY DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} ) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(GDAL) + +# NOTE: if RUNTIME DESTINATION is set to bin/${PROJECT_NAME} the executable +# is not found by ros2 launch: +# +# Caught exception in launch (see debug for traceback): executable +# 'test_tif_loader' not found on the libexec directory +# '/ros2-aerial/install/grid_map_geo/lib/grid_map_geo' install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} - FILES_MATCHING PATTERN "*.hpp" + TARGETS + test_tif_loader + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 08aad80..eaea08e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,13 +1,12 @@ # Add gtest based cpp test target and link libraries find_package(ament_cmake_gtest) ament_add_gtest(${PROJECT_NAME}-test - main.cpp - test_grid_map_geo.cpp + main.cpp + test_grid_map_geo.cpp ) - target_link_libraries(${PROJECT_NAME}-test - ${PROJECT_NAME} - # ${YAML_CPP_LIBRARIES} - # ${GDAL_LIBRARY} + ${PROJECT_NAME} + # ${YAML_CPP_LIBRARIES} + # ${GDAL_LIBRARY} )