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.

cartographer_ros/configuration_files/backpack_3d.lua

@@ -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

cartographer_ros/launch/backpack_3d.launch

@@ -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" />
 

cartographer_ros/ogre_media/materials/glsl120/glsl120.program

@@ -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
-}

cartographer_ros/ogre_media/materials/glsl120/screen_blit.frag

@@ -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);
-}

cartographer_ros/ogre_media/materials/glsl120/screen_blit.vert

@@ -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;
-}

cartographer_ros/ogre_media/materials/glsl120/submap.frag

@@ -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 value = u_alpha * texture_value.r;
-  float alpha = texture_value.g;
+  float alpha = u_alpha * texture_value.g;
-  float is_known = step(1e-3, alpha + value);
+  gl_FragColor = vec4(value, value, value, alpha);
-  gl_FragColor = vec4(u_alpha * value, u_alpha * is_known, 0., u_alpha * alpha);
 }

cartographer_ros/ogre_media/materials/scripts/screen_blit.material

@@ -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
-      }
-    }
-  }
-}

cartographer_ros/src/drawable_submap.cc

@@ -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(

cartographer_ros/src/drawable_submap.h

@@ -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_;

cartographer_ros/src/submaps_display.cc

@@ -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()) {
+  ::cartographer::common::MutexLocker locker(&mutex_);
-    return;
+  for (auto& trajectory : trajectories_) {
-  }
+    for (int i = 0; i < trajectory->Size(); ++i) {
-  const int width = scene_manager_->getCurrentViewport()->getActualWidth();
+      trajectory->Get(i).Transform(ros::Time(0) /* latest */);
-  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());
       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) {
+      } catch (const tf2::TransformException& ex) {
-        ROS_WARN("Could not compute submap fading: %s", e.what());
+        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

cartographer_ros/src/submaps_display.h

@@ -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
+// We show an X-ray view of the map which is achieved by shipping textures for
-// every submap as a SceneNode. We show an X-ray view of the map which is
+// every submap containing pre-multiplied alpha and grayscale values, these are
-// achieved by shipping textures for every submap containing pre-multiplied
+// then alpha blended together.
-// 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.
 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_;
 };
 

cartographer_ros/urdf/backpack_2d.urdf

@@ -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">
+  <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>
 
   <joint name="imu_link_joint" type="fixed">
     <parent link="base_link"/>

cartographer_ros/urdf/backpack_3d.urdf

@@ -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">
+  <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>
 
-  <joint name="base_link_joint" type="fixed">
+  <joint name="imu_link_joint" type="fixed">
-    <parent link="/base_link"/>
+    <parent link="base_link"/>
-    <child link="/imu_link"/>
+    <child link="imu_link"/>
-    <origin xyz="0 0 0" rpy="0 0 3.1416"/>
+    <origin xyz="0 0 0" rpy="3.1416 0 0"/>
   </joint>
 
   <joint name="horizontal_vlp16_link_joint" type="fixed">
-    <parent link="/base_link"/>
+    <parent link="base_link"/>
-    <child link="/horizontal_vlp16_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"/>
+    <parent link="base_link"/>
-    <child link="/vertical_vlp16_link"/>
+    <child link="vertical_vlp16_link"/>
     <origin xyz="0.19 0. 0.04" rpy="0. 1.3963 0."/>
   </joint>
 </robot>