Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

🌐 Spherical coordinates conversion and commands #177

Merged
merged 5 commits into from
Sep 16, 2021
Merged

🌐 Spherical coordinates conversion and commands #177

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
927dbe2
spherical coords conversion
chapulina Sep 3, 2021
00df67b
codecheck
chapulina Sep 4, 2021
c11ebb3
Print warnings
chapulina Sep 10, 2021
b64edb8
Support spawning and moving in spherical coordinates
chapulina Sep 10, 2021
9ca5628
Merge branch 'main' into chapulina/8/spherical_coords
chapulina Sep 13, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 25 additions & 0 deletions include/ignition/msgs/Utility.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <ignition/math/Inertial.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Plane.hh>
#include <ignition/math/SphericalCoordinates.hh>
#include <ignition/math/Vector3.hh>

#include "ignition/msgs/config.hh"
Expand Down Expand Up @@ -85,6 +86,14 @@ namespace ignition
IGNITION_MSGS_VISIBLE
math::Inertiald Convert(const msgs::Inertial &_i);

/// \brief Convert a msgs::SphericalCoordinates to an
/// ignition::math::SphericalCoordinates
/// \param[in] _coord The spherical coordinates to convert
/// \return An ignition::math::SphericalCoordinates object
IGNITION_MSGS_VISIBLE
math::SphericalCoordinates Convert(
const msgs::SphericalCoordinates &_coord);

/// \brief Convert a msgs::AxisAlignedBox to an
/// ignition::math::AxisAlignedBox
/// \param[in] _b The axis aligned box to convert
Expand Down Expand Up @@ -195,6 +204,14 @@ namespace ignition
IGNITION_MSGS_VISIBLE
msgs::Inertial Convert(const math::MassMatrix3d &_m);

/// \brief Convert an math::SphericalCoordinates to a
/// msgs::SphericalCoordinates
/// \param[in] _coord The SphericalCoordinates to convert
/// \return A msgs::SphericalCoordinates object
IGNITION_MSGS_VISIBLE
msgs::SphericalCoordinates Convert(
const math::SphericalCoordinates &_coord);

/// \brief Convert a ignition::math::Planed to a msgs::PlaneGeom
/// \param[in] _p The plane to convert
/// \return A msgs::PlaneGeom object
Expand Down Expand Up @@ -352,6 +369,14 @@ namespace ignition
IGNITION_MSGS_VISIBLE
void Set(msgs::Inertial *_i, const math::MassMatrix3d &_m);

/// \brief Set a msgs::SphericalCoordinates from a
/// math::SphericalCoordinates
/// \param[out] _sc A msgs::SphericalCoordinates pointer
/// \param[in] _m An math::SphericalCoordinates reference
IGNITION_MSGS_VISIBLE
void Set(msgs::SphericalCoordinates *_sc,
const math::SphericalCoordinates &_m);

/// \brief Set a msgs::Plane from an ignition::math::Planed
/// \param[out] _p A msgs::Plane pointer
/// \param[in] _v An ignition::math::Planed reference
Expand Down
11 changes: 9 additions & 2 deletions proto/ignition/msgs/entity_factory.proto
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,11 @@ option java_outer_classname = "EntityFactoryProtos";
/// 3. From a message (model / light)
/// 4. Cloning an existing entity (clone_name)

import "ignition/msgs/pose.proto";
import "ignition/msgs/header.proto";
import "ignition/msgs/light.proto";
import "ignition/msgs/model.proto";
import "ignition/msgs/header.proto";
import "ignition/msgs/pose.proto";
import "ignition/msgs/spherical_coordinates.proto";

message EntityFactory
{
Expand All @@ -60,6 +61,7 @@ message EntityFactory
}

/// \brief Pose where the entity will be spawned in the world.
/// If set, `spherical_coordinates` will be ignored.
Pose pose = 7;

/// \brief New name for the entity, overrides the name on the SDF.
Expand All @@ -72,4 +74,9 @@ message EntityFactory
/// \brief The pose will be defined relative to this frame. If left empty,
/// the "world" frame will be used.
string relative_to = 10;

/// \brief Spherical coordinates where the entity will be spawned in the
/// world.
/// It's ignored if `pose` is set.
SphericalCoordinates spherical_coordinates = 11;
}
20 changes: 18 additions & 2 deletions proto/ignition/msgs/spherical_coordinates.proto
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,23 +22,39 @@ option java_outer_classname = "SphericalCoordinatesProtos";

/// \ingroup ignition.msgs
/// \interface SphericalCoordinates
/// \brief Spherical coordinates information
/// \brief Spherical coordinates information

import "ignition/msgs/entity.proto";
import "ignition/msgs/header.proto";

message SphericalCoordinates
{
/// \brief Planetary surface models.
enum SurfaceModel
{
/// \brief World Geodetic System 1984
EARTH_WGS84 = 0;
}

/// \brief Optional header data
/// \brief Optional header data.
Header header = 1;

/// \brief Planetary surface model.
SurfaceModel surface_model = 2;

/// \brief Latitude in degrees.
double latitude_deg = 3;

/// \brief Longitude in degrees.
double longitude_deg = 4;

/// \brief Elevation in meters.
double elevation = 5;

/// \brief Heading in degrees.
double heading_deg = 6;

/// \brief Entity that the coordinates apply to.
/// If not set, defaults to the world origin.
Entity entity = 7;
}
49 changes: 49 additions & 0 deletions src/Utility.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,27 @@ namespace ignition
pose);
}

/////////////////////////////////////////////
math::SphericalCoordinates Convert(const msgs::SphericalCoordinates &_sc)
{
math::SphericalCoordinates out;
if (_sc.surface_model() == msgs::SphericalCoordinates::EARTH_WGS84)
{
out.SetSurface(math::SphericalCoordinates::EARTH_WGS84);
}
else
{
std::cerr << "Unrecognized spherical surface type ["
<< _sc.surface_model()
<< "]. Using default." << std::endl;
}
out.SetLatitudeReference(IGN_DTOR(_sc.latitude_deg()));
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does it make sense to continue doing the conversion if the surface_model isn't supported? I think we shouldn't do it as the values will be probably incorrect.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The ign-math class is populated with default values, so the caller wouldn't know if we're populating the message or leaving the default values. So instead of skipping the rest of the message, I'm printing a warning on c11ebb3.

For the messages, the field is left empty, and I also printed a warning, but still populated the rest of the function.

I think that ultimately, it shouldn't be the conversion function's responsibility to decide what are valid combinations. It should convert as much as possible and the caller should be the one deciding what is acceptable. What do you think?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Practically speaking I don't see any problems right now because we only have one surface type in Ignition Msgs, so it's impossible to receive an unsupported type. I agree that potentially receiving a default math::SphericalCoordinates is ambiguous but returning a math::SphericalCoordinates with default surface type (WGS_84) but potentially lat/lon, heading and elevation references coming from a different surface type could be hard to debug. The error message definitely helps.

I'm approving but as an idea, we could create a new type in ignition::math::SurfaceType that is Unsupported or similar. We could use this type in this conversion for example, when we detect an unsupported type set in ignition::msgs::SphericalCoordinates. This way, after calling convert() we can programmatically check if the conversion was right.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yup, makes sense. Ticketed gazebosim/gz-math#237.

out.SetLongitudeReference(IGN_DTOR(_sc.longitude_deg()));
out.SetElevationReference(_sc.elevation());
out.SetHeadingOffset(IGN_DTOR(_sc.heading_deg()));
return out;
}

/////////////////////////////////////////////
math::AxisAlignedBox Convert(const msgs::AxisAlignedBox &_b)
{
Expand Down Expand Up @@ -278,6 +299,14 @@ namespace ignition
return result;
}

/////////////////////////////////////////////
msgs::SphericalCoordinates Convert(const math::SphericalCoordinates &_sc)
{
msgs::SphericalCoordinates result;
msgs::Set(&result, _sc);
return result;
}

/////////////////////////////////////////////
msgs::PlaneGeom Convert(const ignition::math::Planed &_p)
{
Expand Down Expand Up @@ -433,6 +462,26 @@ namespace ignition
msgs::Set(_i->mutable_pose(), _m.Pose());
}

/////////////////////////////////////////////////
void Set(msgs::SphericalCoordinates *_sc,
const math::SphericalCoordinates &_m)
{
if (_m.Surface() == math::SphericalCoordinates::EARTH_WGS84)
{
_sc->set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84);
}
else
{
std::cerr << "Unrecognized spherical surface type ["
<< _m.Surface()
<< "]. Not populating message field." << std::endl;
}
_sc->set_latitude_deg(_m.LatitudeReference().Degree());
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See my previous comment about unsuported surface model.

_sc->set_longitude_deg(_m.LongitudeReference().Degree());
_sc->set_elevation(_m.ElevationReference());
_sc->set_heading_deg(_m.HeadingOffset().Degree());
}

/////////////////////////////////////////////////
void Set(msgs::StringMsg *_p, const std::string &_v)
{
Expand Down
38 changes: 38 additions & 0 deletions src/Utility_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -236,6 +236,29 @@ TEST(MsgsTest, ConvertMathMassMatrix3ToMsgs)
EXPECT_EQ(ignition::math::Pose3d::Zero, msgs::Convert(msg.pose()));
}

/////////////////////////////////////////////////
TEST(MsgsTest, ConvertMathSphericalCoordinatesToMsgs)
{
auto msg = msgs::Convert(
math::SphericalCoordinates(
math::SphericalCoordinates::SurfaceType::EARTH_WGS84,
IGN_DTOR(1.1), IGN_DTOR(2.2), 3.3, IGN_DTOR(0.4)));

EXPECT_EQ(msgs::SphericalCoordinates::EARTH_WGS84, msg.surface_model());
EXPECT_DOUBLE_EQ(1.1, msg.latitude_deg());
EXPECT_DOUBLE_EQ(2.2, msg.longitude_deg());
EXPECT_DOUBLE_EQ(3.3, msg.elevation());
EXPECT_DOUBLE_EQ(0.4, msg.heading_deg());

auto math = msgs::Convert(msg);

EXPECT_EQ(math::SphericalCoordinates::EARTH_WGS84, math.Surface());
EXPECT_DOUBLE_EQ(1.1, math.LatitudeReference().Degree());
EXPECT_DOUBLE_EQ(2.2, math.LongitudeReference().Degree());
EXPECT_DOUBLE_EQ(3.3, math.ElevationReference());
EXPECT_DOUBLE_EQ(0.4, math.HeadingOffset().Degree());
}

/////////////////////////////////////////////////
TEST(UtilityTest, ConvertStringMsg)
{
Expand Down Expand Up @@ -451,6 +474,21 @@ TEST(MsgsTest, SetMassMatrix3)
EXPECT_EQ(ignition::math::Pose3d::Zero, msgs::Convert(msg.pose()));
}

/////////////////////////////////////////////////
TEST(MsgsTest, SetSphericalCoordinates)
{
msgs::SphericalCoordinates msg;
msgs::Set(&msg, math::SphericalCoordinates(
math::SphericalCoordinates::SurfaceType::EARTH_WGS84,
IGN_DTOR(1.1), IGN_DTOR(2.2), 3.3, IGN_DTOR(0.4)));

EXPECT_EQ(msgs::SphericalCoordinates::EARTH_WGS84, msg.surface_model());
EXPECT_DOUBLE_EQ(1.1, msg.latitude_deg());
EXPECT_DOUBLE_EQ(2.2, msg.longitude_deg());
EXPECT_DOUBLE_EQ(3.3, msg.elevation());
EXPECT_DOUBLE_EQ(0.4, msg.heading_deg());
}

/////////////////////////////////////////////////
TEST(MsgsTest, SetStringMsg)
{
Expand Down