mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-09-16 08:10:19 +08:00
map_assembler node is now available on ROS2
This commit is contained in:
matlabbe
@@ -73,6 +73,7 @@ SET(rtabmap_util_plugins_lib_src
|
|||||||
src/nodelets/lidar_deskewing.cpp
|
src/nodelets/lidar_deskewing.cpp
|
||||||
src/nodelets/rgbd_relay.cpp
|
src/nodelets/rgbd_relay.cpp
|
||||||
src/nodelets/rgbd_split.cpp
|
src/nodelets/rgbd_split.cpp
|
||||||
|
src/nodelets/map_assembler.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -134,6 +135,7 @@ rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudT
|
|||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ObstaclesDetection")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ObstaclesDetection")
|
||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAggregator")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAggregator")
|
||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAssembler")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAssembler")
|
||||||
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::MapAssembler")
|
||||||
|
|
||||||
add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp)
|
add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp)
|
||||||
ament_target_dependencies(rtabmap_rgbd_relay ${Libraries})
|
ament_target_dependencies(rtabmap_rgbd_relay ${Libraries})
|
||||||
@@ -150,10 +152,10 @@ set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split")
|
|||||||
#target_link_libraries(rtabmap_map_optimizer rtabmap_util_plugins)
|
#target_link_libraries(rtabmap_map_optimizer rtabmap_util_plugins)
|
||||||
#set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer")
|
#set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer")
|
||||||
|
|
||||||
#add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp)
|
add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp)
|
||||||
#ament_target_dependencies(rtabmap_map_assembler ${Libraries})
|
ament_target_dependencies(rtabmap_map_assembler ${Libraries})
|
||||||
#target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins rtabmap_util_plugins)
|
target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins)
|
||||||
#set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler")
|
set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler")
|
||||||
|
|
||||||
add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp)
|
add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp)
|
||||||
ament_target_dependencies(rtabmap_imu_to_tf ${Libraries})
|
ament_target_dependencies(rtabmap_imu_to_tf ${Libraries})
|
||||||
@@ -241,7 +243,6 @@ install(TARGETS
|
|||||||
RUNTIME DESTINATION bin
|
RUNTIME DESTINATION bin
|
||||||
)
|
)
|
||||||
install(TARGETS
|
install(TARGETS
|
||||||
# rtabmap_map_assembler
|
|
||||||
# rtabmap_map_optimizer
|
# rtabmap_map_optimizer
|
||||||
# rtabmap_data_player
|
# rtabmap_data_player
|
||||||
# rtabmap_odom_msg_to_tf
|
# rtabmap_odom_msg_to_tf
|
||||||
@@ -256,6 +257,7 @@ install(TARGETS
|
|||||||
rtabmap_obstacles_detection
|
rtabmap_obstacles_detection
|
||||||
rtabmap_point_cloud_aggregator
|
rtabmap_point_cloud_aggregator
|
||||||
rtabmap_point_cloud_assembler
|
rtabmap_point_cloud_assembler
|
||||||
|
rtabmap_map_assembler
|
||||||
DESTINATION lib/${PROJECT_NAME}
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,104 @@
|
|||||||
|
/*
|
||||||
|
Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of the Universite de Sherbrooke nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <rtabmap_util/visibility.h>
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
|
||||||
|
#include <rtabmap_msgs/srv/get_map.hpp>
|
||||||
|
#include "rtabmap_msgs/msg/map_data.hpp"
|
||||||
|
#include "rtabmap_util/MapsManager.h"
|
||||||
|
|
||||||
|
#include <std_srvs/srv/empty.hpp>
|
||||||
|
|
||||||
|
#ifdef WITH_OCTOMAP_MSGS
|
||||||
|
#ifdef RTABMAP_OCTOMAP
|
||||||
|
#include <octomap_msgs/srv/get_octomap.hpp>
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
namespace rtabmap_util
|
||||||
|
{
|
||||||
|
|
||||||
|
class MapAssembler: public rclcpp::Node
|
||||||
|
{
|
||||||
|
|
||||||
|
public:
|
||||||
|
RTABMAP_UTIL_PUBLIC
|
||||||
|
explicit MapAssembler(const rclcpp::NodeOptions & options);
|
||||||
|
virtual ~MapAssembler();
|
||||||
|
|
||||||
|
private:
|
||||||
|
void mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg);
|
||||||
|
|
||||||
|
void processMapData(const rtabmap_msgs::msg::MapData & msg);
|
||||||
|
|
||||||
|
void reset(const std::shared_ptr<rmw_request_id_t>,
|
||||||
|
const std::shared_ptr<std_srvs::srv::Empty::Request>,
|
||||||
|
std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||||
|
|
||||||
|
void timerCallback();
|
||||||
|
|
||||||
|
#ifdef WITH_OCTOMAP_MSGS
|
||||||
|
#ifdef RTABMAP_OCTOMAP
|
||||||
|
void octomapBinaryCallback(
|
||||||
|
const std::shared_ptr<rmw_request_id_t>,
|
||||||
|
const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
|
||||||
|
std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res);
|
||||||
|
|
||||||
|
void octomapFullCallback(
|
||||||
|
const std::shared_ptr<rmw_request_id_t>,
|
||||||
|
const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
|
||||||
|
std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res);
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
private:
|
||||||
|
MapsManager mapsManager_;
|
||||||
|
std::map<int, rtabmap::Signature> nodes_;
|
||||||
|
std::map<int, rtabmap::Transform> optimizedPoses_;
|
||||||
|
std::string mapFrameId_;
|
||||||
|
std::string rtabmapNodeName_;
|
||||||
|
|
||||||
|
rclcpp::Subscription<rtabmap_msgs::msg::MapData>::SharedPtr mapDataSub_;
|
||||||
|
|
||||||
|
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resetService_;
|
||||||
|
|
||||||
|
rclcpp::CallbackGroup::SharedPtr serviceCbGroup_;
|
||||||
|
rclcpp::CallbackGroup::SharedPtr timerCbGroup_;
|
||||||
|
rclcpp::TimerBase::SharedPtr timer_;
|
||||||
|
rclcpp::Client<rtabmap_msgs::srv::GetMap>::SharedPtr client_;
|
||||||
|
|
||||||
|
#ifdef WITH_OCTOMAP_MSGS
|
||||||
|
#ifdef RTABMAP_OCTOMAP
|
||||||
|
rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapBinarySrv_;
|
||||||
|
rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapFullSrv_;
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
bool localGridsRegenerated_;
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
@@ -25,349 +25,17 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|||||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include "rtabmap_util/map_assembler.hpp"
|
||||||
#include "rtabmap_msgs/MapData.h"
|
|
||||||
#include "rtabmap_conversions/MsgConversion.h"
|
|
||||||
#include "rtabmap_util/MapsManager.h"
|
|
||||||
#include "rtabmap_msgs/GetMap.h"
|
|
||||||
#include <rtabmap/core/util3d_transforms.h>
|
|
||||||
#include <rtabmap/core/util3d.h>
|
|
||||||
#include <rtabmap/core/util3d_filtering.h>
|
|
||||||
#include <rtabmap/core/util3d_mapping.h>
|
|
||||||
#include <rtabmap/core/Compression.h>
|
|
||||||
#include <rtabmap/core/Graph.h>
|
|
||||||
#include <rtabmap/utilite/ULogger.h>
|
|
||||||
#include <rtabmap/utilite/UStl.h>
|
#include <rtabmap/utilite/UStl.h>
|
||||||
#include <rtabmap/utilite/UTimer.h>
|
|
||||||
#include <rtabmap/utilite/UFile.h>
|
|
||||||
#include <pcl_ros/transforms.h>
|
|
||||||
#include <pcl_conversions/pcl_conversions.h>
|
|
||||||
#include <nav_msgs/OccupancyGrid.h>
|
|
||||||
#include <std_srvs/Empty.h>
|
|
||||||
|
|
||||||
#ifdef WITH_OCTOMAP_MSGS
|
int main(int argc, char **argv)
|
||||||
#ifdef RTABMAP_OCTOMAP
|
|
||||||
#include <octomap_msgs/GetOctomap.h>
|
|
||||||
#include <octomap_msgs/conversions.h>
|
|
||||||
#include <rtabmap/core/OctoMap.h>
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
|
|
||||||
using namespace rtabmap;
|
|
||||||
|
|
||||||
class MapAssembler
|
|
||||||
{
|
{
|
||||||
|
|
||||||
public:
|
|
||||||
MapAssembler(int & argc, char** argv) :
|
|
||||||
localGridsRegenerated_(false)
|
|
||||||
{
|
|
||||||
ros::NodeHandle pnh("~");
|
|
||||||
ros::NodeHandle nh;
|
|
||||||
|
|
||||||
std::string configPath;
|
|
||||||
pnh.param("config_path", configPath, configPath);
|
|
||||||
pnh.param("regenerate_local_grids", localGridsRegenerated_, localGridsRegenerated_);
|
|
||||||
|
|
||||||
//parameters
|
|
||||||
rtabmap::ParametersMap parameters;
|
|
||||||
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
|
|
||||||
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
|
|
||||||
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
|
|
||||||
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
|
|
||||||
uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
|
|
||||||
if(!configPath.empty())
|
|
||||||
{
|
|
||||||
if(UFile::exists(configPath.c_str()))
|
|
||||||
{
|
|
||||||
ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
|
|
||||||
rtabmap::ParametersMap allParameters;
|
|
||||||
Parameters::readINI(configPath.c_str(), allParameters);
|
|
||||||
// only update odometry parameters
|
|
||||||
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
|
||||||
{
|
|
||||||
ParametersMap::iterator jter = allParameters.find(iter->first);
|
|
||||||
if(jter!=allParameters.end())
|
|
||||||
{
|
|
||||||
iter->second = jter->second;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
|
||||||
{
|
|
||||||
std::string vStr;
|
|
||||||
bool vBool;
|
|
||||||
int vInt;
|
|
||||||
double vDouble;
|
|
||||||
if(pnh.getParam(iter->first, vStr))
|
|
||||||
{
|
|
||||||
ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
|
|
||||||
iter->second = vStr;
|
|
||||||
}
|
|
||||||
else if(pnh.getParam(iter->first, vBool))
|
|
||||||
{
|
|
||||||
ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
|
|
||||||
iter->second = uBool2Str(vBool);
|
|
||||||
}
|
|
||||||
else if(pnh.getParam(iter->first, vDouble))
|
|
||||||
{
|
|
||||||
ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
|
|
||||||
iter->second = uNumber2Str(vDouble);
|
|
||||||
}
|
|
||||||
else if(pnh.getParam(iter->first, vInt))
|
|
||||||
{
|
|
||||||
ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
|
|
||||||
iter->second = uNumber2Str(vInt);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
|
|
||||||
for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
|
|
||||||
{
|
|
||||||
rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
|
|
||||||
if(jter!=parameters.end())
|
|
||||||
{
|
|
||||||
ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
|
|
||||||
jter->second = iter->second;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Backward compatibility
|
|
||||||
for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
|
|
||||||
iter!=Parameters::getRemovedParameters().end();
|
|
||||||
++iter)
|
|
||||||
{
|
|
||||||
std::string vStr;
|
|
||||||
if(pnh.getParam(iter->first, vStr))
|
|
||||||
{
|
|
||||||
if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
|
|
||||||
{
|
|
||||||
// can be migrated
|
|
||||||
parameters.at(iter->second.second)= vStr;
|
|
||||||
ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
|
|
||||||
ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if(iter->second.second.empty())
|
|
||||||
{
|
|
||||||
ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
|
|
||||||
ros::this_node::getName().c_str(), iter->first.c_str());
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
|
|
||||||
ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// set private parameters
|
|
||||||
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
|
||||||
{
|
|
||||||
pnh.setParam(iter->first, iter->second);
|
|
||||||
}
|
|
||||||
|
|
||||||
ROS_INFO("%s: regenerate_local_grids = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false");
|
|
||||||
mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
|
|
||||||
mapsManager_.backwardCompatibilityParameters(pnh, parameters);
|
|
||||||
mapsManager_.setParameters(parameters);
|
|
||||||
|
|
||||||
std::list<std::string> splitName = uSplit(nh.resolveName("mapData"), '/');
|
|
||||||
std::string rtabmapNs;
|
|
||||||
for(std::list<std::string>::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter)
|
|
||||||
{
|
|
||||||
if(!rtabmapNs.empty())
|
|
||||||
{
|
|
||||||
rtabmapNs += "/";
|
|
||||||
}
|
|
||||||
rtabmapNs += *iter;
|
|
||||||
}
|
|
||||||
ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str());
|
|
||||||
if(rtabmapNs.empty())
|
|
||||||
{
|
|
||||||
rtabmapNs = "get_map_data";
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
rtabmapNs += "/get_map_data";
|
|
||||||
}
|
|
||||||
|
|
||||||
rtabmap_msgs::GetMap getMapSrv;
|
|
||||||
getMapSrv.request.global = false;
|
|
||||||
getMapSrv.request.optimized = true;
|
|
||||||
getMapSrv.request.graphOnly = false;
|
|
||||||
if(ros::service::waitForService(rtabmapNs, 5000))
|
|
||||||
{
|
|
||||||
if(!ros::service::call(rtabmapNs, getMapSrv))
|
|
||||||
{
|
|
||||||
ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str());
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str());
|
|
||||||
processMapData(getMapSrv.response.data);
|
|
||||||
ROS_INFO("Called \"%s\" service, initializing cache... done! The map"
|
|
||||||
" will be assembled on next subscriber connection.", rtabmapNs.c_str());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, "
|
|
||||||
"may not be a problem if rtabmap is started afterwards. If rtabmap "
|
|
||||||
"is started after in localization mode, call /rtabmap/publish_maps "
|
|
||||||
"service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str());
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
|
|
||||||
|
|
||||||
// private services
|
|
||||||
resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
|
|
||||||
|
|
||||||
#ifdef WITH_OCTOMAP_MSGS
|
|
||||||
#ifdef RTABMAP_OCTOMAP
|
|
||||||
octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this);
|
|
||||||
octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this);
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
~MapAssembler()
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
void mapDataReceivedCallback(const rtabmap_msgs::MapDataConstPtr & msg)
|
|
||||||
{
|
|
||||||
processMapData(*msg);
|
|
||||||
}
|
|
||||||
void processMapData(const rtabmap_msgs::MapData & msg)
|
|
||||||
{
|
|
||||||
UTimer timer;
|
|
||||||
|
|
||||||
std::map<int, Transform> poses;
|
|
||||||
std::multimap<int, Link> constraints;
|
|
||||||
Transform mapOdom;
|
|
||||||
rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
|
|
||||||
for(unsigned int i=0; i<msg.nodes.size(); ++i)
|
|
||||||
{
|
|
||||||
if(msg.nodes[i].data.left_compressed.size() ||
|
|
||||||
msg.nodes[i].data.right_compressed.size() ||
|
|
||||||
msg.nodes[i].data.laser_scan_compressed.size())
|
|
||||||
{
|
|
||||||
Signature data = rtabmap_conversions::nodeFromROS(msg.nodes[i]);
|
|
||||||
if(localGridsRegenerated_)
|
|
||||||
{
|
|
||||||
data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
|
|
||||||
}
|
|
||||||
uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// create a tmp signature with latest sensory data
|
|
||||||
if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
|
|
||||||
{
|
|
||||||
Signature tmpS = nodes_.at(poses.rbegin()->first);
|
|
||||||
SensorData tmpData = tmpS.sensorData();
|
|
||||||
tmpData.setId(0);
|
|
||||||
uInsert(nodes_, std::make_pair(0, Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
|
|
||||||
poses.insert(std::make_pair(0, poses.rbegin()->second));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Update maps
|
|
||||||
if(!nodes_.empty())
|
|
||||||
{
|
|
||||||
poses = mapsManager_.updateMapCaches(
|
|
||||||
poses,
|
|
||||||
0,
|
|
||||||
false,
|
|
||||||
false,
|
|
||||||
nodes_);
|
|
||||||
}
|
|
||||||
double updateTime = timer.ticks();
|
|
||||||
|
|
||||||
mapFrameId_ = msg.header.frame_id;
|
|
||||||
optimizedPoses_ = poses;
|
|
||||||
|
|
||||||
mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
|
|
||||||
|
|
||||||
ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
|
|
||||||
}
|
|
||||||
|
|
||||||
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
|
||||||
{
|
|
||||||
ROS_INFO("map_assembler: reset!");
|
|
||||||
mapsManager_.clear();
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef WITH_OCTOMAP_MSGS
|
|
||||||
#ifdef RTABMAP_OCTOMAP
|
|
||||||
bool octomapBinaryCallback(
|
|
||||||
octomap_msgs::GetOctomap::Request &req,
|
|
||||||
octomap_msgs::GetOctomap::Response &res)
|
|
||||||
{
|
|
||||||
ROS_INFO("Sending binary map data on service request");
|
|
||||||
res.map.header.frame_id = mapFrameId_;
|
|
||||||
res.map.header.stamp = ros::Time::now();
|
|
||||||
|
|
||||||
mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
|
|
||||||
|
|
||||||
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
|
|
||||||
bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
|
|
||||||
return success;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool octomapFullCallback(
|
|
||||||
octomap_msgs::GetOctomap::Request &req,
|
|
||||||
octomap_msgs::GetOctomap::Response &res)
|
|
||||||
{
|
|
||||||
ROS_INFO("Sending full map data on service request");
|
|
||||||
res.map.header.frame_id = mapFrameId_;
|
|
||||||
res.map.header.stamp = ros::Time::now();
|
|
||||||
|
|
||||||
mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
|
|
||||||
|
|
||||||
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
|
|
||||||
bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
|
|
||||||
return success;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
|
|
||||||
private:
|
|
||||||
rtabmap_util::MapsManager mapsManager_;
|
|
||||||
std::map<int, Signature> nodes_;
|
|
||||||
std::map<int, Transform> optimizedPoses_;
|
|
||||||
std::string mapFrameId_;
|
|
||||||
|
|
||||||
ros::Subscriber mapDataTopic_;
|
|
||||||
|
|
||||||
ros::ServiceServer resetService_;
|
|
||||||
#ifdef WITH_OCTOMAP_MSGS
|
|
||||||
#ifdef RTABMAP_OCTOMAP
|
|
||||||
ros::ServiceServer octomapBinarySrv_;
|
|
||||||
ros::ServiceServer octomapFullSrv_;
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
bool localGridsRegenerated_;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
|
||||||
{
|
|
||||||
ULogger::setLevel(ULogger::kError);
|
|
||||||
ULogger::setType(ULogger::kTypeConsole);
|
ULogger::setType(ULogger::kTypeConsole);
|
||||||
|
ULogger::setLevel(ULogger::kError);
|
||||||
ros::init(argc, argv, "map_assembler");
|
|
||||||
|
|
||||||
// process "--params" argument
|
// process "--params" argument
|
||||||
|
std::vector<std::string> arguments;
|
||||||
for(int i=1;i<argc;++i)
|
for(int i=1;i<argc;++i)
|
||||||
{
|
{
|
||||||
if(strcmp(argv[i], "--params") == 0)
|
if(strcmp(argv[i], "--params") == 0)
|
||||||
@@ -389,7 +57,7 @@ int main(int argc, char** argv)
|
|||||||
"]" <<
|
"]" <<
|
||||||
std::endl;
|
std::endl;
|
||||||
}
|
}
|
||||||
ROS_WARN("Node will now exit after showing default parameters because "
|
UWARN("Node will now exit after showing default parameters because "
|
||||||
"argument \"--params\" is detected!");
|
"argument \"--params\" is detected!");
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
@@ -401,9 +69,20 @@ int main(int argc, char** argv)
|
|||||||
{
|
{
|
||||||
ULogger::setLevel(ULogger::kInfo);
|
ULogger::setLevel(ULogger::kInfo);
|
||||||
}
|
}
|
||||||
|
else if(strcmp(argv[i], "--uwarn") == 0)
|
||||||
|
{
|
||||||
|
ULogger::setLevel(ULogger::kWarning);
|
||||||
|
}
|
||||||
|
arguments.push_back(argv[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
MapAssembler assembler(argc, argv);
|
rclcpp::NodeOptions options;
|
||||||
ros::spin();
|
options.arguments(arguments);
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
auto node = std::make_shared<rtabmap_util::MapAssembler>(options);
|
||||||
|
rclcpp::executors::MultiThreadedExecutor executor;
|
||||||
|
executor.add_node(node);
|
||||||
|
executor.spin();
|
||||||
|
rclcpp::shutdown();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
@@ -0,0 +1,355 @@
|
|||||||
|
/*
|
||||||
|
Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of the Universite de Sherbrooke nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <rtabmap_util/map_assembler.hpp>
|
||||||
|
|
||||||
|
#include <rtabmap_conversions/MsgConversion.h>
|
||||||
|
|
||||||
|
#include <rtabmap/utilite/UFile.h>
|
||||||
|
#include <rtabmap/utilite/UStl.h>
|
||||||
|
#include <rtabmap/utilite/UTimer.h>
|
||||||
|
|
||||||
|
#ifdef WITH_OCTOMAP_MSGS
|
||||||
|
#ifdef RTABMAP_OCTOMAP
|
||||||
|
#include <octomap_msgs/conversions.h>
|
||||||
|
#include <rtabmap/core/OctoMap.h>
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
using namespace std::chrono_literals;
|
||||||
|
|
||||||
|
namespace rtabmap_util
|
||||||
|
{
|
||||||
|
MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
|
||||||
|
Node("map_assembler", options),
|
||||||
|
rtabmapNodeName_("rtabmap"),
|
||||||
|
localGridsRegenerated_(false)
|
||||||
|
{
|
||||||
|
std::string configPath;
|
||||||
|
configPath = this->declare_parameter("config_path", configPath);
|
||||||
|
localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_);
|
||||||
|
rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_);
|
||||||
|
|
||||||
|
//parameters
|
||||||
|
rtabmap::ParametersMap parameters;
|
||||||
|
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
|
||||||
|
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
|
||||||
|
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
|
||||||
|
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
|
||||||
|
uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
|
||||||
|
if(!configPath.empty())
|
||||||
|
{
|
||||||
|
if(UFile::exists(configPath.c_str()))
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "MapAssembler: Loading parameters from %s", configPath.c_str());
|
||||||
|
rtabmap::ParametersMap allParameters;
|
||||||
|
rtabmap::Parameters::readINI(configPath.c_str(), allParameters);
|
||||||
|
// only update odometry parameters
|
||||||
|
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
||||||
|
{
|
||||||
|
rtabmap::ParametersMap::iterator jter = allParameters.find(iter->first);
|
||||||
|
if(jter!=allParameters.end())
|
||||||
|
{
|
||||||
|
iter->second = jter->second;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "Config file \"%s\" not found!", configPath.c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
||||||
|
{
|
||||||
|
rclcpp::Parameter parameter;
|
||||||
|
std::string vStr = this->declare_parameter(iter->first, iter->second);
|
||||||
|
if(vStr.compare(iter->second)!=0)
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "MapAssembler: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
|
||||||
|
iter->second = vStr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<std::string> tmpList = this->get_node_options().arguments();
|
||||||
|
std::vector<std::string> argList;
|
||||||
|
for(unsigned int i=0; i<tmpList.size(); ++i)
|
||||||
|
{
|
||||||
|
// Issue with ros2 launch files in which we cannot pass a
|
||||||
|
// list of strings as argument (they will appear in same string)
|
||||||
|
std::list<std::string> v = uSplit(tmpList[i]);
|
||||||
|
for(std::list<std::string>::iterator iter=v.begin(); iter!=v.end(); ++iter)
|
||||||
|
{
|
||||||
|
argList.push_back(*iter);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
char ** argv = new char*[argList.size()];
|
||||||
|
for(unsigned int i=0; i<argList.size(); ++i)
|
||||||
|
{
|
||||||
|
argv[i] = &argList[i].at(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
|
||||||
|
delete [] argv;
|
||||||
|
for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
|
||||||
|
{
|
||||||
|
rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
|
||||||
|
if(jter!=parameters.end())
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "MapAssembler: Update parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
|
||||||
|
jter->second = iter->second;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "MapAssembler: Ignored parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Backward compatibility
|
||||||
|
for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin();
|
||||||
|
iter!=rtabmap::Parameters::getRemovedParameters().end();
|
||||||
|
++iter)
|
||||||
|
{
|
||||||
|
rclcpp::Parameter parameter;
|
||||||
|
if(get_parameter(iter->first, parameter))
|
||||||
|
{
|
||||||
|
std::string vStr = parameter.as_string();
|
||||||
|
if(!iter->second.second.empty() && parameters.find(iter->second.second)!=parameters.end())
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".",
|
||||||
|
iter->first.c_str(), iter->second.second.c_str(), parameters.find(iter->second.second)->second.c_str(), vStr.c_str());
|
||||||
|
}
|
||||||
|
else if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
|
||||||
|
{
|
||||||
|
// can be migrated
|
||||||
|
parameters.at(iter->second.second)= vStr;
|
||||||
|
RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
|
||||||
|
iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if(iter->second.second.empty())
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore!",
|
||||||
|
iter->first.c_str());
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
|
||||||
|
iter->first.c_str(), iter->second.second.c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids = %s", this->get_name(), localGridsRegenerated_?"true":"false");
|
||||||
|
mapsManager_.init(*this, this->get_name(), true);
|
||||||
|
mapsManager_.backwardCompatibilityParameters(*this, parameters);
|
||||||
|
mapsManager_.setParameters(parameters);
|
||||||
|
|
||||||
|
const std::string servicePrefix = get_name() + std::string("/");
|
||||||
|
resetService_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset", std::bind(&MapAssembler::reset, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||||
|
|
||||||
|
#ifdef WITH_OCTOMAP_MSGS
|
||||||
|
#ifdef RTABMAP_OCTOMAP
|
||||||
|
octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_binary", std::bind(&MapAssembler::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||||
|
octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_full", std::bind(&MapAssembler::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
|
||||||
|
|
||||||
|
// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
|
||||||
|
serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||||
|
timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||||
|
client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rmw_qos_profile_services_default, serviceCbGroup_); // Put it in a different group than the timer
|
||||||
|
timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
|
||||||
|
}
|
||||||
|
|
||||||
|
MapAssembler::~MapAssembler() {}
|
||||||
|
|
||||||
|
void MapAssembler::timerCallback()
|
||||||
|
{
|
||||||
|
// Just do this callback one time
|
||||||
|
timer_->cancel();
|
||||||
|
if(mapDataSub_.get())
|
||||||
|
{
|
||||||
|
// double call? ignore
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
|
||||||
|
RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str());
|
||||||
|
|
||||||
|
if(client_->wait_for_service(5s))
|
||||||
|
{
|
||||||
|
auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||||
|
request->global_map = false;
|
||||||
|
request->optimized = true;
|
||||||
|
request->graph_only = false;
|
||||||
|
|
||||||
|
auto future = client_->async_send_request(request);
|
||||||
|
std::future_status status = future.wait_for(10s);
|
||||||
|
|
||||||
|
if (status == std::future_status::ready) {
|
||||||
|
RCLCPP_INFO(this->get_logger(), "Initializing cache...");
|
||||||
|
processMapData(future.get()->data);
|
||||||
|
RCLCPP_INFO(this->get_logger(), "Initializing cache... done! The map"
|
||||||
|
" will be assembled on next subscriber connection.");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(this->get_logger(), "Service \"%s\" not responding after waiting for 10 seconds.",
|
||||||
|
getMapSrv.c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, "
|
||||||
|
"may not be a problem if rtabmap is started afterwards. If rtabmap "
|
||||||
|
"is started after in localization mode, call %s/publish_maps "
|
||||||
|
"service with graph_only=false to make sure map_assembler has all the data.",
|
||||||
|
getMapSrv.c_str(),
|
||||||
|
rtabmapNodeName_.c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::SubscriptionOptions options;
|
||||||
|
options.callback_group = timerCbGroup_;
|
||||||
|
mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1),
|
||||||
|
std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg)
|
||||||
|
{
|
||||||
|
processMapData(*msg);
|
||||||
|
}
|
||||||
|
void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg)
|
||||||
|
{
|
||||||
|
UTimer timer;
|
||||||
|
|
||||||
|
std::map<int, rtabmap::Transform> poses;
|
||||||
|
std::multimap<int, rtabmap::Link> constraints;
|
||||||
|
rtabmap::Transform mapOdom;
|
||||||
|
rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
|
||||||
|
for(unsigned int i=0; i<msg.nodes.size(); ++i)
|
||||||
|
{
|
||||||
|
if(msg.nodes[i].data.left_compressed.size() ||
|
||||||
|
msg.nodes[i].data.right_compressed.size() ||
|
||||||
|
msg.nodes[i].data.laser_scan_compressed.size())
|
||||||
|
{
|
||||||
|
rtabmap::Signature data = rtabmap_conversions::nodeFromROS(msg.nodes[i]);
|
||||||
|
if(localGridsRegenerated_)
|
||||||
|
{
|
||||||
|
data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
|
||||||
|
}
|
||||||
|
uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// create a tmp signature with latest sensory data
|
||||||
|
if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
|
||||||
|
{
|
||||||
|
rtabmap::Signature tmpS = nodes_.at(poses.rbegin()->first);
|
||||||
|
rtabmap::SensorData tmpData = tmpS.sensorData();
|
||||||
|
tmpData.setId(0);
|
||||||
|
uInsert(nodes_, std::make_pair(0, rtabmap::Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), rtabmap::Transform(), tmpData)));
|
||||||
|
poses.insert(std::make_pair(0, poses.rbegin()->second));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update maps
|
||||||
|
if(!nodes_.empty())
|
||||||
|
{
|
||||||
|
poses = mapsManager_.updateMapCaches(
|
||||||
|
poses,
|
||||||
|
0,
|
||||||
|
false,
|
||||||
|
false,
|
||||||
|
nodes_);
|
||||||
|
}
|
||||||
|
double updateTime = timer.ticks();
|
||||||
|
|
||||||
|
mapFrameId_ = msg.header.frame_id;
|
||||||
|
optimizedPoses_ = poses;
|
||||||
|
|
||||||
|
mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
|
||||||
|
|
||||||
|
RCLCPP_INFO(this->get_logger(), "map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapAssembler::reset(const std::shared_ptr<rmw_request_id_t>,
|
||||||
|
const std::shared_ptr<std_srvs::srv::Empty::Request>,
|
||||||
|
std::shared_ptr<std_srvs::srv::Empty::Response>)
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "map_assembler: reset!");
|
||||||
|
mapsManager_.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef WITH_OCTOMAP_MSGS
|
||||||
|
#ifdef RTABMAP_OCTOMAP
|
||||||
|
void MapAssembler::octomapBinaryCallback(
|
||||||
|
const std::shared_ptr<rmw_request_id_t>,
|
||||||
|
const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
|
||||||
|
std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res)
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "Sending binary map data on service request");
|
||||||
|
res->map.header.frame_id = mapFrameId_;
|
||||||
|
res->map.header.stamp = now();
|
||||||
|
|
||||||
|
mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
|
||||||
|
|
||||||
|
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
|
||||||
|
if(octomap->octree()->size())
|
||||||
|
octomap_msgs::binaryMapToMsg(*octomap->octree(), res->map);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapAssembler::octomapFullCallback(
|
||||||
|
const std::shared_ptr<rmw_request_id_t>,
|
||||||
|
const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
|
||||||
|
std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res)
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "Sending full map data on service request");
|
||||||
|
res->map.header.frame_id = mapFrameId_;
|
||||||
|
res->map.header.stamp = now();
|
||||||
|
|
||||||
|
mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
|
||||||
|
|
||||||
|
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
|
||||||
|
if(octomap->octree()->size())
|
||||||
|
octomap_msgs::fullMapToMsg(*octomap->octree(), res->map);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#include "rclcpp_components/register_node_macro.hpp"
|
||||||
|
|
||||||
|
// Register the component with class_loader.
|
||||||
|
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||||
|
// is being loaded into a running process.
|
||||||
|
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::MapAssembler)
|
||||||
@@ -129,7 +129,7 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
|
|||||||
initCachePath = UDirectory::currentDir(true) + initCachePath;
|
initCachePath = UDirectory::currentDir(true) + initCachePath;
|
||||||
}
|
}
|
||||||
RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Initializing cache with local database \"%s\"", initCachePath.c_str());
|
RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Initializing cache with local database \"%s\"", initCachePath.c_str());
|
||||||
if(!callMapDataService("get_map_data", false, true, true))
|
if(!callMapDataService(rtabmapNodeName_+"/get_map_data", false, true, true))
|
||||||
{
|
{
|
||||||
RCLCPP_ERROR(this->get_logger(),
|
RCLCPP_ERROR(this->get_logger(),
|
||||||
"The cache will still be loaded "
|
"The cache will still be loaded "
|
||||||
|
|||||||
Reference in New Issue
Block a user