Skip to content

Commit

Permalink
Merge ign-gazebo3 ➡️ ign-gazebo6 (#1481)
Browse files Browse the repository at this point in the history 
* Extruded 2D polyline geometries (#1456)

Signed-off-by: Louise Poubel <[email protected]>

* View polyline collisions on the GUI (#1481)

Signed-off-by: Louise Poubel <[email protected]>
  • Loading branch information
@chapulina
chapulina authored May 16, 2022
2 parents ce4241c + 7225f38 commit bc8e5c1
Show file tree
Hide file tree
Showing 6 changed files with 402 additions and 2 deletions.
233 changes: 233 additions & 0 deletions examples/worlds/polylines.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,233 @@
<?xml version="1.0" ?>
<!--
Demo of polyline geometries for collisions and visuals.
-->
<sdf version="1.6">
<world name="polylines">

<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.8 0.8 0.8 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>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</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="triangle">
<pose>-5 0 5 0 1.57 0</pose>
<link name="link">
<inertial>
<pose>0 0 0.5 0 0 0</pose>
<mass>1.0</mass>
<inertia>
<ixx>0.1666</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1666</iyy>
<iyz>0</iyz>
<izz>0.1666</izz>
</inertia>
</inertial>

<collision name="collision">
<geometry>
<polyline>
<point>-0.5 -0.5</point>
<point>-0.5 0.5</point>
<point>0.5 0.5</point>
<point>0 0</point>
<point>0.5 -0.5</point>
<height>1</height>
</polyline>
</geometry>
</collision>

<visual name="triangle">
<geometry>
<polyline>
<point>-0.5 -0.5</point>
<point>-0.5 0.5</point>
<point>0.5 0.5</point>
<point>0 0</point>
<point>0.5 -0.5</point>
<height>1</height>
</polyline>
</geometry>
<material>
<ambient>1.0 0 0 1</ambient>
<diffuse>1.0 0 0 1</diffuse>
<specular>1.0 0 0 1</specular>
</material>
</visual>
</link>
</model>

<model name="pentagon">
<pose>-5 -3 5 0 1.3 0</pose>
<link name="polyLine2">
<inertial>
<pose>0 0 2 0 0 0</pose>
</inertial>
<collision name="collision">
<geometry>
<polyline>
<point>-0.3 0.5</point>
<point>0.3 0.3</point>
<point>0.3 -0.3</point>
<point>-0.3 -0.5</point>
<point>-0.5 0</point>
<height>4</height>
</polyline>
</geometry>
</collision>

<visual name="polyLineGeom2">
<geometry>
<polyline>
<point>-0.3 0.5</point>
<point>0.3 0.3</point>
<point>0.3 -0.3</point>
<point>-0.3 -0.5</point>
<point>-0.5 0</point>
<height>4</height>
</polyline>
</geometry>
<material>
<ambient>0 1.0 0 1</ambient>
<diffuse>0 1.0 0 1</diffuse>
<specular>0 1.0 0 1</specular>
</material>
</visual>
</link>
</model>

<model name="cube">
<pose>-5 2 5 0 1.57 0</pose>
<link name="polyLine2">
<inertial>
<pose>0.5 0.5 0.75 0 0 0</pose>
</inertial>
<collision name="collision">
<geometry>
<polyline>
<point>0 0</point>
<point>0 1</point>
<point>1 1</point>
<point>1 0 </point>
<height>1.5</height>
</polyline>
</geometry>
</collision>

<visual name="polyLineGeom2">
<geometry>
<polyline>
<point>0 0</point>
<point>0 1</point>
<point>1 1</point>
<point>1 0 </point>
<height>1.5</height>
</polyline>
</geometry>
<material>
<ambient>0 1.0 1.0 1</ambient>
<diffuse>0 1.0 1.0 1</diffuse>
<specular>0 1.0 1.0 1</specular>
</material>
</visual>
</link>
</model>

<model name="letter">
<pose>-5 2 5 0 1.57 0</pose>
<link name="link">
<inertial>
<pose>2.2 1.5 0.5 0 0 0</pose>
</inertial>
<collision name="collision">
<geometry>
<polyline>
<point>2.27467 1.0967</point>
<point>1.81094 2.35418</point>
<point>2.74009 2.35418</point>
<height>1.0</height>
</polyline>
<polyline>
<point>2.08173 0.7599</point>
<point>2.4693 0.7599</point>
<point>3.4323 3.28672</point>
<point>3.07689 3.28672</point>
<point>2.84672 2.63851</point>
<point>1.7077 2.63851</point>
<point>1.47753 3.28672</point>
<point>1.11704 3.28672</point>
<height>1.0</height>
</polyline>
</geometry>
</collision>

<visual name="visual">
<geometry>
<polyline>
<point>2.27467 1.0967</point>
<point>1.81094 2.35418</point>
<point>2.74009 2.35418</point>
<height>1.0</height>
</polyline>
<polyline>
<point>2.08173 0.7599</point>
<point>2.4693 0.7599</point>
<point>3.4323 3.28672</point>
<point>3.07689 3.28672</point>
<point>2.84672 2.63851</point>
<point>1.7077 2.63851</point>
<point>1.47753 3.28672</point>
<point>1.11704 3.28672</point>
<height>1.0</height>
</polyline>
</geometry>
<material>
<ambient>1.0 0 1.0 1</ambient>
<diffuse>1.0 0 1.0 1</diffuse>
<specular>1.0 0 1.0 1</specular>
</material>
</visual>
</link>
</model>
</world>
</sdf>
37 changes: 37 additions & 0 deletions src/Conversions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@
#include <sdf/NavSat.hh>
#include <sdf/Pbr.hh>
#include <sdf/Plane.hh>
#include <sdf/Polyline.hh>
#include <sdf/Sphere.hh>

#include <algorithm>
Expand Down Expand Up @@ -242,6 +243,20 @@ msgs::Geometry ignition::gazebo::convert(const sdf::Geometry &_in)
blendMsg->set_fade_dist(blendSdf->FadeDistance());
}
}
else if (_in.Type() == sdf::GeometryType::POLYLINE &&
!_in.PolylineShape().empty())
{
out.set_type(msgs::Geometry::POLYLINE);
for (const auto &polyline : _in.PolylineShape())
{
auto polylineMsg = out.add_polyline();
polylineMsg->set_height(polyline.Height());
for (const auto &point : polyline.Points())
{
msgs::Set(polylineMsg->add_point(), point);
}
}
}
else
{
ignerr << "Geometry type [" << static_cast<int>(_in.Type())
Expand Down Expand Up @@ -357,6 +372,28 @@ sdf::Geometry ignition::gazebo::convert(const msgs::Geometry &_in)

out.SetHeightmapShape(heightmapShape);
}
else if (_in.type() == msgs::Geometry::POLYLINE && _in.polyline_size() > 0)
{
out.SetType(sdf::GeometryType::POLYLINE);

std::vector<sdf::Polyline> polylines;

for (auto i = 0; i < _in.polyline().size(); ++i)
{
auto polylineMsg = _in.polyline(i);
sdf::Polyline polylineShape;
polylineShape.SetHeight(polylineMsg.height());

for (auto j = 0; j < polylineMsg.point().size(); ++j)
{
polylineShape.AddPoint(msgs::Convert(polylineMsg.point(j)));
}

polylines.push_back(polylineShape);
}

out.SetPolylineShape(polylines);
}
else
{
ignerr << "Geometry type [" << static_cast<int>(_in.type())
Expand Down
39 changes: 37 additions & 2 deletions src/Conversions_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include <sdf/Pbr.hh>
#include <sdf/Physics.hh>
#include <sdf/Plane.hh>
#include <sdf/Polyline.hh>
#include <sdf/Root.hh>
#include <sdf/Scene.hh>
#include <sdf/Sphere.hh>
Expand Down Expand Up @@ -564,6 +565,40 @@ TEST(Conversions, GeometryHeightmap)
EXPECT_DOUBLE_EQ(456.789, newHeightmap->BlendByIndex(0)->FadeDistance());
}

/////////////////////////////////////////////////
TEST(Conversions, GeometryPolyline)
{
sdf::Geometry geometry;
geometry.SetType(sdf::GeometryType::POLYLINE);

sdf::Polyline polylineShape;
polylineShape.SetHeight(1.23);
polylineShape.AddPoint({4.5, 6.7});
polylineShape.AddPoint({8.9, 10.11});
geometry.SetPolylineShape({polylineShape});

auto geometryMsg = convert<msgs::Geometry>(geometry);
EXPECT_EQ(msgs::Geometry::POLYLINE, geometryMsg.type());
ASSERT_EQ(1, geometryMsg.polyline_size());
EXPECT_DOUBLE_EQ(1.23, geometryMsg.polyline(0).height());
ASSERT_EQ(2, geometryMsg.polyline(0).point_size());
EXPECT_DOUBLE_EQ(4.5, geometryMsg.polyline(0).point(0).x());
EXPECT_DOUBLE_EQ(6.7, geometryMsg.polyline(0).point(0).y());
EXPECT_DOUBLE_EQ(8.9, geometryMsg.polyline(0).point(1).x());
EXPECT_DOUBLE_EQ(10.11, geometryMsg.polyline(0).point(1).y());

auto newGeometry = convert<sdf::Geometry>(geometryMsg);
EXPECT_EQ(sdf::GeometryType::POLYLINE, newGeometry.Type());
ASSERT_FALSE(newGeometry.PolylineShape().empty());
ASSERT_EQ(1u, newGeometry.PolylineShape().size());
EXPECT_DOUBLE_EQ(1.23, newGeometry.PolylineShape()[0].Height());
ASSERT_EQ(2u, newGeometry.PolylineShape()[0].PointCount());
EXPECT_DOUBLE_EQ(4.5, newGeometry.PolylineShape()[0].PointByIndex(0)->X());
EXPECT_DOUBLE_EQ(6.7, newGeometry.PolylineShape()[0].PointByIndex(0)->Y());
EXPECT_DOUBLE_EQ(8.9, newGeometry.PolylineShape()[0].PointByIndex(1)->X());
EXPECT_DOUBLE_EQ(10.11, newGeometry.PolylineShape()[0].PointByIndex(1)->Y());
}

/////////////////////////////////////////////////
TEST(Conversions, Inertial)
{
Expand Down Expand Up @@ -720,7 +755,7 @@ TEST(Conversions, Atmosphere)
}

/////////////////////////////////////////////////
TEST(CONVERSIONS, MagnetometerSensor)
TEST(Conversions, MagnetometerSensor)
{
sdf::Sensor sensor;
sensor.SetName("my_sensor");
Expand Down Expand Up @@ -760,7 +795,7 @@ TEST(CONVERSIONS, MagnetometerSensor)
}

/////////////////////////////////////////////////
TEST(CONVERSIONS, AltimeterSensor)
TEST(Conversions, AltimeterSensor)
{
sdf::Sensor sensor;
sensor.SetName("my_sensor");
Expand Down
Loading

0 comments on commit bc8e5c1

Please sign in to comment.