Skip to content

Commit

Permalink
Add laser_retro support (#603)
Browse files Browse the repository at this point in the history
Signed-off-by: Guillaume Doisy <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
  • Loading branch information
doisyg and chapulina authored Feb 12, 2021
1 parent 1847513 commit 3465bbe
Show file tree
Hide file tree
Showing 7 changed files with 290 additions and 0 deletions.
214 changes: 214 additions & 0 deletions examples/worlds/gpu_lidar_retro_values_sensor.sdf
Original file line number Diff line number Diff line change
@@ -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>
41 changes: 41 additions & 0 deletions include/ignition/gazebo/components/LaserRetro.hh
Original file line number Diff line number Diff line change
@@ -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
7 changes: 7 additions & 0 deletions src/SdfEntityCreator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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,
Expand Down
7 changes: 7 additions & 0 deletions src/SdfEntityCreator_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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());
Expand All @@ -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());
Expand All @@ -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());
Expand Down
13 changes: 13 additions & 0 deletions src/rendering/RenderUtil.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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?
Expand Down Expand Up @@ -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;
Expand Down
5 changes: 5 additions & 0 deletions src/rendering/SceneManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down
Loading

0 comments on commit 3465bbe

Please sign in to comment.