12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix draw ordering issue of the RViz visualization. (#5) 

Instead of drawing into a offscreen texture and blitting the
result as an overlay (which shows up on top of everything else)
we directly draw to the RViz scene. To get proper X-rays one
should use a 0.5 gray background for now.

Also fixes the configuration to enable 3D mapping from a bag.
master
Wolfgang Hess 2016-08-04 14:38:55 +02:00 committed by GitHub
parent b89d9a51a5
commit 5497e9c343
13 changed files with 44 additions and 335 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -29,6 +29,6 @@ options.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_
options.sparse_pose_graph.constraint_builder.min_score = 0.62
options.sparse_pose_graph.constraint_builder.log_matches = true
options.trajectory_builder.scans_per_accumulation = 2
options.trajectory_builder.scans_per_accumulation = 20
return options

59
cartographer_ros/launch/backpack_3d.launch
View File

@ -15,65 +15,6 @@
-->
<launch>
<node pkg="nodelet" type="nodelet" name="velodyne_nodelet_manager"
args="manager" />
<node pkg="nodelet" type="nodelet" name="horizontal_driver_nodelet"
args="load velodyne_driver/DriverNodelet velodyne_nodelet_manager" >
<remap from="velodyne_packets" to="horizontal_velodyne_packets" />
<param name="model" value="VLP16"/>
<param name="pcap" value=""/>
<param name="read_once" value="false"/>
<param name="read_fast" value="false"/>
<param name="repeat_delay" value="0.0"/>
<param name="rpm" value="600.0"/>
<param name="port" value="2368" />
<param name="frame_id" value="horizontal_vlp16_link"/>
</node>
<node pkg="nodelet" type="nodelet" name="vertical_driver_nodelet"
args="load velodyne_driver/DriverNodelet velodyne_nodelet_manager" >
<remap from="velodyne_packets" to="vertical_velodyne_packets" />
<param name="model" value="VLP16"/>
<param name="pcap" value=""/>
<param name="read_once" value="false"/>
<param name="read_fast" value="false"/>
<param name="repeat_delay" value="0.0"/>
<param name="rpm" value="600.0"/>
<param name="port" value="2369" />
<param name="frame_id" value="vertical_vlp16_link"/>
</node>
<remap from="/imu/imu" to="/imu" />
<!-- TODO(hrapp): the IMU has a bug that makes it impossible to use arg for
setting the parameter. so we work around this by setting the param
directly -->
<param name="/imu/frameId" value="imu_link"/>
<include file="$(find imu_3dm_gx4)/launch/imu.launch">
<arg name="frame_id" value="imu_link"/>
</include>
<!-- VLP16 packets -> points -->
<arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
<arg name="min_range" default="0.4" />
<arg name="max_range" default="130.0" />
<node pkg="nodelet" type="nodelet" name="cloud_nodelet_horizontal"
args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
<remap from="velodyne_packets" to="horizontal_velodyne_packets" />
<remap from="velodyne_points" to="horizontal_3d_laser" />
<param name="calibration" value="$(arg calibration)"/>
<param name="min_range" value="$(arg min_range)"/>
<param name="max_range" value="$(arg max_range)"/>
</node>
<node pkg="nodelet" type="nodelet" name="cloud_nodelet_vertical"
args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
<remap from="velodyne_packets" to="vertical_velodyne_packets" />
<remap from="velodyne_points" to="vertical_3d_laser" />
<param name="calibration" value="$(arg calibration)"/>
<param name="min_range" value="$(arg min_range)"/>
<param name="max_range" value="$(arg max_range)"/>
</node>
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_3d.urdf" />

10
cartographer_ros/ogre_media/materials/glsl120/glsl120.program
View File

@ -23,13 +23,3 @@ vertex_program cartographer_ros/glsl120/submap.vert glsl
{
source submap.vert
}
fragment_program cartographer_ros/glsl120/screen_blit.frag glsl
{
source screen_blit.frag
}
vertex_program cartographer_ros/glsl120/screen_blit.vert glsl
{
source screen_blit.vert
}

29
cartographer_ros/ogre_media/materials/glsl120/screen_blit.frag
View File

@ -1,29 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#version 120
varying vec2 out_texture_coordinate;
uniform sampler2D u_texture;
void main()
{
vec2 texture_color = texture2D(u_texture, out_texture_coordinate).rg;
float opacity = texture_color.g;
float value = texture_color.r;
gl_FragColor = vec4(value, value, value, opacity);
}

28
cartographer_ros/ogre_media/materials/glsl120/screen_blit.vert
View File

@ -1,28 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#version 120
attribute vec4 vertex;
attribute vec4 uv0;
varying vec2 out_texture_coordinate;
void main()
{
out_texture_coordinate = vec2(uv0);
gl_Position = vertex;
}

7
cartographer_ros/ogre_media/materials/glsl120/submap.frag
View File

@ -24,8 +24,7 @@ uniform float u_alpha;
void main()
{
vec2 texture_value = texture2D(u_submap, out_submap_texture_coordinate).rg;
float value = texture_value.r;
float alpha = texture_value.g;
float is_known = step(1e-3, alpha + value);
gl_FragColor = vec4(u_alpha * value, u_alpha * is_known, 0., u_alpha * alpha);
float value = u_alpha * texture_value.r;
float alpha = u_alpha * texture_value.g;
gl_FragColor = vec4(value, value, value, alpha);
}

31
cartographer_ros/ogre_media/materials/scripts/screen_blit.material
View File

@ -1,31 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
material cartographer_ros/ScreenBlit
{
technique
{
pass
{
vertex_program_ref cartographer_ros/glsl120/screen_blit.vert {}
fragment_program_ref cartographer_ros/glsl120/screen_blit.frag
{
param_named u_texture int 0
}
}
}
}

3
cartographer_ros/src/drawable_submap.cc
View File

@ -60,6 +60,7 @@ DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
::rviz::FrameManager* const frame_manager,
Ogre::SceneManager* const scene_manager)
: frame_manager_(frame_manager),
scene_manager_(scene_manager),
scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()),
manual_object_(scene_manager->createManualObject(
kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))),
@ -86,6 +87,8 @@ DrawableSubmap::~DrawableSubmap() {
Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
texture_.setNull();
}
scene_manager_->destroySceneNode(scene_node_);
scene_manager_->destroyManualObject(manual_object_);
}
bool DrawableSubmap::Update(

1
cartographer_ros/src/drawable_submap.h
View File

@ -89,6 +89,7 @@ class DrawableSubmap : public QObject {
::cartographer::common::Mutex mutex_;
::rviz::FrameManager* frame_manager_;
Ogre::SceneManager* const scene_manager_;
Ogre::SceneNode* const scene_node_;
Ogre::ManualObject* manual_object_;
Ogre::TexturePtr texture_;

128
cartographer_ros/src/submaps_display.cc
View File

@ -16,23 +16,8 @@
#include "submaps_display.h"
#include <OgreColourValue.h>
#include <OgreHardwarePixelBuffer.h>
#include <OgreManualObject.h>
#include <OgreMaterialManager.h>
#include <OgreOverlay.h>
#include <OgreOverlayContainer.h>
#include <OgreOverlayManager.h>
#include <OgreRenderTexture.h>
#include <OgreResourceGroupManager.h>
#include <OgreRoot.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <OgreSharedPtr.h>
#include <OgreTechnique.h>
#include <OgreTexture.h>
#include <OgreTextureManager.h>
#include <OgreViewport.h>
#include <cartographer/common/mutex.h>
#include <cartographer_ros_msgs/SubmapList.h>
#include <cartographer_ros_msgs/SubmapQuery.h>
@ -56,13 +41,6 @@ constexpr int kMaxOnGoingRequests = 6;
constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
constexpr char kGlsl120Directory[] = "/glsl120";
constexpr char kScriptsDirectory[] = "/scripts";
constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial";
constexpr char kScreenBlitSourceMaterialName[] = "cartographer_ros/ScreenBlit";
constexpr char kSubmapsRttPrefix[] = "SubmapsRtt";
constexpr char kMapTextureName[] = "MapTexture";
constexpr char kMapOverlayName[] = "MapOverlay";
constexpr char kSubmapsSceneCameraName[] = "SubmapsSceneCamera";
constexpr char kSubmapTexturesGroup[] = "SubmapTexturesGroup";
constexpr char kDefaultMapFrame[] = "map";
constexpr char kDefaultTrackingFrame[] = "base_link";
@ -70,7 +48,6 @@ constexpr char kDefaultTrackingFrame[] = "base_link";
SubmapsDisplay::SubmapsDisplay()
: Display(),
rtt_count_(0),
scene_manager_listener_([this]() { UpdateMapTexture(); }),
tf_listener_(tf_buffer_) {
connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps()));
@ -107,32 +84,6 @@ SubmapsDisplay::SubmapsDisplay()
SubmapsDisplay::~SubmapsDisplay() { UnsubscribeAndClear(); }
void SubmapsDisplay::onInitialize() {
submaps_scene_manager_ =
Ogre::Root::getSingletonPtr()->createSceneManager(Ogre::ST_GENERIC);
submaps_scene_camera_ =
submaps_scene_manager_->createCamera(kSubmapsSceneCameraName);
submap_scene_material_ = Ogre::MaterialManager::getSingleton().create(
kMapTextureName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
screen_blit_material_ = Ogre::MaterialManager::getSingleton().getByName(
kScreenBlitSourceMaterialName);
screen_blit_material_ = screen_blit_material_->clone(kScreenBlitMaterialName);
screen_blit_material_->setReceiveShadows(false);
screen_blit_material_->getTechnique(0)->setLightingEnabled(false);
screen_blit_material_->setDepthWriteEnabled(false);
Ogre::OverlayManager& overlay_manager = Ogre::OverlayManager::getSingleton();
overlay_ = overlay_manager.create(kMapOverlayName);
panel_ = static_cast<Ogre::OverlayContainer*>(
overlay_manager.createOverlayElement("Panel", "PanelName"));
overlay_->add2D(panel_);
panel_->setPosition(0.0, 0.0);
panel_->setDimensions(1., 1.);
panel_->setMaterialName(kScreenBlitMaterialName);
Ogre::ResourceGroupManager::getSingleton().createResourceGroup(
kSubmapTexturesGroup);
scene_manager_->addListener(&scene_manager_listener_);
UpdateSubmapTopicOrService();
}
@ -166,9 +117,9 @@ void SubmapsDisplay::Subscribe() {
&SubmapsDisplay::IncomingSubmapList, this,
ros::TransportHints().reliable());
setStatus(::rviz::StatusProperty::Ok, "Topic", "OK");
} catch (ros::Exception& e) {
} catch (const ros::Exception& ex) {
setStatus(::rviz::StatusProperty::Error, "Topic",
QString("Error subscribing: ") + e.what());
QString("Error subscribing: ") + ex.what());
}
}
}
@ -183,15 +134,7 @@ void SubmapsDisplay::UnsubscribeAndClear() {
submap_list_subscriber_.shutdown();
client_.shutdown();
::cartographer::common::MutexLocker locker(&mutex_);
submaps_scene_manager_->clearScene();
if (!rttTexture_.isNull()) {
rttTexture_->unload();
rttTexture_.setNull();
}
Ogre::ResourceGroupManager::getSingleton().clearResourceGroup(
kSubmapTexturesGroup);
trajectories_.clear();
overlay_->hide();
}
void SubmapsDisplay::RequestNewSubmaps() {
@ -210,7 +153,7 @@ void SubmapsDisplay::RequestNewSubmaps() {
submap_id < submap_entries.size(); ++submap_id) {
trajectories_[trajectory_id]->Add(submap_id, trajectory_id,
context_->getFrameManager(),
submaps_scene_manager_);
context_->getSceneManager());
}
}
int num_ongoing_requests = 0;
@ -242,73 +185,22 @@ void SubmapsDisplay::RequestNewSubmaps() {
}
void SubmapsDisplay::UpdateMapTexture() {
if (trajectories_.empty()) {
return;
}
const int width = scene_manager_->getCurrentViewport()->getActualWidth();
const int height = scene_manager_->getCurrentViewport()->getActualHeight();
if (!rttTexture_.isNull()) {
rttTexture_->unload();
rttTexture_.setNull();
}
// If the rtt texture is freed every time UpdateMapTexture() is called, the
// code slows down a lot. Therefore, we assign them to a group and free them
// every 100th texture.
if (rtt_count_ % 100 == 0) {
Ogre::ResourceGroupManager::getSingleton().clearResourceGroup(
kSubmapTexturesGroup);
}
rttTexture_ =
Ogre::Root::getSingletonPtr()->getTextureManager()->createManual(
kSubmapsRttPrefix + std::to_string(rtt_count_), kSubmapTexturesGroup,
Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_RG8,
Ogre::TU_RENDERTARGET);
rtt_count_++;
Ogre::Pass* rtt_pass = submap_scene_material_->getTechnique(0)->getPass(0);
Ogre::TextureUnitState* const rtt_tex_unit =
rtt_pass->getNumTextureUnitStates() > 0
? rtt_pass->getTextureUnitState(0)
: rtt_pass->createTextureUnitState();
rtt_tex_unit->setTexture(rttTexture_);
Ogre::RenderTexture* const renderTexture =
rttTexture_->getBuffer()->getRenderTarget();
renderTexture->addViewport(submaps_scene_camera_)
->setBackgroundColour(Ogre::ColourValue(0.5f, 0.f, 0.f));
{
::cartographer::common::MutexLocker locker(&mutex_);
// TODO(pedrofernandez): Add support for more than one trajectory.
for (int i = 0; i < trajectories_.front()->Size(); ++i) {
trajectories_.front()->Get(i).Transform(ros::Time());
::cartographer::common::MutexLocker locker(&mutex_);
for (auto& trajectory : trajectories_) {
for (int i = 0; i < trajectory->Size(); ++i) {
trajectory->Get(i).Transform(ros::Time(0) /* latest */);
try {
const ::geometry_msgs::TransformStamped transform_stamped =
tf_buffer_.lookupTransform(map_frame_property_->getStdString(),
tracking_frame_property_->getStdString(),
ros::Time(0) /* latest */);
trajectories_.front()->Get(i).SetAlpha(
trajectory->Get(i).SetAlpha(
transform_stamped.transform.translation.z);
} catch (tf2::TransformException& e) {
ROS_WARN("Could not compute submap fading: %s", e.what());
} catch (const tf2::TransformException& ex) {
ROS_WARN("Could not compute submap fading: %s", ex.what());
}
}
}
Ogre::Camera* const actual_camera =
scene_manager_->getCurrentViewport()->getCamera();
submaps_scene_camera_->synchroniseBaseSettingsWith(actual_camera);
submaps_scene_camera_->setCustomProjectionMatrix(
true, actual_camera->getProjectionMatrix());
renderTexture->update();
Ogre::Pass* const pass = screen_blit_material_->getTechnique(0)->getPass(0);
pass->setSceneBlending(Ogre::SBF_SOURCE_ALPHA,
Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
Ogre::TextureUnitState* const tex_unit = pass->getNumTextureUnitStates() > 0
? pass->getTextureUnitState(0)
: pass->createTextureUnitState();
tex_unit->setTextureName(rttTexture_->getName());
tex_unit->setTextureFiltering(Ogre::TFO_NONE);
overlay_->show();
}
} // namespace rviz

19
cartographer_ros/src/submaps_display.h
View File

@ -18,8 +18,6 @@
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_
#include <OgreMaterial.h>
#include <OgreOverlay.h>
#include <OgreOverlayContainer.h>
#include <OgreSceneManager.h>
#include <OgreSharedPtr.h>
#include <OgreTexture.h>
@ -46,12 +44,9 @@ namespace rviz {
// RViz plugin used for displaying maps which are represented by a collection of
// submaps.
//
// We keep a separate Ogre scene to the one provided by rviz and in it we place
// every submap as a SceneNode. We show an X-ray view of the map which is
// achieved by shipping textures for every submap containing pre-multiplied
// alpha and grayscale values, these are then alpha blended together onto an
// offscreen texture. This offscreen texture is then screen blit onto the screen
// as a grayscale image.
// We show an X-ray view of the map which is achieved by shipping textures for
// every submap containing pre-multiplied alpha and grayscale values, these are
// then alpha blended together.
class SubmapsDisplay : public ::rviz::Display {
Q_OBJECT
@ -95,7 +90,6 @@ class SubmapsDisplay : public ::rviz::Display {
void IncomingSubmapList(
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg);
int rtt_count_;
SceneManagerListener scene_manager_listener_;
::cartographer_ros_msgs::SubmapList submap_list_;
ros::Subscriber submap_list_subscriber_;
@ -107,13 +101,6 @@ class SubmapsDisplay : public ::rviz::Display {
::rviz::StringProperty* map_frame_property_;
::rviz::StringProperty* tracking_frame_property_;
std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
Ogre::SceneManager* submaps_scene_manager_;
Ogre::Camera* submaps_scene_camera_;
Ogre::MaterialPtr submap_scene_material_;
Ogre::MaterialPtr screen_blit_material_;
Ogre::Overlay* overlay_;
Ogre::OverlayContainer* panel_;
Ogre::TexturePtr rttTexture_;
::cartographer::common::Mutex mutex_;
};

18
cartographer_ros/urdf/backpack_2d.urdf
View File

@ -27,9 +27,9 @@
<link name="imu_link">
<visual>
<origin xyz="0.01 0.01 -0.01"/>
<origin xyz="0.0 0.0 0.0"/>
<geometry>
<box size="0.055 0.055 0.02"/>
<box size="0.06 0.04 0.02"/>
</geometry>
<material name="orange"/>
</visual>
@ -37,7 +37,7 @@
<link name="horizontal_laser_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<origin xyz="0.0 0.0 0.0"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
</geometry>
@ -47,7 +47,7 @@
<link name="vertical_laser_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<origin xyz="0.0 0.0 0.0"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
</geometry>
@ -55,15 +55,7 @@
</visual>
</link>
<link name="base_link">
<visual>
<origin xyz="0.18 0.0 0.2"/>
<geometry>
<box size="0.03 0.4 0.8"/>
</geometry>
<material name="green"/>
</visual>
</link>
<link name="base_link" />
<joint name="imu_link_joint" type="fixed">
<parent link="base_link"/>

44
cartographer_ros/urdf/backpack_3d.urdf
View File

@ -25,61 +25,53 @@
<color rgba="0.2 0.4 0.2 1"/>
</material>
<link name="/imu_link">
<link name="imu_link">
<visual>
<origin xyz="0.01 0.01 -0.01"/>
<origin xyz="0.0 0.0 0.0"/>
<geometry>
<box size="0.055 0.055 0.02"/>
<box size="0.06 0.04 0.02"/>
</geometry>
<material name="orange"/>
</visual>
</link>
<link name="/horizontal_vlp16_link">
<link name="horizontal_vlp16_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<origin xyz="0.0 0.0 0.0"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
<cylinder length="0.07" radius="0.05"/>
</geometry>
<material name="gray"/>
</visual>
</link>
<link name="/vertical_vlp16_link">
<link name="vertical_vlp16_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<origin xyz="0.0 0.0 0.0"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
<cylinder length="0.07" radius="0.05"/>
</geometry>
<material name="gray"/>
</visual>
</link>
<link name="/base_link">
<visual>
<origin xyz="0.18 0.0 0.2"/>
<geometry>
<box size="0.03 0.4 0.8"/>
</geometry>
<material name="green"/>
</visual>
</link>
<link name="base_link" />
<joint name="base_link_joint" type="fixed">
<parent link="/base_link"/>
<child link="/imu_link"/>
<origin xyz="0 0 0" rpy="0 0 3.1416"/>
<joint name="imu_link_joint" type="fixed">
<parent link="base_link"/>
<child link="imu_link"/>
<origin xyz="0 0 0" rpy="3.1416 0 0"/>
</joint>
<joint name="horizontal_vlp16_link_joint" type="fixed">
<parent link="/base_link"/>
<child link="/horizontal_vlp16_link"/>
<parent link="base_link"/>
<child link="horizontal_vlp16_link"/>
<origin xyz="0.01 0. 0.19" rpy="0. -0.1745 3.1416" />
</joint>
<joint name="vertical_vlp16_link_joint" type="fixed">
<parent link="/base_link"/>
<child link="/vertical_vlp16_link"/>
<parent link="base_link"/>
<child link="vertical_vlp16_link"/>
<origin xyz="0.19 0. 0.04" rpy="0. 1.3963 0."/>
</joint>
</robot>