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

Added Image display in rgbd.rviz

Browse Source 
rviz plugin:  fixed assertion on Ogre::AxisAlignedBox: Assertion "(min.x <= max.x && min.y <= max.y && min.z <= max.z) && The minimum corner of the box must be less than or equal to maximum corner"
rviz plugin: added node_filtering_radius and node_filtering_angle properties for cloud filtering
rtabmap-ros: added set_mode_localization and set_mode_mapping services

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1489 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-09 21:12:24 +00:00
parent 7efd5028f2
commit b492c4678a
5 changed files with 97 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/MapCloudDisplay.cpp
+36 -15
View File
@@ -35,6 +35,7 @@ namespace rtabmap
MapCloudDisplay::CloudInfo::CloudInfo() :
manager_(0),
pose_(Transform::getIdentity()),
scene_node_(0)
{}
@@ -108,12 +109,24 @@ MapCloudDisplay::MapCloudDisplay()
cloud_max_depth_->setMin( 0.0f );
cloud_max_depth_->setMax( 999.0f );
cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.02f,
cloud_voxel_size_ = new rviz::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 );
node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f,
"(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::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_map_ = new rviz::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 );
@@ -279,6 +292,13 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
poses.insert(std::make_pair(map.poseIDs[i], transformFromPoseMsg(map.poses[i])));
}
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{
poses = util3d::radiusPosesFiltering(poses,
node_filtering_radius_->getFloat(),
node_filtering_angle_->getFloat()*CV_PI/180.0);
}
{
boost::mutex::scoped_lock lock(current_map_mutex_);
current_map_ = poses;
@@ -512,7 +532,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
cloud_info->manager_ = context_->getSceneManager();
cloud_info->scene_node_ = scene_node_->createChildSceneNode( cloud_info->position_, cloud_info->orientation_ );
cloud_info->scene_node_ = scene_node_->createChildSceneNode();
cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
@@ -548,6 +568,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
}
int totalPoints = 0;
int totalNodesShown = 0;
{
// update poses
boost::mutex::scoped_lock lock(current_map_mutex_);
@@ -560,23 +581,27 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
{
totalPoints += cloudInfoIt->second->transformed_points_.size();
cloudInfoIt->second->pose_ = it->second;
if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, cloudInfoIt->second->position_, cloudInfoIt->second->orientation_))
Ogre::Vector3 framePosition;
Ogre::Quaternion frameOrientation;
if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, framePosition, frameOrientation))
{
// Set pose
// Multiply frame with pose
Ogre::Matrix4 frameTransform;
frameTransform.makeTransform( cloudInfoIt->second->position_, Ogre::Vector3(1,1,1), cloudInfoIt->second->orientation_ );
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;
cloudInfoIt->second->position_ = frameTransform.getTrans();
cloudInfoIt->second->orientation_ = frameTransform.extractQuaternion();
Ogre::Vector3 posePosition = frameTransform.getTrans();
Ogre::Quaternion poseOrientation = frameTransform.extractQuaternion();
poseOrientation.normalise();
cloudInfoIt->second->scene_node_->setPosition(cloudInfoIt->second->position_);
cloudInfoIt->second->scene_node_->setOrientation(cloudInfoIt->second->orientation_);
cloudInfoIt->second->scene_node_->setPosition(posePosition);
cloudInfoIt->second->scene_node_->setOrientation(poseOrientation);
cloudInfoIt->second->scene_node_->setVisible(true);
++totalNodesShown;
}
else
{
@@ -596,12 +621,8 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
}
}
std::stringstream ss;
ss << totalPoints;
this->setStatusStd(rviz::StatusProperty::Ok, "Points", ss.str());
std::stringstream ss2;
ss2 << cloud_infos_.size();
this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", ss2.str());
this->setStatusStd(rviz::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString());
}
void MapCloudDisplay::reset()
src/rviz/MapCloudDisplay.h
+2 -3
View File
@@ -63,9 +63,6 @@ public:
boost::shared_ptr<rviz::PointCloud> cloud_;
std::vector<rviz::PointCloud::Point> transformed_points_;
Ogre::Quaternion orientation_;
Ogre::Vector3 position_;
};
typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
@@ -84,6 +81,8 @@ public:
rviz::IntProperty* cloud_decimation_;
rviz::FloatProperty* cloud_max_depth_;
rviz::FloatProperty* cloud_voxel_size_;
rviz::FloatProperty* node_filtering_radius_;
rviz::FloatProperty* node_filtering_angle_;
rviz::BoolProperty* download_map_;
public Q_SLOTS: