feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,978 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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_rviz_plugins/MapCloudDisplay.h"
|
||||
|
||||
#include <QApplication>
|
||||
#include <QMessageBox>
|
||||
#include <QTimer>
|
||||
|
||||
#include <OgreSceneNode.h>
|
||||
#include <OgreSceneManager.h>
|
||||
|
||||
#include "rclcpp/clock.hpp"
|
||||
|
||||
#include "rviz_common/display.hpp"
|
||||
#include "rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.hpp"
|
||||
#include "rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp"
|
||||
#include <rviz_common/validate_floats.hpp>
|
||||
#include <rviz_common/properties/int_property.hpp>
|
||||
#include "rviz_common/properties/bool_property.hpp"
|
||||
#include "rviz_common/properties/enum_property.hpp"
|
||||
#include "rviz_common/properties/float_property.hpp"
|
||||
#include "rviz_common/properties/vector_property.hpp"
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/Graph.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap_msgs/srv/get_map.hpp>
|
||||
|
||||
namespace rtabmap_rviz_plugins
|
||||
{
|
||||
|
||||
|
||||
CloudInfo::CloudInfo() :
|
||||
manager_(nullptr),
|
||||
pose_(rtabmap::Transform::getIdentity()),
|
||||
id_(0),
|
||||
scene_node_(nullptr)
|
||||
{}
|
||||
|
||||
CloudInfo::~CloudInfo()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
void CloudInfo::clear()
|
||||
{
|
||||
if ( scene_node_ )
|
||||
{
|
||||
manager_->destroySceneNode( scene_node_ );
|
||||
scene_node_=nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
const std::string MapCloudDisplay::message_status_name_ = "Message"; // NOLINT allow std::string
|
||||
|
||||
MapCloudDisplay::MapCloudDisplay()
|
||||
: auto_size_(false),
|
||||
current_map_updated_(false),
|
||||
lastCloudAdded_(-1),
|
||||
new_xyz_transformer_(false),
|
||||
new_color_transformer_(false),
|
||||
needs_retransform_(false),
|
||||
transformer_factory_(std::make_unique<rviz_default_plugins::PointCloudTransformerFactory>())
|
||||
{
|
||||
//QIcon icon;
|
||||
//this->setIcon(icon);
|
||||
|
||||
auto options = rclcpp::NodeOptions().arguments(
|
||||
{"--ros-args", "--remap", "__node:=rviz_map_cloud_action_client", "--"});
|
||||
clientNode_ = std::make_shared<rclcpp::Node>("_", options);
|
||||
|
||||
style_property_ = new rviz_common::properties::EnumProperty( "Style", "Flat Squares",
|
||||
"Rendering mode to use, in order of computational complexity.",
|
||||
this, SLOT( updateStyle() ), this );
|
||||
style_property_->addOption( "Points", rviz_rendering::PointCloud::RM_POINTS );
|
||||
style_property_->addOption( "Squares", rviz_rendering::PointCloud::RM_SQUARES );
|
||||
style_property_->addOption( "Flat Squares", rviz_rendering::PointCloud::RM_FLAT_SQUARES );
|
||||
style_property_->addOption( "Spheres", rviz_rendering::PointCloud::RM_SPHERES );
|
||||
style_property_->addOption( "Boxes", rviz_rendering::PointCloud::RM_BOXES );
|
||||
|
||||
point_world_size_property_ = new rviz_common::properties::FloatProperty( "Size (m)", 0.01,
|
||||
"Point size in meters.",
|
||||
this, SLOT( updateBillboardSize() ), this );
|
||||
point_world_size_property_->setMin( 0.0001 );
|
||||
|
||||
point_pixel_size_property_ = new rviz_common::properties::FloatProperty( "Size (Pixels)", 3,
|
||||
"Point size in pixels.",
|
||||
this, SLOT( updateBillboardSize() ), this );
|
||||
point_pixel_size_property_->setMin( 1 );
|
||||
|
||||
alpha_property_ = new rviz_common::properties::FloatProperty( "Alpha", 1.0,
|
||||
"Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.",
|
||||
this, SLOT( updateAlpha() ), this );
|
||||
alpha_property_->setMin( 0 );
|
||||
alpha_property_->setMax( 1 );
|
||||
|
||||
xyz_transformer_property_ = new rviz_common::properties::EnumProperty( "Position Transformer", "",
|
||||
"Set the transformer to use to set the position of the points.",
|
||||
this, SLOT( updateXyzTransformer() ), this );
|
||||
connect( xyz_transformer_property_, SIGNAL( requestOptions( rviz_common::properties::EnumProperty* )),
|
||||
this, SLOT( setXyzTransformerOptions( rviz_common::properties::EnumProperty* )));
|
||||
|
||||
color_transformer_property_ = new rviz_common::properties::EnumProperty( "Color Transformer", "",
|
||||
"Set the transformer to use to set the color of the points.",
|
||||
this, SLOT( updateColorTransformer() ), this );
|
||||
connect( color_transformer_property_, SIGNAL( requestOptions( rviz_common::properties::EnumProperty* )),
|
||||
this, SLOT( setColorTransformerOptions( rviz_common::properties::EnumProperty* )));
|
||||
|
||||
cloud_from_scan_ = new rviz_common::properties::BoolProperty( "Cloud from scan", false,
|
||||
"Create the cloud from laser scans instead of the RGB-D/Stereo images.",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
fromScan_ = cloud_from_scan_->getBool();
|
||||
|
||||
cloud_decimation_ = new rviz_common::properties::IntProperty( "Cloud decimation", 4,
|
||||
"Decimation of the input RGB and depth images before creating the cloud.",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
cloud_decimation_->setMin( 1 );
|
||||
cloud_decimation_->setMax( 16 );
|
||||
|
||||
cloud_max_depth_ = new rviz_common::properties::FloatProperty( "Cloud max depth (m)", 4.0f,
|
||||
"Maximum depth of the generated clouds.",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
cloud_max_depth_->setMin( 0.0f );
|
||||
cloud_max_depth_->setMax( 999.0f );
|
||||
|
||||
cloud_min_depth_ = new rviz_common::properties::FloatProperty( "Cloud min depth (m)", 0.0f,
|
||||
"Minimum depth of the generated clouds.",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
cloud_min_depth_->setMin( 0.0f );
|
||||
cloud_min_depth_->setMax( 999.0f );
|
||||
|
||||
cloud_voxel_size_ = new rviz_common::properties::FloatProperty( "Cloud voxel size (m)", 0.01f,
|
||||
"Voxel size of the generated clouds.",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
cloud_voxel_size_->setMin( 0.0f );
|
||||
cloud_voxel_size_->setMax( 1.0f );
|
||||
|
||||
cloud_filter_floor_height_ = new rviz_common::properties::FloatProperty( "Filter floor (m)", 0.0f,
|
||||
"Filter the floor up to maximum height set here "
|
||||
"(only appropriate for 2D mapping).",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
cloud_filter_floor_height_->setMin( -999.0f );
|
||||
cloud_filter_floor_height_->setMax( 999.0f );
|
||||
|
||||
cloud_filter_ceiling_height_ = new rviz_common::properties::FloatProperty( "Filter ceiling (m)", 0.0f,
|
||||
"Filter the ceiling at the specified height set here "
|
||||
"(only appropriate for 2D mapping).",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
cloud_filter_ceiling_height_->setMin( -999.0f );
|
||||
cloud_filter_ceiling_height_->setMax( 999.0f );
|
||||
|
||||
node_filtering_radius_ = new rviz_common::properties::FloatProperty( "Node filtering radius (m)", 0.0f,
|
||||
"(Disabled=0) Only keep one node in the specified radius.",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
node_filtering_radius_->setMin( 0.0f );
|
||||
node_filtering_radius_->setMax( 10.0f );
|
||||
|
||||
node_filtering_angle_ = new rviz_common::properties::FloatProperty( "Node filtering angle (degrees)", 30.0f,
|
||||
"(Disabled=0) Only keep one node in the specified angle in the filtering radius.",
|
||||
this, SLOT( updateCloudParameters() ), this );
|
||||
node_filtering_angle_->setMin( 0.0f );
|
||||
node_filtering_angle_->setMax( 359.0f );
|
||||
|
||||
download_namespace = new rviz_common::properties::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this, SLOT( downloadNamespaceChanged() ), this);
|
||||
|
||||
download_map_ = new rviz_common::properties::BoolProperty( "Download map", false,
|
||||
"Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.",
|
||||
this, SLOT( downloadMap() ), this );
|
||||
|
||||
download_graph_ = new rviz_common::properties::BoolProperty( "Download graph", false,
|
||||
"Download the optimized global graph (without cloud data) using rtabmap/GetMap service.",
|
||||
this, SLOT( downloadGraph() ), this );
|
||||
}
|
||||
|
||||
|
||||
void MapCloudDisplay::onInitialize()
|
||||
{
|
||||
MFDClass::onInitialize();
|
||||
|
||||
loadTransformers();
|
||||
|
||||
updateStyle();
|
||||
updateBillboardSize();
|
||||
updateAlpha();
|
||||
|
||||
downloadNamespaceChanged();
|
||||
}
|
||||
|
||||
void MapCloudDisplay::loadTransformers()
|
||||
{
|
||||
auto plugins = transformer_factory_->getDeclaredPlugins();
|
||||
for (auto const & plugin : plugins) {
|
||||
auto plugin_name_std = plugin.name.toStdString();
|
||||
if (transformers_.count(plugin_name_std) > 0) {
|
||||
RVIZ_COMMON_LOG_ERROR_STREAM("Transformer type " << plugin_name_std << " is already loaded.");
|
||||
continue;
|
||||
}
|
||||
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> trans(transformer_factory_->make(plugin.id));
|
||||
loadTransformer(trans, plugin_name_std, plugin.id.toStdString());
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::loadTransformer(
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> trans,
|
||||
std::string name,
|
||||
const std::string & lookup_name)
|
||||
{
|
||||
trans->init();
|
||||
connect(trans.get(), SIGNAL(needRetransform()), this, SLOT(causeRetransform()));
|
||||
|
||||
TransformerInfo info;
|
||||
info.transformer = trans;
|
||||
info.readable_name = name;
|
||||
info.lookup_name = lookup_name;
|
||||
|
||||
info.transformer->createProperties(
|
||||
this, rviz_default_plugins::PointCloudTransformer::Support_XYZ, info.xyz_props);
|
||||
setPropertiesHidden(info.xyz_props, true);
|
||||
|
||||
info.transformer->createProperties(
|
||||
this, rviz_default_plugins::PointCloudTransformer::Support_Color, info.color_props);
|
||||
setPropertiesHidden(info.color_props, true);
|
||||
|
||||
transformers_[name] = info;
|
||||
}
|
||||
|
||||
void MapCloudDisplay::processMessage( const rtabmap_msgs::msg::MapData::ConstSharedPtr msg )
|
||||
{
|
||||
processMapData(*msg);
|
||||
|
||||
this->emitTimeSignal(msg->header.stamp);
|
||||
}
|
||||
|
||||
void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
|
||||
{
|
||||
std::map<int, rtabmap::Transform> poses;
|
||||
for(unsigned int i=0; i<map.graph.poses_id.size() && i<map.graph.poses.size(); ++i)
|
||||
{
|
||||
poses.insert(std::make_pair(map.graph.poses_id[i], rtabmap_conversions::transformFromPoseMsg(map.graph.poses[i])));
|
||||
}
|
||||
|
||||
// Add new clouds...
|
||||
bool fromDepth = !cloud_from_scan_->getBool();
|
||||
std::set<int> nodeDataReceived;
|
||||
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
|
||||
{
|
||||
int id = map.nodes[i].id;
|
||||
|
||||
// Always refresh the cloud if there are data
|
||||
rtabmap::Signature s = rtabmap_conversions::nodeFromROS(map.nodes[i]);
|
||||
if((fromDepth &&
|
||||
!s.sensorData().imageCompressed().empty() &&
|
||||
!s.sensorData().depthOrRightCompressed().empty() &&
|
||||
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModels().size())) ||
|
||||
(!fromDepth && !s.sensorData().laserScanCompressed().isEmpty()))
|
||||
{
|
||||
cv::Mat image, depth;
|
||||
rtabmap::LaserScan scan;
|
||||
|
||||
s.sensorData().uncompressData(fromDepth?&image:0, fromDepth?&depth:0, !fromDepth?&scan:0);
|
||||
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr cloudMsg(new sensor_msgs::msg::PointCloud2);
|
||||
if(fromDepth && !s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
|
||||
pcl::IndicesPtr validIndices(new std::vector<int>);
|
||||
|
||||
cloud = rtabmap::util3d::cloudRGBFromSensorData(
|
||||
s.sensorData(),
|
||||
cloud_decimation_->getInt(),
|
||||
cloud_max_depth_->getFloat(),
|
||||
cloud_min_depth_->getFloat(),
|
||||
validIndices.get());
|
||||
|
||||
if(!cloud->empty())
|
||||
{
|
||||
if(cloud_voxel_size_->getFloat())
|
||||
{
|
||||
cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
|
||||
}
|
||||
|
||||
if(cloud_filter_floor_height_->getFloat() != 0.0f || cloud_filter_ceiling_height_->getFloat() != 0.0f)
|
||||
{
|
||||
// convert in /odom frame
|
||||
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose());
|
||||
cloud = rtabmap::util3d::passThrough(cloud, "z",
|
||||
cloud_filter_floor_height_->getFloat()!=0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
|
||||
cloud_filter_ceiling_height_->getFloat()!=0.0f && (cloud_filter_floor_height_->getFloat()==0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
|
||||
// convert back in /base_link frame
|
||||
if(!cloud->empty())
|
||||
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
|
||||
}
|
||||
|
||||
if(!cloud->empty())
|
||||
{
|
||||
pcl::toROSMsg(*cloud, *cloudMsg);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(!fromDepth && !scan.isEmpty())
|
||||
{
|
||||
scan = rtabmap::util3d::commonFiltering(
|
||||
scan,
|
||||
1,
|
||||
cloud_min_depth_->getFloat(),
|
||||
cloud_max_depth_->getFloat(),
|
||||
cloud_voxel_size_->getFloat());
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
|
||||
cloud = rtabmap::util3d::laserScanToPointCloudI(scan, scan.localTransform());
|
||||
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
|
||||
{
|
||||
// convert in /odom frame
|
||||
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose());
|
||||
cloud = rtabmap::util3d::passThrough(cloud, "z",
|
||||
cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
|
||||
cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
|
||||
// convert back in /base_link frame
|
||||
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
|
||||
}
|
||||
|
||||
if(!cloud->empty())
|
||||
{
|
||||
pcl::toROSMsg(*cloud, *cloudMsg);
|
||||
}
|
||||
}
|
||||
|
||||
if(!cloudMsg->data.empty())
|
||||
{
|
||||
cloudMsg->header = map.header;
|
||||
CloudInfoPtr info(new CloudInfo);
|
||||
info->message_ = cloudMsg;
|
||||
info->pose_ = rtabmap::Transform::getIdentity();
|
||||
info->id_ = id;
|
||||
|
||||
if (transformCloud(info, true))
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(new_clouds_mutex_);
|
||||
new_cloud_infos_.erase(id);
|
||||
new_cloud_infos_.insert(std::make_pair(id, info));
|
||||
}
|
||||
}
|
||||
}
|
||||
nodeDataReceived.insert(id);
|
||||
}
|
||||
|
||||
// Update graph
|
||||
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
|
||||
{
|
||||
poses = rtabmap::graph::radiusPosesFiltering(poses,
|
||||
node_filtering_radius_->getFloat(),
|
||||
node_filtering_angle_->getFloat()*CV_PI/180.0);
|
||||
}
|
||||
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(current_map_mutex_);
|
||||
current_map_ = poses;
|
||||
current_map_updated_ = true;
|
||||
nodeDataReceived_.insert(nodeDataReceived.begin(), nodeDataReceived.end());
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::setPropertiesHidden( const QList<rviz_common::properties::Property*>& props, bool hide )
|
||||
{
|
||||
for (auto prop : props) {
|
||||
prop->setHidden(hide);
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::updateTransformers( const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud )
|
||||
{
|
||||
std::string xyz_name = xyz_transformer_property_->getStdString();
|
||||
std::string color_name = color_transformer_property_->getStdString();
|
||||
|
||||
xyz_transformer_property_->clearOptions();
|
||||
color_transformer_property_->clearOptions();
|
||||
|
||||
// Get the channels that we could potentially render
|
||||
typedef std::set<std::pair<uint8_t, std::string>> S_string;
|
||||
S_string valid_xyz, valid_color;
|
||||
bool cur_xyz_valid = false;
|
||||
bool cur_color_valid = false;
|
||||
bool has_rgb_transformer = false;
|
||||
for (auto transformer : transformers_) {
|
||||
const std::string & name = transformer.first;
|
||||
const std::shared_ptr<rviz_default_plugins::PointCloudTransformer> & trans = transformer.second.transformer;
|
||||
uint32_t mask = trans->supports(cloud);
|
||||
if (mask & rviz_default_plugins::PointCloudTransformer::Support_XYZ) {
|
||||
valid_xyz.insert(std::make_pair(trans->score(cloud), name));
|
||||
if (name == xyz_name) {
|
||||
cur_xyz_valid = true;
|
||||
}
|
||||
xyz_transformer_property_->addOptionStd(name);
|
||||
}
|
||||
|
||||
if (mask & rviz_default_plugins::PointCloudTransformer::Support_Color) {
|
||||
valid_color.insert(std::make_pair(trans->score(cloud), name));
|
||||
if (name == color_name) {
|
||||
cur_color_valid = true;
|
||||
}
|
||||
if (name == "RGB8") {
|
||||
has_rgb_transformer = true;
|
||||
}
|
||||
color_transformer_property_->addOptionStd(name);
|
||||
}
|
||||
}
|
||||
|
||||
if (!cur_xyz_valid) {
|
||||
if (!valid_xyz.empty()) {
|
||||
xyz_transformer_property_->setStringStd(valid_xyz.rbegin()->second);
|
||||
}
|
||||
}
|
||||
|
||||
if (!cur_color_valid) {
|
||||
if (!valid_color.empty()) {
|
||||
if (has_rgb_transformer) {
|
||||
color_transformer_property_->setStringStd("RGB8");
|
||||
} else {
|
||||
color_transformer_property_->setStringStd(valid_color.rbegin()->second);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::updateAlpha()
|
||||
{
|
||||
for (auto const & cloud_info : cloud_infos_) {
|
||||
bool per_point_alpha = rviz_default_plugins::findChannelIndex(cloud_info.second->message_, "rgba") != -1;
|
||||
cloud_info.second->cloud_->setAlpha(alpha_property_->getFloat(), per_point_alpha);
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::updateStyle()
|
||||
{
|
||||
auto mode = static_cast<rviz_rendering::PointCloud::RenderMode>(style_property_->getOptionInt());
|
||||
if (mode == rviz_rendering::PointCloud::RM_POINTS) {
|
||||
point_world_size_property_->hide();
|
||||
point_pixel_size_property_->show();
|
||||
} else {
|
||||
point_world_size_property_->show();
|
||||
point_pixel_size_property_->hide();
|
||||
}
|
||||
for (auto const & cloud_info : cloud_infos_) {
|
||||
cloud_info.second->cloud_->setRenderMode(mode);
|
||||
}
|
||||
updateBillboardSize();
|
||||
}
|
||||
|
||||
void MapCloudDisplay::updateBillboardSize()
|
||||
{
|
||||
auto mode = static_cast<rviz_rendering::PointCloud::RenderMode>(style_property_->getOptionInt());
|
||||
float size;
|
||||
if (mode == rviz_rendering::PointCloud::RM_POINTS) {
|
||||
size = point_pixel_size_property_->getFloat();
|
||||
} else {
|
||||
size = point_world_size_property_->getFloat();
|
||||
}
|
||||
for (auto & cloud_info : cloud_infos_) {
|
||||
cloud_info.second->cloud_->setDimensions(size, size, size);
|
||||
}
|
||||
context_->queueRender();
|
||||
}
|
||||
|
||||
void MapCloudDisplay::updateCloudParameters()
|
||||
{
|
||||
// do nothing for most parameters... only take effect on next generated clouds
|
||||
|
||||
// if we change the kind of map, clear
|
||||
if(fromScan_ != cloud_from_scan_->getBool())
|
||||
{
|
||||
reset();
|
||||
}
|
||||
fromScan_ = cloud_from_scan_->getBool();
|
||||
}
|
||||
|
||||
void MapCloudDisplay::downloadMap(bool graphOnly)
|
||||
{
|
||||
auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
request->global_map = false;
|
||||
request->optimized = true;
|
||||
request->graph_only = graphOnly;
|
||||
std::string rtabmapNs = download_namespace->getStdString();
|
||||
std::string srvName = rtabmapNs+"/get_map_data";
|
||||
QMessageBox * messageBox = new QMessageBox(
|
||||
QMessageBox::NoIcon,
|
||||
tr("Calling \"%1\" service...").arg(srvName.c_str()),
|
||||
tr("Downloading the map... please wait (rviz could become gray!)"),
|
||||
QMessageBox::NoButton,
|
||||
getAssociatedWidget());
|
||||
messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
|
||||
messageBox->show();
|
||||
QApplication::processEvents();
|
||||
uSleep(100); // hack make sure the text in the QMessageBox is shown...
|
||||
QApplication::processEvents();
|
||||
|
||||
RVIZ_COMMON_LOG_INFO(uFormat("Wait for service %s", srvName.c_str()));
|
||||
|
||||
auto client = clientNode_->create_client<rtabmap_msgs::srv::GetMap>(srvName);
|
||||
if(client->wait_for_service(std::chrono::seconds(1)))
|
||||
{
|
||||
RVIZ_COMMON_LOG_INFO(uFormat("Calling service %s", srvName.c_str()));
|
||||
auto result = client->async_send_request(request);
|
||||
if (rclcpp::spin_until_future_complete(clientNode_, result) ==
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RVIZ_COMMON_LOG_INFO(uFormat("Process data"));
|
||||
auto future = result.get();
|
||||
if(graphOnly)
|
||||
{
|
||||
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(future->data.graph.poses.size()));
|
||||
QApplication::processEvents();
|
||||
processMapData(future->data);
|
||||
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(future->data.graph.poses.size()));
|
||||
QApplication::processEvents();
|
||||
QTimer::singleShot(1000, messageBox, SLOT(close()));
|
||||
}
|
||||
else
|
||||
{
|
||||
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
|
||||
.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
|
||||
QApplication::processEvents();
|
||||
this->reset();
|
||||
processMapData(future->data);
|
||||
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
|
||||
.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
|
||||
|
||||
QTimer::singleShot(1000, messageBox, SLOT(close()));
|
||||
}
|
||||
} else {
|
||||
std::string msg = uFormat("Failed to call service %s", srvName.c_str());
|
||||
RVIZ_COMMON_LOG_ERROR(msg);
|
||||
messageBox->setText(msg.c_str());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
std::string msg = uFormat("MapCloudDisplay: Cannot call \"%s\" service. "
|
||||
"Tip: if rtabmap node is not in \"%s\" namespace, you can "
|
||||
"change the \"Download namespace\" option.",
|
||||
srvName.c_str(),
|
||||
rtabmapNs.c_str());
|
||||
RVIZ_COMMON_LOG_ERROR(msg);
|
||||
messageBox->setText(msg.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::downloadNamespaceChanged()
|
||||
{
|
||||
std::string rtabmapNs = download_namespace->getStdString();
|
||||
std::string topicName = uFormat("%s/republish_node_data", rtabmapNs.c_str());
|
||||
republishNodeDataPub_ = rviz_ros_node_.lock()->get_raw_node()->create_publisher<std_msgs::msg::Int32MultiArray>(topicName, 1);
|
||||
}
|
||||
|
||||
void MapCloudDisplay::downloadMap()
|
||||
{
|
||||
if(download_map_->getBool())
|
||||
{
|
||||
downloadMap(false);
|
||||
download_map_->blockSignals(true);
|
||||
download_map_->setBool(false);
|
||||
download_map_->blockSignals(false);
|
||||
}
|
||||
else
|
||||
{
|
||||
// just stay true if double-clicked on DownloadMap property, let the
|
||||
// first process above finishes
|
||||
download_map_->blockSignals(true);
|
||||
download_map_->setBool(true);
|
||||
download_map_->blockSignals(false);
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::downloadGraph()
|
||||
{
|
||||
if(download_graph_->getBool())
|
||||
{
|
||||
downloadMap(true);
|
||||
download_graph_->blockSignals(true);
|
||||
download_graph_->setBool(false);
|
||||
download_graph_->blockSignals(false);
|
||||
}
|
||||
else
|
||||
{
|
||||
// just stay true if double-clicked on DownloadGraph property, let the
|
||||
// first process above finishes
|
||||
download_graph_->blockSignals(true);
|
||||
download_graph_->setBool(true);
|
||||
download_graph_->blockSignals(false);
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::causeRetransform()
|
||||
{
|
||||
needs_retransform_ = true;
|
||||
}
|
||||
|
||||
void MapCloudDisplay::update( float, float )
|
||||
{
|
||||
auto mode = static_cast<rviz_rendering::PointCloud::RenderMode>(style_property_->getOptionInt());
|
||||
|
||||
int lastCloudAdded = -1;
|
||||
|
||||
if (needs_retransform_)
|
||||
{
|
||||
retransform();
|
||||
needs_retransform_ = false;
|
||||
}
|
||||
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(new_clouds_mutex_);
|
||||
if( !new_cloud_infos_.empty() )
|
||||
{
|
||||
float size;
|
||||
if (mode == rviz_rendering::PointCloud::RM_POINTS) {
|
||||
size = point_pixel_size_property_->getFloat();
|
||||
} else {
|
||||
size = point_world_size_property_->getFloat();
|
||||
}
|
||||
|
||||
auto it = new_cloud_infos_.begin();
|
||||
auto end = new_cloud_infos_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
CloudInfoPtr cloud_info = it->second;
|
||||
|
||||
bool per_point_alpha = rviz_default_plugins::findChannelIndex(cloud_info->message_, "rgba") != -1;
|
||||
|
||||
cloud_info->cloud_.reset( new rviz_rendering::PointCloud() );
|
||||
cloud_info->cloud_->addPoints(cloud_info->transformed_points_.begin(), cloud_info->transformed_points_.end());
|
||||
cloud_info->cloud_->setRenderMode( mode );
|
||||
cloud_info->cloud_->setAlpha( alpha_property_->getFloat(), per_point_alpha);
|
||||
cloud_info->cloud_->setDimensions( size, size, size );
|
||||
cloud_info->cloud_->setAutoSize(auto_size_);
|
||||
|
||||
cloud_info->manager_ = context_->getSceneManager();
|
||||
|
||||
cloud_info->scene_node_ = scene_node_->createChildSceneNode();
|
||||
|
||||
cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
|
||||
cloud_info->scene_node_->setVisible(false);
|
||||
|
||||
cloud_infos_.erase(it->first);
|
||||
cloud_infos_.insert(*it);
|
||||
lastCloudAdded = it->first;
|
||||
}
|
||||
|
||||
new_cloud_infos_.clear();
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
std::unique_lock<std::recursive_mutex> lock( transformers_mutex_ );
|
||||
|
||||
if( new_xyz_transformer_ || new_color_transformer_ )
|
||||
{
|
||||
for (auto transformer : transformers_) {
|
||||
const std::string & name = transformer.first;
|
||||
TransformerInfo & info = transformer.second;
|
||||
|
||||
setPropertiesHidden(info.xyz_props, name != xyz_transformer_property_->getStdString());
|
||||
setPropertiesHidden(info.color_props,
|
||||
name != color_transformer_property_->getStdString());
|
||||
|
||||
if (name == xyz_transformer_property_->getStdString() ||
|
||||
name == color_transformer_property_->getStdString())
|
||||
{
|
||||
info.transformer->hideUnusedProperties();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
new_xyz_transformer_ = false;
|
||||
new_color_transformer_ = false;
|
||||
}
|
||||
|
||||
int totalPoints = 0;
|
||||
int totalNodesShown = 0;
|
||||
{
|
||||
// update poses
|
||||
std::unique_lock<std::mutex> lock(current_map_mutex_);
|
||||
if(!current_map_.empty())
|
||||
{
|
||||
std::vector<int> missingNodes;
|
||||
for (std::map<int, rtabmap::Transform>::iterator it=current_map_.begin(); it != current_map_.end(); ++it)
|
||||
{
|
||||
std::map<int, CloudInfoPtr>::iterator cloudInfoIt = cloud_infos_.find(it->first);
|
||||
if(cloudInfoIt != cloud_infos_.end())
|
||||
{
|
||||
totalPoints += cloudInfoIt->second->transformed_points_.size();
|
||||
cloudInfoIt->second->pose_ = it->second;
|
||||
Ogre::Vector3 framePosition;
|
||||
Ogre::Quaternion frameOrientation;
|
||||
std::string error;
|
||||
if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, framePosition, frameOrientation))
|
||||
{
|
||||
// Multiply frame with pose
|
||||
Ogre::Matrix4 frameTransform;
|
||||
frameTransform.makeTransform( framePosition, Ogre::Vector3(1,1,1), frameOrientation);
|
||||
const rtabmap::Transform & p = cloudInfoIt->second->pose_;
|
||||
Ogre::Matrix4 pose(p[0], p[1], p[2], p[3],
|
||||
p[4], p[5], p[6], p[7],
|
||||
p[8], p[9], p[10], p[11],
|
||||
0, 0, 0, 1);
|
||||
frameTransform = frameTransform * pose;
|
||||
Ogre::Vector3 posePosition = frameTransform.getTrans();
|
||||
Ogre::Quaternion poseOrientation(frameTransform.linear());
|
||||
poseOrientation.normalise();
|
||||
|
||||
cloudInfoIt->second->scene_node_->setPosition(posePosition);
|
||||
cloudInfoIt->second->scene_node_->setOrientation(poseOrientation);
|
||||
cloudInfoIt->second->scene_node_->setVisible(true);
|
||||
++totalNodesShown;
|
||||
}
|
||||
else if(context_->getFrameManager()->transformHasProblems(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, error))
|
||||
{
|
||||
RVIZ_COMMON_LOG_ERROR(uFormat("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\" (reason=%s), set fixed frame in global options to \"%s\")",
|
||||
it->first,
|
||||
cloudInfoIt->second->message_->header.frame_id.c_str(),
|
||||
error.c_str(),
|
||||
cloudInfoIt->second->message_->header.frame_id.c_str()));
|
||||
}
|
||||
}
|
||||
else if(it->first>0 && current_map_updated_&& nodeDataReceived_.find(it->first) == nodeDataReceived_.end())
|
||||
{
|
||||
missingNodes.push_back(it->first);
|
||||
}
|
||||
}
|
||||
//hide not used clouds
|
||||
for(std::map<int, CloudInfoPtr>::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end();)
|
||||
{
|
||||
if(current_map_.find(iter->first) == current_map_.end())
|
||||
{
|
||||
if(iter->first == lastCloudAdded_)
|
||||
{
|
||||
// remove from cache, the node has been discarded
|
||||
cloud_infos_.erase(iter++);
|
||||
lastCloudAdded_ = -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
iter->second->scene_node_->setVisible(false);
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
|
||||
if(!missingNodes.empty() && republishNodeDataPub_.get())
|
||||
{
|
||||
std_msgs::msg::Int32MultiArray::UniquePtr msg(new std_msgs::msg::Int32MultiArray);
|
||||
msg->data = missingNodes;
|
||||
republishNodeDataPub_->publish(std::move(msg));
|
||||
}
|
||||
}
|
||||
current_map_updated_ = false;
|
||||
}
|
||||
if(lastCloudAdded>0)
|
||||
{
|
||||
lastCloudAdded_ = lastCloudAdded;
|
||||
}
|
||||
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString());
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString());
|
||||
}
|
||||
|
||||
void MapCloudDisplay::reset()
|
||||
{
|
||||
lastCloudAdded_ = -1;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(new_clouds_mutex_);
|
||||
cloud_infos_.clear();
|
||||
new_cloud_infos_.clear();
|
||||
}
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(current_map_mutex_);
|
||||
current_map_.clear();
|
||||
current_map_updated_ = false;
|
||||
nodeDataReceived_.clear();
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::updateXyzTransformer()
|
||||
{
|
||||
std::unique_lock<std::recursive_mutex> lock( transformers_mutex_ );
|
||||
if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
|
||||
{
|
||||
return;
|
||||
}
|
||||
new_xyz_transformer_ = true;
|
||||
causeRetransform();
|
||||
}
|
||||
|
||||
void MapCloudDisplay::updateColorTransformer()
|
||||
{
|
||||
std::unique_lock<std::recursive_mutex> lock( transformers_mutex_ );
|
||||
if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
|
||||
{
|
||||
return;
|
||||
}
|
||||
new_color_transformer_ = true;
|
||||
causeRetransform();
|
||||
}
|
||||
|
||||
void MapCloudDisplay::setXyzTransformerOptions( rviz_common::properties::EnumProperty* prop )
|
||||
{
|
||||
fillTransformerOptions( prop, rviz_default_plugins::PointCloudTransformer::Support_XYZ );
|
||||
}
|
||||
|
||||
void MapCloudDisplay::setColorTransformerOptions( rviz_common::properties::EnumProperty* prop )
|
||||
{
|
||||
fillTransformerOptions( prop, rviz_default_plugins::PointCloudTransformer::Support_Color );
|
||||
}
|
||||
|
||||
void MapCloudDisplay::fillTransformerOptions(
|
||||
rviz_common::properties::EnumProperty * prop,
|
||||
uint32_t mask)
|
||||
{
|
||||
prop->clearOptions();
|
||||
|
||||
if (cloud_infos_.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
|
||||
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg = cloud_infos_.begin()->second->message_;
|
||||
|
||||
for (auto transformer : transformers_) {
|
||||
const std::shared_ptr<rviz_default_plugins::PointCloudTransformer> & trans = transformer.second.transformer;
|
||||
if ((trans->supports(msg) & mask) == mask) {
|
||||
prop->addOption(QString::fromStdString(transformer.first));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> MapCloudDisplay::getXYZTransformer(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & cloud)
|
||||
{
|
||||
std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
|
||||
auto it = transformers_.find(xyz_transformer_property_->getStdString());
|
||||
if (it != transformers_.end()) {
|
||||
const std::shared_ptr<rviz_default_plugins::PointCloudTransformer> & trans = it->second.transformer;
|
||||
if (trans->supports(cloud) & rviz_default_plugins::PointCloudTransformer::Support_XYZ) {
|
||||
return trans;
|
||||
}
|
||||
}
|
||||
|
||||
return std::shared_ptr<rviz_default_plugins::PointCloudTransformer>();
|
||||
}
|
||||
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> MapCloudDisplay::getColorTransformer(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & cloud)
|
||||
{
|
||||
std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
|
||||
auto it = transformers_.find(color_transformer_property_->getStdString());
|
||||
if (it != transformers_.end()) {
|
||||
const std::shared_ptr<rviz_default_plugins::PointCloudTransformer> & trans = it->second.transformer;
|
||||
if (trans->supports(cloud) & rviz_default_plugins::PointCloudTransformer::Support_Color) {
|
||||
return trans;
|
||||
}
|
||||
}
|
||||
|
||||
return std::shared_ptr<rviz_default_plugins::PointCloudTransformer>();
|
||||
}
|
||||
|
||||
|
||||
void MapCloudDisplay::retransform()
|
||||
{
|
||||
std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
|
||||
|
||||
for (auto const & cloud_info : cloud_infos_) {
|
||||
transformCloud(cloud_info.second, false);
|
||||
cloud_info.second->cloud_->clear();
|
||||
cloud_info.second->cloud_->addPoints(
|
||||
cloud_info.second->transformed_points_.begin(), cloud_info.second->transformed_points_.end());
|
||||
}
|
||||
}
|
||||
|
||||
bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
|
||||
{
|
||||
this->deleteStatusStd(message_status_name_);
|
||||
|
||||
rviz_default_plugins::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
|
||||
cloud_points.clear();
|
||||
|
||||
size_t size = cloud_info->message_->width * cloud_info->message_->height;
|
||||
rviz_rendering::PointCloud::Point default_pt;
|
||||
default_pt.color = Ogre::ColourValue(1, 1, 1);
|
||||
default_pt.position = Ogre::Vector3::ZERO;
|
||||
cloud_points.resize(size, default_pt);
|
||||
|
||||
{
|
||||
std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
|
||||
if( update_transformers )
|
||||
{
|
||||
updateTransformers( cloud_info->message_ );
|
||||
}
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> xyz_trans = getXYZTransformer(cloud_info->message_);
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> color_trans = getColorTransformer(cloud_info->message_);
|
||||
|
||||
if (cloud_info->message_->data.size() !=
|
||||
cloud_info->message_->width * cloud_info->message_->height * cloud_info->message_->point_step)
|
||||
{
|
||||
std::string status = "PointCloud contained not enough or too much data";
|
||||
this->setStatusStd(
|
||||
rviz_common::properties::StatusProperty::Error, message_status_name_, status);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!xyz_trans)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "No position transformer available for cloud";
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Error, message_status_name_, ss.str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!color_trans)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "No color transformer available for cloud";
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Error, message_status_name_, ss.str());
|
||||
return false;
|
||||
}
|
||||
|
||||
xyz_trans->transform(cloud_info->message_, rviz_default_plugins::PointCloudTransformer::Support_XYZ, Ogre::Matrix4::IDENTITY, cloud_points);
|
||||
color_trans->transform(cloud_info->message_, rviz_default_plugins::PointCloudTransformer::Support_Color, Ogre::Matrix4::IDENTITY, cloud_points);
|
||||
}
|
||||
|
||||
for (auto & cloud_point : cloud_points) {
|
||||
if (!rviz_common::validateFloats(cloud_point.position)) {
|
||||
cloud_point.position.x = 999999.0f;
|
||||
cloud_point.position.y = 999999.0f;
|
||||
cloud_point.position.z = 999999.0f;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace rtabmap_rviz_plugins
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_rviz_plugins::MapCloudDisplay, rviz_common::Display)
|
||||
Reference in New Issue
Block a user