Skip to content

Commit

Permalink
Merge pull request #372 from azeey/9_to_main
Browse files Browse the repository at this point in the history
Merge gz-msgs9 ➡️  main
  • Loading branch information
azeey authored Aug 24, 2023
2 parents 634e567 + bd715af commit a8b2d84
Show file tree
Hide file tree
Showing 7 changed files with 187 additions and 6 deletions.
8 changes: 3 additions & 5 deletions .github/workflows/triage.yml
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,8 @@ jobs:
runs-on: ubuntu-latest
steps:
- name: Add ticket to inbox
uses: technote-space/create-project-card-action@v1
uses: actions/[email protected]
with:
PROJECT: Core development
COLUMN: Inbox
GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }}
CHECK_ORG_PROJECT: true
project-url: https:/orgs/gazebosim/projects/7
github-token: ${{ secrets.TRIAGE_TOKEN }}

16 changes: 16 additions & 0 deletions include/gz/msgs/Utility.hh
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,14 @@ namespace gz
math::SphericalCoordinates Convert(
const msgs::SphericalCoordinates &_coord);

/// \brief Convert a msgs::SphericalCoordinatesType to an
/// gz::math::SphericalCoordinates::CoordinateTpye
/// \param[in] _sc The spherical coordinate type to convert
/// \return A gz::math::SphericalCoordinatesType object
GZ_MSGS_VISIBLE
math::SphericalCoordinates::CoordinateType Convert(
const msgs::SphericalCoordinatesType &_sc);

/// \brief Convert a msgs::AxisAlignedBox to an
/// gz::math::AxisAlignedBox
/// \param[in] _b The axis aligned box to convert
Expand Down Expand Up @@ -212,6 +220,14 @@ namespace gz
msgs::SphericalCoordinates Convert(
const math::SphericalCoordinates &_coord);

/// \brief Convert a gz::math::SphericalCoordinates::CoordinateType
/// to a msgs::SphericalCoordinatesType
/// \param[in] _coord The spherical coordinates type to convert
/// \return A gz::msgs::SphericalCoordinatesType object
GZ_MSGS_VISIBLE
msgs::SphericalCoordinatesType ConvertCoord(
const math::SphericalCoordinates::CoordinateType &_coord);

/// \brief Convert a gz::math::Planed to a msgs::PlaneGeom
/// \param[in] _p The plane to convert
/// \return A msgs::PlaneGeom object
Expand Down
58 changes: 58 additions & 0 deletions proto/gz/msgs/data_load_options.proto
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
/*
* Copyright (C) 2023 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.
*
*/

syntax = "proto3";
package gz.msgs;
option java_package = "com.gz.msgs";
option java_outer_classname = "DataLoadPathOptions";

import "gz/msgs/spherical_coordinates.proto";

/// \brief Used for specifying how to load environmental data
message DataLoadPathOptions
{
/// \brief Units used by spherical coordinates
enum DataAngularUnits
{
RADIANS = 0;
DEGREES = 1;
}

/// \brief File path to load
string path = 1;

/// \brief Name of time axis
string time = 2;

/// \brief Is the data static in time
bool static_time = 3;

/// \brief Name of x axis
string x = 4;

/// \brief Name of y axis
string y = 5;

/// \brief Name of z axis
string z = 6;

/// Units
DataAngularUnits units = 7;

/// Spherical Coodinate type
gz.msgs.SphericalCoordinatesType coordinate_type = 8;
}
20 changes: 20 additions & 0 deletions proto/gz/msgs/spherical_coordinates.proto
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,26 @@ option java_outer_classname = "SphericalCoordinatesProtos";
import "gz/msgs/entity.proto";
import "gz/msgs/header.proto";

enum SphericalCoordinatesType
{
/// \brief Latitude, Longitude and Altitude by SurfaceType
SPHERICAL = 0;

/// \brief Earth centered, earth fixed Cartesian
ECEF = 1;

/// \brief Local tangent plane (East, North, Up)
GLOBAL = 2;

/// \brief Heading-adjusted tangent plane (X, Y, Z)
/// This has kept a bug for backwards compatibility, use
/// LOCAL2 for the correct behaviour.
LOCAL = 3;

/// \brief Heading-adjusted tangent plane (X, Y, Z)
LOCAL2 = 4;
}

message SphericalCoordinates
{
/// \brief Planetary surface models.
Expand Down
50 changes: 50 additions & 0 deletions src/Utility.cc
Original file line number Diff line number Diff line change
Expand Up @@ -176,6 +176,28 @@ namespace gz
return out;
}

/////////////////////////////////////////////
math::SphericalCoordinates::CoordinateType Convert(
const msgs::SphericalCoordinatesType &_sc)
{
switch (_sc)
{
case msgs::SphericalCoordinatesType::ECEF:
return math::SphericalCoordinates::CoordinateType::ECEF;
case msgs::SphericalCoordinatesType::GLOBAL:
return math::SphericalCoordinates::CoordinateType::GLOBAL;
case msgs::SphericalCoordinatesType::SPHERICAL:
return math::SphericalCoordinates::CoordinateType::SPHERICAL;
case msgs::SphericalCoordinatesType::LOCAL:
return math::SphericalCoordinates::CoordinateType::LOCAL;
case msgs::SphericalCoordinatesType::LOCAL2:
return math::SphericalCoordinates::CoordinateType::LOCAL2;
default:
std::cerr << "Invalid coordinate type passed" << std::endl;
}
return math::SphericalCoordinates::CoordinateType::LOCAL2;
}

/////////////////////////////////////////////
math::AxisAlignedBox Convert(const msgs::AxisAlignedBox &_b)
{
Expand Down Expand Up @@ -342,6 +364,34 @@ namespace gz
return result;
}

/////////////////////////////////////////////
msgs::SphericalCoordinatesType ConvertCoord(
const math::SphericalCoordinates::CoordinateType &_sc)
{
auto result = msgs::SphericalCoordinatesType::LOCAL2;
switch (_sc)
{
case math::SphericalCoordinates::CoordinateType::ECEF:
result = msgs::SphericalCoordinatesType::ECEF;
break;
case math::SphericalCoordinates::CoordinateType::GLOBAL:
result = msgs::SphericalCoordinatesType::GLOBAL;
break;
case math::SphericalCoordinates::CoordinateType::SPHERICAL:
result = msgs::SphericalCoordinatesType::SPHERICAL;
break;
case math::SphericalCoordinates::CoordinateType::LOCAL:
result = msgs::SphericalCoordinatesType::LOCAL;
break;
case math::SphericalCoordinates::CoordinateType::LOCAL2:
result = msgs::SphericalCoordinatesType::LOCAL2;
break;
default:
std::cerr << "Invalid coordinate type passed" << std::endl;
}
return result;
}

/////////////////////////////////////////////
msgs::PlaneGeom Convert(const gz::math::Planed &_p)
{
Expand Down
40 changes: 40 additions & 0 deletions src/Utility_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -398,6 +398,46 @@ TEST(MsgsTest, ConvertMathSphericalCoordinatesToMsgs)
EXPECT_DOUBLE_EQ(10000, mathCustom.SurfaceAxisPolar());
}

/////////////////////////////////////////////////
TEST(MsgsTest, ConvertMsgsSphericalCoordinatesTypeToMath)
{
EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::ECEF),
math::SphericalCoordinates::CoordinateType::ECEF);
EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::GLOBAL),
math::SphericalCoordinates::CoordinateType::GLOBAL);
EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::SPHERICAL),
math::SphericalCoordinates::CoordinateType::SPHERICAL);
EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::LOCAL),
math::SphericalCoordinates::CoordinateType::LOCAL);
EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::LOCAL2),
math::SphericalCoordinates::CoordinateType::LOCAL2);
EXPECT_EQ(Convert((msgs::SphericalCoordinatesType)500000),
math::SphericalCoordinates::CoordinateType::LOCAL2);
}

/////////////////////////////////////////////////
TEST(MsgsTest, ConvertMathSphericalCoordinatedTypeToMsg)
{
EXPECT_EQ(msgs::ConvertCoord(
math::SphericalCoordinates::CoordinateType::ECEF),
msgs::SphericalCoordinatesType::ECEF);
EXPECT_EQ(msgs::ConvertCoord(
math::SphericalCoordinates::CoordinateType::GLOBAL),
msgs::SphericalCoordinatesType::GLOBAL);
EXPECT_EQ(msgs::ConvertCoord(
math::SphericalCoordinates::CoordinateType::SPHERICAL),
msgs::SphericalCoordinatesType::SPHERICAL);
EXPECT_EQ(msgs::ConvertCoord(
math::SphericalCoordinates::CoordinateType::LOCAL),
msgs::SphericalCoordinatesType::LOCAL);
EXPECT_EQ(msgs::ConvertCoord(
math::SphericalCoordinates::CoordinateType::LOCAL2),
msgs::SphericalCoordinatesType::LOCAL2);
EXPECT_EQ(msgs::ConvertCoord(
(math::SphericalCoordinates::CoordinateType)500000),
msgs::SphericalCoordinatesType::LOCAL2);
}

/////////////////////////////////////////////////
TEST(UtilityTest, ConvertStringMsg)
{
Expand Down
1 change: 0 additions & 1 deletion tools/gz_msgs_generate.bzl
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,6 @@ _gz_msgs_generate_gen = rule(
cfg = "host",
),
},
output_to_genfiles = True,
implementation = _gz_msgs_generate_impl,
)

Expand Down

0 comments on commit a8b2d84

Please sign in to comment.