From 2a1e9607f07ceac77ca7b4ed1aeda4dc6b5ca4d0 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 14 Sep 2024 00:18:47 +0200 Subject: [PATCH 1/5] demo launch file: add "mm_file" as a proper launch required argument instead of an optional env variable --- mrpt_tutorials/launch/demo_map_server_from_mm.launch.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/mrpt_tutorials/launch/demo_map_server_from_mm.launch.py b/mrpt_tutorials/launch/demo_map_server_from_mm.launch.py index 8db1255..dd39265 100644 --- a/mrpt_tutorials/launch/demo_map_server_from_mm.launch.py +++ b/mrpt_tutorials/launch/demo_map_server_from_mm.launch.py @@ -16,12 +16,16 @@ def generate_launch_description(): tutsDir = get_package_share_directory("mrpt_tutorials") + arg_mm_file = DeclareLaunchArgument( + name='mm_file' + ) + mrpt_map_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('mrpt_map_server'), 'launch', 'mrpt_map_server.launch.py')]), launch_arguments={ - 'mm_file': os.environ.get('MM_FILE', 'default.mm'), + 'mm_file': LaunchConfiguration('mm_file'), }.items() ) @@ -34,5 +38,6 @@ def generate_launch_description(): ) return LaunchDescription([ mrpt_map_launch, + arg_mm_file, rviz2_node ]) From 6a1ea4aeadc32450038ff505f835daf05188e8f3 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 14 Sep 2024 01:02:40 +0200 Subject: [PATCH 2/5] Add new msg GeoreferencingMetadata.msg --- mrpt_nav_interfaces/CMakeLists.txt | 23 +++++++++++-------- .../msg/GeoreferencingMetadata.msg | 18 +++++++++++++++ mrpt_nav_interfaces/package.xml | 1 + 3 files changed, 32 insertions(+), 10 deletions(-) create mode 100644 mrpt_nav_interfaces/msg/GeoreferencingMetadata.msg diff --git a/mrpt_nav_interfaces/CMakeLists.txt b/mrpt_nav_interfaces/CMakeLists.txt index d3b8432..aec4705 100644 --- a/mrpt_nav_interfaces/CMakeLists.txt +++ b/mrpt_nav_interfaces/CMakeLists.txt @@ -15,21 +15,24 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(mrpt_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "action/NavigateGoal.action" - "action/NavigateWaypoints.action" - "msg/NavigationFeedback.msg" - "msg/NavigationFinalStatus.msg" - "srv/GetLayers.srv" - "srv/GetGridmapLayer.srv" - "srv/GetPointmapLayer.srv" - "srv/MakePlanFromTo.srv" - "srv/MakePlanTo.srv" - DEPENDENCIES + action/NavigateGoal.action + action/NavigateWaypoints.action + msg/GeoreferencingMetadata.msg + msg/NavigationFeedback.msg + msg/NavigationFinalStatus.msg + srv/GetLayers.srv + srv/GetGridmapLayer.srv + srv/GetPointmapLayer.srv + srv/MakePlanFromTo.srv + srv/MakePlanTo.srv +DEPENDENCIES std_msgs nav_msgs mrpt_msgs + geometry_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/mrpt_nav_interfaces/msg/GeoreferencingMetadata.msg b/mrpt_nav_interfaces/msg/GeoreferencingMetadata.msg new file mode 100644 index 0000000..5f7e5da --- /dev/null +++ b/mrpt_nav_interfaces/msg/GeoreferencingMetadata.msg @@ -0,0 +1,18 @@ +# For a diagram of the frames defined in this message, refer to online docs: +# https://docs.mola-slam.org/latest/geo-referencing.html + +# This is true only if the map is georeferenced. If it is false, +# the rest of the fields in this message should be ignored. +bool valid + +geometry_msgs/PoseWithCovariance t_enu_to_map +geometry_msgs/Pose t_enu_to_utm + +# The geodetic coordinates of the ENU reference frame: +float64 latitude +float64 longitude +float64 height + +# UTM zone, for the ENU point. +int32 utm_zone +string utm_band # just one letter (or empty if valid=false) diff --git a/mrpt_nav_interfaces/package.xml b/mrpt_nav_interfaces/package.xml index 772cf25..3f5df67 100644 --- a/mrpt_nav_interfaces/package.xml +++ b/mrpt_nav_interfaces/package.xml @@ -15,6 +15,7 @@ rosidl_default_generators action_msgs + geometry_msgs mrpt_msgs nav_msgs From 3222b0a00277f3b61e4dc233ec7d3e82c7fce679 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 14 Sep 2024 01:03:19 +0200 Subject: [PATCH 3/5] mrpt_map_server now publishes the map georeferenciation metadata, as topics and /tf (frames: utm, enu) --- mrpt_map_server/CMakeLists.txt | 8 ++ mrpt_map_server/README.md | 3 +- .../include/mrpt_map_server/map_server_node.h | 28 +++--- mrpt_map_server/package.xml | 4 + mrpt_map_server/src/map_server_node.cpp | 96 ++++++++++++++++++- 5 files changed, 124 insertions(+), 15 deletions(-) diff --git a/mrpt_map_server/CMakeLists.txt b/mrpt_map_server/CMakeLists.txt index 9067926..5db864f 100644 --- a/mrpt_map_server/CMakeLists.txt +++ b/mrpt_map_server/CMakeLists.txt @@ -8,9 +8,13 @@ find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) find_package(mrpt_msgs REQUIRED) find_package(mrpt_nav_interfaces REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(mrpt-maps REQUIRED) find_package(mrpt-ros2bridge REQUIRED) +find_package(mrpt-topography REQUIRED) # shipped by ROS pkg mrpt_libobs find_package(mp2p_icp_map REQUIRED) message(STATUS "MRPT_VERSION: ${MRPT_VERSION}") @@ -56,6 +60,7 @@ target_include_directories(map_server_node target_link_libraries(map_server_node mrpt::maps mrpt::ros2bridge + mrpt::topography mola::mp2p_icp_map ) @@ -66,6 +71,9 @@ ament_target_dependencies( nav_msgs mrpt_msgs mrpt_nav_interfaces + tf2 + tf2_ros + tf2_geometry_msgs ) ############# diff --git a/mrpt_map_server/README.md b/mrpt_map_server/README.md index ec52bae..45d9c7b 100644 --- a/mrpt_map_server/README.md +++ b/mrpt_map_server/README.md @@ -64,6 +64,7 @@ None. ### Published topics * ``${pub_mm_topic}/metric_map`` (Default: ``mrpt_map/metric_map``) (``mrpt_msgs::msg::GenericObject``) (topic name can be changed with parameter `pub_mm_topic`). * ``${pub_mm_topic}/geo_ref`` (Default: ``mrpt_map/geo_ref``) (``mrpt_msgs::msg::GenericObject``). An MRPT-serialization of ``mp2p_icp::metric_map_t::Georeferencing`` metadata (topic name can be changed with parameter `pub_mm_topic`). +* ``${pub_mm_topic}/geo_ref_metadata`` (Default: ``mrpt_map/geo_ref_metadata``)(``mrpt_nav_interfaces::msgs::msg::GeoreferencingMetadata``). A ROS plain message with the contents of ``mp2p_icp::metric_map_t::Georeferencing`` metadata. * ``${pub_mm_topic}/`` (Default: ``mrpt_map/``) (``mrpt_msgs::msg::GenericObject``) * ``${pub_mm_topic}/_points`` (``sensor_msgs::msg::PointCloud2``), one per map layer. * ``${pub_mm_topic}/_gridmap`` (``nav_msgs::msg::OccupancyGrid``) @@ -126,5 +127,5 @@ Launch a map server from a custom `.mm` map ([launch file](../mrpt_tutorials/lau which in the launch file is read from the environment variable `MM_FILE`, so it can be used like: ```bash -MM_FILE=/path/to/my/map.mm ros2 launch mrpt_tutorials demo_map_server_from_mm.launch.py +ros2 launch mrpt_tutorials demo_map_server_from_mm.launch.py mm_file:=/path/to/my/map.mm ``` diff --git a/mrpt_map_server/include/mrpt_map_server/map_server_node.h b/mrpt_map_server/include/mrpt_map_server/map_server_node.h index a083828..8536a5c 100644 --- a/mrpt_map_server/include/mrpt_map_server/map_server_node.h +++ b/mrpt_map_server/include/mrpt_map_server/map_server_node.h @@ -11,16 +11,18 @@ #include #include #include - -#include "mrpt_msgs/msg/generic_object.hpp" -#include "mrpt_nav_interfaces/srv/get_gridmap_layer.hpp" -#include "mrpt_nav_interfaces/srv/get_layers.hpp" -#include "mrpt_nav_interfaces/srv/get_pointmap_layer.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/srv/get_map.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include class MapServer : public rclcpp::Node { @@ -38,8 +40,7 @@ class MapServer : public rclcpp::Node std::string frame_id_ = "map"; - rclcpp::Service::SharedPtr m_service_map; //!< service for map server - nav_msgs::srv::GetMap::Response m_response_ros; //!< response from the map server + std::shared_ptr tf_broadcaster_; /// metric map: will be used whatever is the incoming map format. mp2p_icp::metric_map_t theMap_; @@ -84,6 +85,9 @@ class MapServer : public rclcpp::Node // for the top-level mm metric map: PerTopicData pubMM_; + PerTopicData pubGeoRef_; + PerTopicData pubGeoRefMsg_; + // clang-format off // Binary form of each layer: std::map> pubLayers_; diff --git a/mrpt_map_server/package.xml b/mrpt_map_server/package.xml index 2d66a20..21ef606 100644 --- a/mrpt_map_server/package.xml +++ b/mrpt_map_server/package.xml @@ -14,11 +14,15 @@ ament_cmake mp2p_icp + mrpt_libobs mrpt_libmaps mrpt_libros_bridge mrpt_msgs mrpt_nav_interfaces rclcpp_components + tf2 + tf2_ros + tf2_geometry_msgs ament_cmake_lint_cmake ament_cmake_xmllint diff --git a/mrpt_map_server/src/map_server_node.cpp b/mrpt_map_server/src/map_server_node.cpp index c7a4375..f2b0d8d 100644 --- a/mrpt_map_server/src/map_server_node.cpp +++ b/mrpt_map_server/src/map_server_node.cpp @@ -6,8 +6,7 @@ | All rights reserved. Released under BSD 3-Clause license. See LICENSE | +------------------------------------------------------------------------+ */ -#include "mrpt_map_server/map_server_node.h" - +#include #include #include #include @@ -20,11 +19,15 @@ #include #include #include +#include #include #include #include // ASSERT_FILE_EXISTS_() +#include +#include #include +#include using namespace mrpt::config; using mrpt::maps::CMultiMetricMap; @@ -132,6 +135,9 @@ void MapServer::init() const mrpt_nav_interfaces::srv::GetPointmapLayer::Request::SharedPtr req, mrpt_nav_interfaces::srv::GetPointmapLayer::Response::SharedPtr res) { srv_get_pointmap(req, res); }); + + // Create TF broadcaster: + tf_broadcaster_ = std::make_shared(*this); } void MapServer::publish_map() @@ -237,6 +243,92 @@ void MapServer::publish_map() } } // end for each layer + + // Publish the geo-referenced metadata: + // 1/2: as mrpt serialized object + { + // If it's nullopt, it will be serialized correctly as such. + mrpt::io::CMemoryStream memBuf; + auto arch = mrpt::serialization::archiveFrom(memBuf); + + arch << theMap_.georeferencing; + + if (!pubGeoRef_.pub) + { + pubGeoRef_.pub = this->create_publisher( + pub_mm_topic_ + "/geo_ref"s, QoS); + } + + mrpt_msgs::msg::GenericObject msg; + msg.data.resize(memBuf.getTotalBytesCount()); + ::memcpy(msg.data.data(), memBuf.getRawBufferData(), msg.data.size()); + pubGeoRef_.pub->publish(msg); + } + + // Now, only if there is valid geo-ref data, publish /tf's for + // ENU and UTM: + // See: https://docs.mola-slam.org/latest/geo-referencing.html + + mrpt_nav_interfaces::msg::GeoreferencingMetadata geoMsg; + + if (theMap_.georeferencing) + { + // ENU -> MAP + const auto T_enu_to_map = theMap_.georeferencing->T_enu_to_map.mean; + geometry_msgs::msg::TransformStamped tfMsg; + + tfMsg.header.stamp = this->get_clock()->now(); + tfMsg.header.frame_id = "enu"; // parent + tfMsg.child_frame_id = "map"; // child + tfMsg.transform = tf2::toMsg(mrpt::ros2bridge::toROS_tfTransform(T_enu_to_map)); + tf_broadcaster_->sendTransform(tfMsg); + + // ENU -> UTM + mrpt::topography::TUTMCoords utmCoordsOfENU; + int utmZone = 0; + char utmBand = 0; + mrpt::topography::GeodeticToUTM( + theMap_.georeferencing->geo_coord, utmCoordsOfENU, utmZone, utmBand); + + // T_enu_to_utm = - utmCoordsOfENU (without rotation, both are "ENU") + const auto T_enu_to_utm = mrpt::poses::CPose3D::FromTranslation(-utmCoordsOfENU); + + tfMsg.header.frame_id = "enu"; // parent + tfMsg.child_frame_id = "utm"; // child + tfMsg.transform = tf2::toMsg(mrpt::ros2bridge::toROS_tfTransform(T_enu_to_utm)); + tf_broadcaster_->sendTransform(tfMsg); + + geoMsg.t_enu_to_map = + tf2::toMsg(mrpt::ros2bridge::toROS_Pose(theMap_.georeferencing->T_enu_to_map)); + geoMsg.t_enu_to_utm = tf2::toMsg(mrpt::ros2bridge::toROS_Pose(T_enu_to_utm)); + geoMsg.utm_zone = utmZone; + geoMsg.utm_band = std::string{utmBand}; + } + + // 2/2: as ROS msg + if (!pubGeoRefMsg_.pub) + { + pubGeoRefMsg_.pub = + this->create_publisher( + pub_mm_topic_ + "/geo_ref_metadata"s, QoS); + } + + if (theMap_.georeferencing) + { + auto g = *theMap_.georeferencing; + + geoMsg.valid = true; + + geoMsg.latitude = g.geo_coord.lat.decimal_value; + geoMsg.longitude = g.geo_coord.lon.decimal_value; + geoMsg.height = g.geo_coord.height; + // the rest of fields are already filled in above. + } + else + { + geoMsg.valid = false; + } + pubGeoRefMsg_.pub->publish(geoMsg); } void MapServer::loop() From 1e5e73905241ed45ad70891df4ad8949370f460e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 14 Sep 2024 01:08:48 +0200 Subject: [PATCH 4/5] CI: use testing repo --- .github/workflows/build-ros.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build-ros.yml b/.github/workflows/build-ros.yml index 9dacad3..05c5cb5 100644 --- a/.github/workflows/build-ros.yml +++ b/.github/workflows/build-ros.yml @@ -45,6 +45,7 @@ jobs: uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distribution }} + use-ros2-testing: true - name: Install rosdep dependencies run: | From b5236a3051c13ad41cc4ff32d29df4eb3ccbdf66 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 18 Sep 2024 16:31:24 +0200 Subject: [PATCH 5/5] Update package.xml: minimum required version of mp2p_icp --- mrpt_map_server/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mrpt_map_server/package.xml b/mrpt_map_server/package.xml index 21ef606..58bc8f6 100644 --- a/mrpt_map_server/package.xml +++ b/mrpt_map_server/package.xml @@ -13,7 +13,7 @@ ament_cmake - mp2p_icp + mp2p_icp mrpt_libobs mrpt_libmaps mrpt_libros_bridge