Add laser_retro support (#603)

Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
gazebosim · Feb 12, 2021 · 3465bbe · 3465bbe
1 parent 1847513
commit 3465bbe
Show file tree

Hide file tree

Showing 7 changed files with 290 additions and 0 deletions.
diff --git a/examples/worlds/gpu_lidar_retro_values_sensor.sdf b/examples/worlds/gpu_lidar_retro_values_sensor.sdf
@@ -0,0 +1,214 @@
+<?xml version="1.0" ?>
+<!--
+ This world has two boxes: a red one with no retro value and a blue one with
+ a retro value of ~ 500
+
+ You can echo lidar messages using:
+ ign topic -e -t /lidar
+ and see the intensities of the points corresponding to the blue box filled
+ with ~ 500 values
+-->
+<sdf version="1.6">
+ <world name="gpu_lidar_retro_values_sensor">
+ <physics name="1ms" type="ignored">
+ <max_step_size>0.001</max_step_size>
+ <real_time_factor>1.0</real_time_factor>
+ </physics>
+ <plugin
+ filename="ignition-gazebo-physics-system"
+ name="ignition::gazebo::systems::Physics">
+ </plugin>
+ <plugin
+ filename="ignition-gazebo-sensors-system"
+ name="ignition::gazebo::systems::Sensors">
+ <render_engine>ogre2</render_engine>
+ </plugin>
+ <plugin
+ filename="ignition-gazebo-scene-broadcaster-system"
+ name="ignition::gazebo::systems::SceneBroadcaster">
+ </plugin>
+
+ <light type="directional" name="sun">
+ <cast_shadows>true</cast_shadows>
+ <pose>0 0 10 0 0 0</pose>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.2 0.2 0.2 1</specular>
+ <attenuation>
+ <range>1000</range>
+ <constant>0.9</constant>
+ <linear>0.01</linear>
+ <quadratic>0.001</quadratic>
+ </attenuation>
+ <direction>-0.5 0.1 -0.9</direction>
+ </light>
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <!--plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane-->
+ <box>
+ <size>20 20 0.1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <!--plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane-->
+ <box>
+ <size>20 20 0.1</size>
+ </box>
+ </geometry>
+ <material>
+ <ambient>0.8 0.8 0.8 1</ambient>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_no_retro_values">
+ <pose>0 -1 0.5 0 0 0</pose>
+ <link name="box_link">
+ <inertial>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ <mass>1.0</mass>
+ </inertial>
+ <collision name="box_collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <visual name="box_visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>1 0 0 1</diffuse>
+ <specular>1 0 0 1</specular>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_retro_500">
+ <pose>0 1 0.5 0 0 0</pose>
+ <link name="box_link">
+ <inertial>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ <mass>1.0</mass>
+ </inertial>
+ <collision name="box_collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <visual name="box_visual">
+ <laser_retro>500</laser_retro>
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <material>
+ <ambient>0 0 1 1</ambient>
+ <diffuse>0 0 1 1</diffuse>
+ <specular>0 0 1 1</specular>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="model_with_lidar">
+ <pose>4 0 0.5 0 0.0 3.14</pose>
+ <link name="link">
+ <pose>0.05 0.05 0.05 0 0 0</pose>
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.000166667</ixx>
+ <iyy>0.000166667</iyy>
+ <izz>0.000166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.1 0.1 0.1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.1 0.1 0.1</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <sensor name='gpu_lidar' type='gpu_lidar'>
+ <topic>lidar</topic>
+ <update_rate>10</update_rate>
+ <lidar>
+ <scan>
+ <horizontal>
+ <samples>640</samples>
+ <resolution>1</resolution>
+ <min_angle>-1.396263</min_angle>
+ <max_angle>1.396263</max_angle>
+ </horizontal>
+ <vertical>
+ <samples>16</samples>
+ <resolution>1</resolution>
+ <min_angle>-0.261799</min_angle>
+ <max_angle>0.261799</max_angle>
+ </vertical>
+ </scan>
+ <range>
+ <min>0.08</min>
+ <max>10.0</max>
+ <resolution>0.01</resolution>
+ </range>
+ </lidar>
+ <alwaysOn>1</alwaysOn>
+ <visualize>true</visualize>
+ </sensor>
+ </link>
+
+ <static>true</static>
+ </model>
+
+
+ </world>
+</sdf>
diff --git a/include/ignition/gazebo/components/LaserRetro.hh b/include/ignition/gazebo/components/LaserRetro.hh
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ */
+#ifndef IGNITION_GAZEBO_COMPONENTS_LASERRETRO_HH_
+#define IGNITION_GAZEBO_COMPONENTS_LASERRETRO_HH_
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+ /// \brief A component used to indicate an lidar reflective value
+ using LaserRetro = Component<double, class LaserRetroTag>;
+ IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.LaserRetro",
+ LaserRetro)
+}
+}
+}
+}
+
+#endif
diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
@@ -41,6 +41,7 @@
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointAxis.hh"
 #include "ignition/gazebo/components/JointType.hh"
+#include "ignition/gazebo/components/LaserRetro.hh"
 #include "ignition/gazebo/components/Lidar.hh"
 #include "ignition/gazebo/components/Light.hh"
 #include "ignition/gazebo/components/LinearAcceleration.hh"
@@ -503,6 +504,12 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual)
  this->dataPtr->ecm->CreateComponent(visualEntity,
  components::Transparency(_visual->Transparency()));
 
+ if (_visual->HasLaserRetro())
+ {
+ this->dataPtr->ecm->CreateComponent(visualEntity,
+ components::LaserRetro(_visual->LaserRetro()));
+ }
+
  if (_visual->Geom())
  {
  this->dataPtr->ecm->CreateComponent(visualEntity,

diff --git a/src/SdfEntityCreator_TEST.cc b/src/SdfEntityCreator_TEST.cc
@@ -35,6 +35,7 @@
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointAxis.hh"
 #include "ignition/gazebo/components/JointType.hh"
+#include "ignition/gazebo/components/LaserRetro.hh"
 #include "ignition/gazebo/components/Light.hh"
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/Material.hh"
@@ -101,6 +102,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities)
  EXPECT_TRUE(this->ecm.HasComponentType(components::Geometry::typeId));
  EXPECT_TRUE(this->ecm.HasComponentType(components::Material::typeId));
  EXPECT_TRUE(this->ecm.HasComponentType(components::Inertial::typeId));
+ EXPECT_TRUE(this->ecm.HasComponentType(components::LaserRetro::typeId));
 
  // Check entities
  // 1 x world + 3 x model + 3 x link + 3 x collision + 3 x visual + 1 x light
@@ -356,6 +358,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities)
  unsigned int visualCount{0};
  this->ecm.Each<components::Visual,
  components::Transparency,
+ components::LaserRetro,
  components::CastShadows,
  components::Geometry,
  components::Material,
@@ -365,6 +368,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities)
  [&](const Entity &_entity,
  const components::Visual *_visual,
  const components::Transparency *_transparency,
+ const components::LaserRetro *_laserRetro,
  const components::CastShadows *_castShadows,
  const components::Geometry *_geometry,
  const components::Material *_material,
@@ -394,6 +398,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities)
  EXPECT_EQ(boxLinkEntity, this->ecm.ParentEntity(_entity));
 
  EXPECT_DOUBLE_EQ(0.0, _transparency->Data());
+ EXPECT_DOUBLE_EQ(1150.0, _laserRetro->Data());
  EXPECT_TRUE(_castShadows->Data());
 
  EXPECT_EQ(sdf::GeometryType::BOX, _geometry->Data().Type());
@@ -417,6 +422,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities)
  EXPECT_EQ(cylLinkEntity, this->ecm.ParentEntity(_entity));
 
  EXPECT_DOUBLE_EQ(0.0, _transparency->Data());
+ EXPECT_DOUBLE_EQ(1654.0, _laserRetro->Data());
  EXPECT_TRUE(_castShadows->Data());
 
  EXPECT_EQ(sdf::GeometryType::CYLINDER, _geometry->Data().Type());
@@ -440,6 +446,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities)
  EXPECT_EQ(sphLinkEntity, this->ecm.ParentEntity(_entity));
 
  EXPECT_DOUBLE_EQ(0.5, _transparency->Data());
+ EXPECT_DOUBLE_EQ(50.0, _laserRetro->Data());
  EXPECT_FALSE(_castShadows->Data());
 
  EXPECT_EQ(sdf::GeometryType::SPHERE, _geometry->Data().Type());

diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
@@ -48,6 +48,7 @@
 #include "ignition/gazebo/components/DepthCamera.hh"
 #include "ignition/gazebo/components/GpuLidar.hh"
 #include "ignition/gazebo/components/Geometry.hh"
+#include "ignition/gazebo/components/LaserRetro.hh"
 #include "ignition/gazebo/components/Light.hh"
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/Material.hh"
@@ -671,6 +672,12 @@ void RenderUtilPrivate::CreateRenderingEntities(
  visual.SetMaterial(material->Data());
  }
 
+ auto laserRetro = _ecm.Component<components::LaserRetro>(_entity);
+ if (laserRetro != nullptr)
+ {
+ visual.SetLaserRetro(laserRetro->Data());
+ }
+
  // todo(anyone) make visual updates more generic without using extra
  // variables like entityTemp just for storing one specific visual
  // param?
@@ -863,6 +870,12 @@ void RenderUtilPrivate::CreateRenderingEntities(
  visual.SetMaterial(material->Data());
  }
 
+ auto laserRetro = _ecm.Component<components::LaserRetro>(_entity);
+ if (laserRetro != nullptr)
+ {
+ visual.SetLaserRetro(laserRetro->Data());
+ }
+
  this->newVisuals.push_back(
  std::make_tuple(_entity, visual, _parent->Data()));
  return true;

diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc
@@ -248,6 +248,11 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id,
  visualVis->SetUserData("pause-update", static_cast<int>(0));
  visualVis->SetLocalPose(_visual.RawPose());
 
+ if (_visual.HasLaserRetro())
+ {
+ visualVis->SetUserData("laser_retro", _visual.LaserRetro());
+ }
+
  math::Vector3d scale = math::Vector3d::One;
  math::Pose3d localPose;
  rendering::GeometryPtr geom =