i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2024-01-03 18:11:16 -08:00
parent 2f365ab7e1
commit 80ae448d1b
14 changed files with 493 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/CMakeLists.txt
+22 -1
View File
@@ -29,6 +29,7 @@ find_package(rtabmap_conversions REQUIRED)
# Optional components
find_package(octomap_msgs)
find_package(grid_map_ros)
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/include
@@ -75,7 +76,7 @@ SET(rtabmap_util_plugins_lib_src
)
# If octomap is found, add definition
# If octomap is found, add dependency
IF(octomap_msgs_FOUND)
MESSAGE(STATUS "WITH octomap_msgs")
include_directories(
@@ -88,6 +89,18 @@ SET(Libraries
ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS")
ENDIF(octomap_msgs_FOUND)
# If grid_map is found, add dependency
IF(grid_map_ros_FOUND)
MESSAGE(STATUS "WITH grid_map_ros")
include_directories(
${grid_map_ros_INCLUDE_DIRS}
)
SET(Libraries
grid_map_ros
${Libraries}
)
ENDIF(grid_map_ros_FOUND)
############################
## Declare a cpp library
############################
@@ -100,6 +113,14 @@ target_include_directories(rtabmap_util_plugins
$<INSTALL_INTERFACE:include>
)
IF(octomap_msgs_FOUND)
target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_OCTOMAP_MSGS)
ENDIF(octomap_msgs_FOUND)
IF(grid_map_ros_FOUND)
target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_GRID_MAP_ROS)
ENDIF(grid_map_ros_FOUND)
ament_target_dependencies(rtabmap_util_plugins ${Libraries})
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay")
rtabmap_util/include/rtabmap_util/MapsManager.h
+13 -2
View File
@@ -38,10 +38,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#ifdef RTABMAP_OCTOMAP
#ifdef WITH_OCTOMAP_MSGS
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
#include <octomap_msgs/msg/octomap.hpp>
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
#include <grid_map_msgs/msg/grid_map.hpp>
#endif
namespace rtabmap {
@@ -49,6 +51,7 @@ class OctoMap;
class Memory;
class OccupancyGrid;
class LocalGridMaker;
class GridMap;
} // namespace rtabmap
@@ -126,6 +129,9 @@ private:
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr octoMapEmptySpace_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr octoMapProj_;
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr elevationMapPub_;
#endif
std::map<int, rtabmap::Transform> assembledGroundPoses_;
std::map<int, rtabmap::Transform> assembledObstaclePoses_;
@@ -148,6 +154,11 @@ private:
int octomapTreeDepth_;
bool octomapUpdated_;
#ifdef RTABMAP_GRIDMAP
rtabmap::GridMap * elevationMap_;
#endif
bool elevationMapUpdated_;
rtabmap::ParametersMap parameters_;
bool latching_;
rtabmap_util/package.xml
+2 -1
View File
@@ -30,7 +30,8 @@
<depend>message_filters</depend>
<depend>rtabmap_msgs</depend>
<depend>rtabmap_conversions</depend>
<depend>grid_map_ros</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
rtabmap_util/src/MapsManager.cpp
+76 -14
View File
@@ -51,6 +51,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OctoMap.h>
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <rtabmap/core/global_map/GridMap.h>
#endif
using namespace rtabmap;
namespace rtabmap_util {
@@ -74,6 +79,10 @@ MapsManager::MapsManager() :
#endif
octomapTreeDepth_(16),
octomapUpdated_(true),
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMap_(new GridMap(&localMaps_)),
#endif
elevationMapUpdated_(true),
latching_(true)
{
}
@@ -96,6 +105,7 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
// connect
latching_ = node.declare_parameter("latch", rclcpp::ParameterValue(latching_)).get<bool>();
RCLCPP_INFO(node.get_logger(), "%s(maps): latch = %s", name.c_str(), latching_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_);
RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_);
RCLCPP_INFO(node.get_logger(), "%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false");
@@ -105,7 +115,6 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_);
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
octomapTreeDepth_ = node.declare_parameter("octomap_tree_depth", rclcpp::ParameterValue(octomapTreeDepth_)).get<int>();
if(octomapTreeDepth_ > 16)
@@ -120,9 +129,6 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
}
RCLCPP_INFO(node.get_logger(), "%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_);
#endif
#endif
// mapping topics
latched_.clear();
@@ -157,6 +163,11 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
octoMapProj_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapProj_, false));
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMapPub_ = node.create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&elevationMapPub_, false));
#endif
}
MapsManager::~MapsManager() {
@@ -168,6 +179,9 @@ MapsManager::~MapsManager() {
#ifdef RTABMAP_OCTOMAP
delete octomap_;
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
delete elevationMap_;
#endif
}
void parameterMoved(
@@ -236,13 +250,16 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
parameters_ = parameters;
delete occupancyGrid_;
occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_);
localMapMaker_->parseParameters(parameters_);
#ifdef RTABMAP_OCTOMAP
delete octomap_;
octomap_ = new OctoMap(&localMaps_, parameters_);
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
delete elevationMap_;
elevationMap_ = new GridMap(&localMaps_, parameters_);
#endif
}
void MapsManager::set2DMap(
@@ -299,9 +316,15 @@ void MapsManager::clear()
groundClouds_.clear();
obstacleClouds_.clear();
occupancyGrid_->clear();
#ifdef RTABMAP_OCTOMAP
octomap_->clear();
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMap_->clear();
#endif
for(std::map<void*, bool>::iterator iter=latched_.begin(); iter!=latched_.end(); ++iter)
{
iter->second = false;
@@ -327,6 +350,9 @@ bool MapsManager::hasSubscribers() const
octoMapGroundCloud_->get_subscription_count() != 0 ||
octoMapEmptySpace_->get_subscription_count() != 0 ||
octoMapProj_->get_subscription_count() != 0
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
|| elevationMapPub_->get_subscription_count() != 0
#endif
;
}
@@ -361,7 +387,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
const std::map<int, rtabmap::Signature> & signatures)
{
bool updateGridCache = updateGrid || updateOctomap;
if(!updateGrid && !updateOctomap)
bool updateElevation = false;
if(!updateGrid && !updateOctomap && !updateOctomap)
{
// all false, update only those where we have subscribers
#ifdef RTABMAP_OCTOMAP
@@ -378,21 +405,29 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
octoMapProj_->get_subscription_count() != 0;
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
updateElevation = elevationMapPub_->get_subscription_count() != 0;
#endif
updateGrid = gridMapPub_->get_subscription_count() != 0 ||
gridProbMapPub_->get_subscription_count() != 0;
updateGridCache = updateOctomap || updateGrid ||
updateGridCache = updateOctomap || updateGrid || updateElevation ||
cloudMapPub_->get_subscription_count() != 0 ||
cloudObstaclesPub_->get_subscription_count() != 0 ||
cloudGroundPub_->get_subscription_count() != 0;
}
#if !defined(WITH_OCTOMAP_MSGS) and !defined(RTABMAP_OCTOMAP)
#if not (defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP))
updateOctomap = false;
#endif
#if not (defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP))
updateElevation = false;
#endif
gridUpdated_ = updateGrid;
octomapUpdated_ = updateOctomap;
elevationMapUpdated_ = updateElevation;
UDEBUG("Updating map caches...");
@@ -595,6 +630,18 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
if(updateElevation)
{
UTimer time;
elevationMapUpdated_ = elevationMap_->update(filteredPoses);
UINFO("GridMap (elevation map) update time = %fs", time.ticks());
}
#endif
localMaps_.clear(true);
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();
iter!=groundClouds_.end();)
{
@@ -796,10 +843,6 @@ void MapsManager::publishMaps(
}
++countObstacles;
}
else
{
//std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
}
}
}
double addingPointsTime = t.ticks();
@@ -1218,7 +1261,6 @@ void MapsManager::publishMaps(
{
latched_.at(&octoMapProj_) = false;
}
#endif
if( gridUpdated_ ||
@@ -1314,7 +1356,27 @@ void MapsManager::publishMaps(
{
latched_.at(&gridProbMapPub_) = false;
}
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
if( elevationMapUpdated_ ||
!latching_ ||
(elevationMapPub_->get_subscription_count() && !latched_.at(&elevationMapPub_)))
{
grid_map_msgs::msg::GridMap::UniquePtr msg;
msg = grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap());
msg->header.frame_id = mapFrameId;
msg->header.stamp = stamp;
elevationMapPub_->publish(std::move(msg));
}
if(elevationMapPub_->get_subscription_count() == 0)
{
latched_.at(&elevationMapPub_) = false;
}
if( mapCacheCleanup_ &&
elevationMapPub_->get_subscription_count() == 0)
{
elevationMap_->clear();
}
#endif
if(!this->hasSubscribers() && mapCacheCleanup_)
{
if(!localMaps_.empty())
rtabmap_util/src/nodelets/obstacles_detection.cpp
+1 -2
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <rtabmap/core/LocalGridMaker.h>
#include <rtabmap_conversions/MsgConversion.h>
@@ -85,8 +86,6 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
}
void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
{
rclcpp::Time time = now();