p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/msgs/msgs_TEST.cc

3533 lines
121 KiB
C++
Raw Normal View History

first
2019-03-28 10:57:49 +08:00
/*
* Copyright (C) 2012 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.
*
*/
#include <gtest/gtest.h>
#include "gazebo/msgs/msgs.hh"
#include "gazebo/common/Exception.hh"
#include "test/util.hh"
using namespace gazebo;
class MsgsTest : public gazebo::testing::AutoLogFixture { };
void TimeTest(const common::Time &_t, const msgs::Time &_msg)
{
EXPECT_LE(_t.sec, _msg.sec());
if (_t.sec == _msg.sec())
EXPECT_LE(_t.nsec, _msg.nsec());
}
TEST_F(MsgsTest, Msg)
{
common::Time t = common::Time::GetWallTime();
msgs::Test msg, msg2;
msgs::Init(msg, "_test_");
msgs::Init(msg2);
ASSERT_TRUE(msg.header().has_stamp());
TimeTest(t, msg.header().stamp());
EXPECT_STREQ("_test_", msg.header().str_id().c_str());
ASSERT_TRUE(msg2.header().has_stamp());
TimeTest(t, msg2.header().stamp());
EXPECT_FALSE(msg2.header().has_str_id());
msgs::Header *header = msgs::GetHeader(msg);
EXPECT_STREQ("_test_", header->str_id().c_str());
msgs::Header testHeader;
testHeader.set_str_id("_hello_");
header = msgs::GetHeader(testHeader);
EXPECT_STREQ("_hello_", header->str_id().c_str());
}
TEST_F(MsgsTest, Request)
{
msgs::Request *request = msgs::CreateRequest("help", "me");
EXPECT_STREQ("help", request->request().c_str());
EXPECT_STREQ("me", request->data().c_str());
EXPECT_GT(request->id(), 0);
}
TEST_F(MsgsTest, Time)
{
common::Time t = common::Time::GetWallTime();
msgs::Time msg;
msgs::Stamp(&msg);
TimeTest(t, msg);
}
TEST_F(MsgsTest, TimeFromHeader)
{
common::Time t = common::Time::GetWallTime();
msgs::Header msg;
msgs::Stamp(&msg);
TimeTest(t, msg.stamp());
}
TEST_F(MsgsTest, Packet)
{
msgs::GzString msg;
msg.set_data("test_string");
std::string data = msgs::Package("test_type", msg);
msgs::Packet packet;
packet.ParseFromString(data);
msg.ParseFromString(packet.serialized_data());
EXPECT_STREQ("test_type", packet.type().c_str());
EXPECT_STREQ("test_string", msg.data().c_str());
}
TEST_F(MsgsTest, BadPackage)
{
msgs::GzString msg;
EXPECT_THROW(msgs::Package("test_type", msg), common::Exception);
}
TEST_F(MsgsTest, ConvertDoubleToAny)
{
msgs::Any msg = msgs::ConvertAny(1.0);
EXPECT_EQ(msg.type(), msgs::Any::DOUBLE);
ASSERT_TRUE(msg.has_double_value());
EXPECT_DOUBLE_EQ(1, msg.double_value());
}
TEST_F(MsgsTest, ConvertIntToAny)
{
msgs::Any msg = msgs::ConvertAny(2);
EXPECT_EQ(msg.type(), msgs::Any::INT32);
ASSERT_TRUE(msg.has_int_value());
EXPECT_DOUBLE_EQ(2, msg.int_value());
}
TEST_F(MsgsTest, ConvertStringToAny)
{
msgs::Any msg = msgs::ConvertAny("test_string");
EXPECT_EQ(msg.type(), msgs::Any::STRING);
ASSERT_TRUE(msg.has_string_value());
EXPECT_EQ("test_string", msg.string_value());
}
TEST_F(MsgsTest, ConvertBoolToAny)
{
msgs::Any msg = msgs::ConvertAny(true);
EXPECT_EQ(msg.type(), msgs::Any::BOOLEAN);
ASSERT_TRUE(msg.has_bool_value());
EXPECT_TRUE(msg.bool_value());
}
TEST_F(MsgsTest, ConvertVector3dToAny)
{
msgs::Any msg = msgs::ConvertAny(ignition::math::Vector3d(1, 2, 3));
EXPECT_EQ(msg.type(), msgs::Any::VECTOR3D);
ASSERT_TRUE(msg.has_vector3d_value());
EXPECT_DOUBLE_EQ(1, msg.vector3d_value().x());
EXPECT_DOUBLE_EQ(2, msg.vector3d_value().y());
EXPECT_DOUBLE_EQ(3, msg.vector3d_value().z());
}
TEST_F(MsgsTest, ConvertColorToAny)
{
msgs::Any msg = msgs::ConvertAny(common::Color(.1, .2, .3, 1.0));
EXPECT_EQ(msg.type(), msgs::Any::COLOR);
ASSERT_TRUE(msg.has_color_value());
EXPECT_DOUBLE_EQ(0.1f, msg.color_value().r());
EXPECT_DOUBLE_EQ(0.2f, msg.color_value().g());
EXPECT_DOUBLE_EQ(0.3f, msg.color_value().b());
EXPECT_DOUBLE_EQ(1.0f, msg.color_value().a());
}
TEST_F(MsgsTest, ConvertPose3dToAny)
{
msgs::Any msg = msgs::ConvertAny(ignition::math::Pose3d(
ignition::math::Vector3d(1, 2, 3),
ignition::math::Quaterniond(4, 5, 6, 7)));
EXPECT_EQ(msg.type(), msgs::Any::POSE3D);
ASSERT_TRUE(msg.has_pose3d_value());
EXPECT_DOUBLE_EQ(1, msg.pose3d_value().position().x());
EXPECT_DOUBLE_EQ(2, msg.pose3d_value().position().y());
EXPECT_DOUBLE_EQ(3, msg.pose3d_value().position().z());
EXPECT_DOUBLE_EQ(msg.pose3d_value().orientation().w(), 4.0);
EXPECT_DOUBLE_EQ(msg.pose3d_value().orientation().x(), 5.0);
EXPECT_DOUBLE_EQ(msg.pose3d_value().orientation().y(), 6.0);
EXPECT_DOUBLE_EQ(msg.pose3d_value().orientation().z(), 7.0);
}
TEST_F(MsgsTest, ConvertQuaternionToAny)
{
msgs::Any msg = msgs::ConvertAny(
ignition::math::Quaterniond(1, 2, 3, 4));
EXPECT_EQ(msg.type(), msgs::Any::QUATERNIOND);
ASSERT_TRUE(msg.has_quaternion_value());
EXPECT_DOUBLE_EQ(msg.quaternion_value().w(), 1.0);
EXPECT_DOUBLE_EQ(msg.quaternion_value().x(), 2.0);
EXPECT_DOUBLE_EQ(msg.quaternion_value().y(), 3.0);
EXPECT_DOUBLE_EQ(msg.quaternion_value().z(), 4.0);
}
TEST_F(MsgsTest, ConvertTimeToAny)
{
msgs::Any msg = msgs::ConvertAny(common::Time(2, 123));
EXPECT_EQ(msg.type(), msgs::Any::TIME);
ASSERT_TRUE(msg.has_time_value());
EXPECT_EQ(2, msg.time_value().sec());
EXPECT_EQ(123, msg.time_value().nsec());
}
TEST_F(MsgsTest, CovertMathVector3ToMsgs)
{
msgs::Vector3d msg = msgs::Convert(ignition::math::Vector3d(1, 2, 3));
EXPECT_DOUBLE_EQ(1, msg.x());
EXPECT_DOUBLE_EQ(2, msg.y());
EXPECT_DOUBLE_EQ(3, msg.z());
}
TEST_F(MsgsTest, ConvertMsgsVector3dToMath)
{
msgs::Vector3d msg = msgs::Convert(ignition::math::Vector3d(1, 2, 3));
ignition::math::Vector3d v = msgs::ConvertIgn(msg);
EXPECT_DOUBLE_EQ(1, v.X());
EXPECT_DOUBLE_EQ(2, v.Y());
EXPECT_DOUBLE_EQ(3, v.Z());
}
TEST_F(MsgsTest, ConvertMathQuaterionToMsgs)
{
msgs::Quaternion msg =
msgs::Convert(ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI));
EXPECT_DOUBLE_EQ(msg.x(), -0.65328148243818818);
EXPECT_DOUBLE_EQ(msg.y(), 0.27059805007309856);
EXPECT_DOUBLE_EQ(msg.z(), 0.65328148243818829);
EXPECT_DOUBLE_EQ(msg.w(), 0.27059805007309851);
}
TEST_F(MsgsTest, ConvertMsgsQuaterionToMath)
{
msgs::Quaternion msg =
msgs::Convert(ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI));
ignition::math::Quaterniond v = msgs::ConvertIgn(msg);
EXPECT_DOUBLE_EQ(v.X(), -0.65328148243818818);
EXPECT_DOUBLE_EQ(v.Y(), 0.27059805007309856);
EXPECT_DOUBLE_EQ(v.Z(), 0.65328148243818829);
EXPECT_DOUBLE_EQ(v.W(), 0.27059805007309851);
}
TEST_F(MsgsTest, ConvertPoseMathToMsgs)
{
msgs::Pose msg = msgs::Convert(ignition::math::Pose3d(
ignition::math::Vector3d(1, 2, 3),
ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI)));
EXPECT_DOUBLE_EQ(1, msg.position().x());
EXPECT_DOUBLE_EQ(2, msg.position().y());
EXPECT_DOUBLE_EQ(3, msg.position().z());
EXPECT_DOUBLE_EQ(msg.orientation().x(), -0.65328148243818818);
EXPECT_DOUBLE_EQ(msg.orientation().y(), 0.27059805007309856);
EXPECT_DOUBLE_EQ(msg.orientation().z(), 0.65328148243818829);
EXPECT_DOUBLE_EQ(msg.orientation().w(), 0.27059805007309851);
}
TEST_F(MsgsTest, ConvertMsgPoseToMath)
{
msgs::Pose msg = msgs::Convert(
ignition::math::Pose3d(ignition::math::Vector3d(1, 2, 3),
ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI)));
ignition::math::Pose3d v = msgs::ConvertIgn(msg);
EXPECT_DOUBLE_EQ(1, v.Pos().X());
EXPECT_DOUBLE_EQ(2, v.Pos().Y());
EXPECT_DOUBLE_EQ(3, v.Pos().Z());
EXPECT_DOUBLE_EQ(v.Rot().X(), -0.65328148243818818);
EXPECT_DOUBLE_EQ(v.Rot().Y(), 0.27059805007309856);
EXPECT_DOUBLE_EQ(v.Rot().Z(), 0.65328148243818829);
EXPECT_DOUBLE_EQ(v.Rot().W(), 0.27059805007309851);
}
TEST_F(MsgsTest, ConvertCommonColorToMsgs)
{
msgs::Color msg = msgs::Convert(common::Color(.1, .2, .3, 1.0));
EXPECT_FLOAT_EQ(0.1f, msg.r());
EXPECT_FLOAT_EQ(0.2f, msg.g());
EXPECT_FLOAT_EQ(0.3f, msg.b());
EXPECT_FLOAT_EQ(1.0f, msg.a());
}
TEST_F(MsgsTest, ConvertMsgsColorToCommon)
{
msgs::Color msg = msgs::Convert(common::Color(.1, .2, .3, 1.0));
common::Color v = msgs::Convert(msg);
EXPECT_FLOAT_EQ(0.1f, v.r);
EXPECT_FLOAT_EQ(0.2f, v.g);
EXPECT_FLOAT_EQ(0.3f, v.b);
EXPECT_FLOAT_EQ(1.0f, v.a);
}
TEST_F(MsgsTest, ConvertCommonTimeToMsgs)
{
msgs::Time msg = msgs::Convert(common::Time(2, 123));
EXPECT_EQ(2, msg.sec());
EXPECT_EQ(123, msg.nsec());
common::Time v = msgs::Convert(msg);
EXPECT_EQ(2, v.sec);
EXPECT_EQ(123, v.nsec);
}
TEST_F(MsgsTest, ConvertMathInertialToMsgs)
{
const auto pose = ignition::math::Pose3d(5, 6, 7, 0.4, 0.5, 0.6);
msgs::Inertial msg = msgs::Convert(
ignition::math::Inertiald(
ignition::math::MassMatrix3d(12.0,
ignition::math::Vector3d(2, 3, 4),
ignition::math::Vector3d(0.1, 0.2, 0.3)),
pose));
EXPECT_DOUBLE_EQ(12.0, msg.mass());
EXPECT_DOUBLE_EQ(2.0, msg.ixx());
EXPECT_DOUBLE_EQ(3.0, msg.iyy());
EXPECT_DOUBLE_EQ(4.0, msg.izz());
EXPECT_DOUBLE_EQ(0.1, msg.ixy());
EXPECT_DOUBLE_EQ(0.2, msg.ixz());
EXPECT_DOUBLE_EQ(0.3, msg.iyz());
EXPECT_EQ(pose, msgs::ConvertIgn(msg.pose()));
}
TEST_F(MsgsTest, ConvertMsgsInertialToMath)
{
const auto pose = ignition::math::Pose3d(5, 6, 7, 0.4, 0.5, 0.6);
msgs::Inertial msg = msgs::Convert(
ignition::math::Inertiald(
ignition::math::MassMatrix3d(12.0,
ignition::math::Vector3d(2, 3, 4),
ignition::math::Vector3d(0.1, 0.2, 0.3)),
pose));
auto inertial = msgs::Convert(msg);
EXPECT_DOUBLE_EQ(12.0, inertial.MassMatrix().Mass());
EXPECT_DOUBLE_EQ(2.0, inertial.MassMatrix().IXX());
EXPECT_DOUBLE_EQ(3.0, inertial.MassMatrix().IYY());
EXPECT_DOUBLE_EQ(4.0, inertial.MassMatrix().IZZ());
EXPECT_DOUBLE_EQ(0.1, inertial.MassMatrix().IXY());
EXPECT_DOUBLE_EQ(0.2, inertial.MassMatrix().IXZ());
EXPECT_DOUBLE_EQ(0.3, inertial.MassMatrix().IYZ());
EXPECT_EQ(pose, inertial.Pose());
}
TEST_F(MsgsTest, ConvertMathMassMatrix3ToMsgs)
{
msgs::Inertial msg = msgs::Convert(
ignition::math::MassMatrix3d(12.0,
ignition::math::Vector3d(2, 3, 4),
ignition::math::Vector3d(0.1, 0.2, 0.3)));
EXPECT_DOUBLE_EQ(12.0, msg.mass());
EXPECT_DOUBLE_EQ(2.0, msg.ixx());
EXPECT_DOUBLE_EQ(3.0, msg.iyy());
EXPECT_DOUBLE_EQ(4.0, msg.izz());
EXPECT_DOUBLE_EQ(0.1, msg.ixy());
EXPECT_DOUBLE_EQ(0.2, msg.ixz());
EXPECT_DOUBLE_EQ(0.3, msg.iyz());
}
TEST_F(MsgsTest, ConvertMathPlaneToMsgs)
{
msgs::PlaneGeom msg = msgs::Convert(
ignition::math::Planed(ignition::math::Vector3d(0, 0, 1),
ignition::math::Vector2d(123, 456), 1.0));
EXPECT_DOUBLE_EQ(0, msg.normal().x());
EXPECT_DOUBLE_EQ(0, msg.normal().y());
EXPECT_DOUBLE_EQ(1, msg.normal().z());
EXPECT_DOUBLE_EQ(123, msg.size().x());
EXPECT_DOUBLE_EQ(456, msg.size().y());
}
TEST_F(MsgsTest, ConvertMsgsPlaneToMath)
{
msgs::PlaneGeom msg = msgs::Convert(
ignition::math::Planed(ignition::math::Vector3d(0, 0, 1),
ignition::math::Vector2d(123, 456), 1.0));
ignition::math::Planed v = msgs::ConvertIgn(msg);
EXPECT_DOUBLE_EQ(0, v.Normal().X());
EXPECT_DOUBLE_EQ(0, v.Normal().Y());
EXPECT_DOUBLE_EQ(1, v.Normal().Z());
EXPECT_DOUBLE_EQ(123, v.Size().X());
EXPECT_DOUBLE_EQ(456, v.Size().Y());
EXPECT_DOUBLE_EQ(1.0, v.Offset());
}
//////////////////////////////////////////////////
void CompareMsgsShaderTypeToString(const msgs::Material::ShaderType _type)
{
EXPECT_EQ(_type, msgs::ConvertShaderType(msgs::ConvertShaderType(_type)));
}
//////////////////////////////////////////////////
TEST_F(MsgsTest, ConvertMsgsShaderTypeToString)
{
CompareMsgsShaderTypeToString(msgs::Material::NORMAL_MAP_OBJECT_SPACE);
CompareMsgsShaderTypeToString(msgs::Material::NORMAL_MAP_TANGENT_SPACE);
CompareMsgsShaderTypeToString(msgs::Material::PIXEL);
CompareMsgsShaderTypeToString(msgs::Material::VERTEX);
}
//////////////////////////////////////////////////
void CompareMsgsJointTypeToString(const msgs::Joint::Type _type)
{
EXPECT_EQ(_type, msgs::ConvertJointType(msgs::ConvertJointType(_type)));
}
//////////////////////////////////////////////////
TEST_F(MsgsTest, ConvertMsgsJointTypeToString)
{
CompareMsgsJointTypeToString(msgs::Joint::REVOLUTE);
CompareMsgsJointTypeToString(msgs::Joint::REVOLUTE2);
CompareMsgsJointTypeToString(msgs::Joint::PRISMATIC);
CompareMsgsJointTypeToString(msgs::Joint::UNIVERSAL);
CompareMsgsJointTypeToString(msgs::Joint::BALL);
CompareMsgsJointTypeToString(msgs::Joint::SCREW);
CompareMsgsJointTypeToString(msgs::Joint::GEARBOX);
CompareMsgsJointTypeToString(msgs::Joint::FIXED);
}
//////////////////////////////////////////////////
void CompareMsgsGeometryTypeToString(const msgs::Geometry::Type _type)
{
EXPECT_EQ(_type, msgs::ConvertGeometryType(msgs::ConvertGeometryType(_type)));
}
//////////////////////////////////////////////////
TEST_F(MsgsTest, ConvertMsgsGeometryTypeToString)
{
CompareMsgsGeometryTypeToString(msgs::Geometry::BOX);
CompareMsgsGeometryTypeToString(msgs::Geometry::SPHERE);
CompareMsgsGeometryTypeToString(msgs::Geometry::CYLINDER);
CompareMsgsGeometryTypeToString(msgs::Geometry::PLANE);
CompareMsgsGeometryTypeToString(msgs::Geometry::IMAGE);
CompareMsgsGeometryTypeToString(msgs::Geometry::HEIGHTMAP);
CompareMsgsGeometryTypeToString(msgs::Geometry::MESH);
CompareMsgsGeometryTypeToString(msgs::Geometry::POLYLINE);
EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::BOX), "box");
EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::SPHERE), "sphere");
EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::CYLINDER), "cylinder");
EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::PLANE), "plane");
EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::IMAGE), "image");
EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::HEIGHTMAP), "heightmap");
EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::MESH), "mesh");
EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::POLYLINE), "polyline");
EXPECT_EQ(msgs::ConvertGeometryType("box"), msgs::Geometry::BOX);
EXPECT_EQ(msgs::ConvertGeometryType("sphere"), msgs::Geometry::SPHERE);
EXPECT_EQ(msgs::ConvertGeometryType("cylinder"), msgs::Geometry::CYLINDER);
EXPECT_EQ(msgs::ConvertGeometryType("plane"), msgs::Geometry::PLANE);
EXPECT_EQ(msgs::ConvertGeometryType("image"), msgs::Geometry::IMAGE);
EXPECT_EQ(msgs::ConvertGeometryType("heightmap"), msgs::Geometry::HEIGHTMAP);
EXPECT_EQ(msgs::ConvertGeometryType("mesh"), msgs::Geometry::MESH);
EXPECT_EQ(msgs::ConvertGeometryType("polyline"), msgs::Geometry::POLYLINE);
}
TEST_F(MsgsTest, SetVector3)
{
msgs::Vector3d msg;
msgs::Set(&msg, ignition::math::Vector3d(1, 2, 3));
EXPECT_DOUBLE_EQ(1, msg.x());
EXPECT_DOUBLE_EQ(2, msg.y());
EXPECT_DOUBLE_EQ(3, msg.z());
}
TEST_F(MsgsTest, SetVector2d)
{
msgs::Vector2d msg;
msgs::Set(&msg, ignition::math::Vector2d(1, 2));
EXPECT_DOUBLE_EQ(1, msg.x());
EXPECT_DOUBLE_EQ(2, msg.y());
}
TEST_F(MsgsTest, SetQuaternion)
{
msgs::Quaternion msg;
msgs::Set(&msg, ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI));
EXPECT_DOUBLE_EQ(msg.x(), -0.65328148243818818);
EXPECT_DOUBLE_EQ(msg.y(), 0.27059805007309856);
EXPECT_DOUBLE_EQ(msg.z(), 0.65328148243818829);
EXPECT_DOUBLE_EQ(msg.w(), 0.27059805007309851);
}
TEST_F(MsgsTest, SetPose)
{
msgs::Pose msg;
msgs::Set(&msg, ignition::math::Pose3d(ignition::math::Vector3d(1, 2, 3),
ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI)));
EXPECT_DOUBLE_EQ(1, msg.position().x());
EXPECT_DOUBLE_EQ(2, msg.position().y());
EXPECT_DOUBLE_EQ(3, msg.position().z());
EXPECT_DOUBLE_EQ(msg.orientation().x(), -0.65328148243818818);
EXPECT_DOUBLE_EQ(msg.orientation().y(), 0.27059805007309856);
EXPECT_DOUBLE_EQ(msg.orientation().z(), 0.65328148243818829);
EXPECT_DOUBLE_EQ(msg.orientation().w(), 0.27059805007309851);
}
TEST_F(MsgsTest, SetColor)
{
msgs::Color msg;
msgs::Set(&msg, common::Color(.1, .2, .3, 1.0));
EXPECT_FLOAT_EQ(0.1f, msg.r());
EXPECT_FLOAT_EQ(0.2f, msg.g());
EXPECT_FLOAT_EQ(0.3f, msg.b());
EXPECT_FLOAT_EQ(1.0f, msg.a());
}
TEST_F(MsgsTest, SetTime)
{
msgs::Time msg;
msgs::Set(&msg, common::Time(2, 123));
EXPECT_EQ(2, msg.sec());
EXPECT_EQ(123, msg.nsec());
}
TEST_F(MsgsTest, SetInertial)
{
const auto pose = ignition::math::Pose3d(5, 6, 7, 0.4, 0.5, 0.6);
msgs::Inertial msg;
msgs::Set(&msg, ignition::math::Inertiald(
ignition::math::MassMatrix3d(
12.0,
ignition::math::Vector3d(2, 3, 4),
ignition::math::Vector3d(0.1, 0.2, 0.3)),
pose));
EXPECT_DOUBLE_EQ(12.0, msg.mass());
EXPECT_DOUBLE_EQ(2.0, msg.ixx());
EXPECT_DOUBLE_EQ(3.0, msg.iyy());
EXPECT_DOUBLE_EQ(4.0, msg.izz());
EXPECT_DOUBLE_EQ(0.1, msg.ixy());
EXPECT_DOUBLE_EQ(0.2, msg.ixz());
EXPECT_DOUBLE_EQ(0.3, msg.iyz());
EXPECT_EQ(pose, msgs::ConvertIgn(msg.pose()));
}
TEST_F(MsgsTest, SetMassMatrix3)
{
msgs::Inertial msg;
msgs::Set(&msg, ignition::math::MassMatrix3d(
12.0,
ignition::math::Vector3d(2, 3, 4),
ignition::math::Vector3d(0.1, 0.2, 0.3)));
EXPECT_DOUBLE_EQ(12.0, msg.mass());
EXPECT_DOUBLE_EQ(2.0, msg.ixx());
EXPECT_DOUBLE_EQ(3.0, msg.iyy());
EXPECT_DOUBLE_EQ(4.0, msg.izz());
EXPECT_DOUBLE_EQ(0.1, msg.ixy());
EXPECT_DOUBLE_EQ(0.2, msg.ixz());
EXPECT_DOUBLE_EQ(0.3, msg.iyz());
}
TEST_F(MsgsTest, SetPlane)
{
msgs::PlaneGeom msg;
msgs::Set(&msg, ignition::math::Planed(
ignition::math::Vector3d(0, 0, 1),
ignition::math::Vector2d(123, 456), 1.0));
EXPECT_DOUBLE_EQ(0, msg.normal().x());
EXPECT_DOUBLE_EQ(0, msg.normal().y());
EXPECT_DOUBLE_EQ(1, msg.normal().z());
EXPECT_DOUBLE_EQ(123, msg.size().x());
EXPECT_DOUBLE_EQ(456, msg.size().y());
EXPECT_DOUBLE_EQ(1.0, msg.d());
}
TEST_F(MsgsTest, Initialization)
{
{
msgs::Vector3d msg;
EXPECT_DOUBLE_EQ(0, msg.x());
EXPECT_DOUBLE_EQ(0, msg.y());
EXPECT_DOUBLE_EQ(0, msg.z());
}
{
msgs::Wrench msg;
EXPECT_DOUBLE_EQ(0, msg.force().x());
EXPECT_DOUBLE_EQ(0, msg.force().y());
EXPECT_DOUBLE_EQ(0, msg.force().z());
EXPECT_DOUBLE_EQ(0, msg.torque().x());
EXPECT_DOUBLE_EQ(0, msg.torque().y());
EXPECT_DOUBLE_EQ(0, msg.torque().z());
}
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, CameraSensorFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("sensor.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<sensor name='camera' type='camera'>\
<always_on>true</always_on>\
<update_rate>15</update_rate>\
<pose>1 2 3 0.1 0.2 0.3</pose>\
<visualize>true</visualize>\
<topic>/gazebo/test</topic>\
<camera>\
<horizontal_fov>0.123</horizontal_fov>\
<image>\
<width>320</width>\
<height>240</height>\
<format>R8G8B8</format>\
</image>\
<clip>\
<near>0.1</near>\
<far>10.5</far>\
</clip>\
<save enabled='true'>\
<path>/tmp</path>\
</save>\
<distortion>\
<k1>0.1</k1>\
<k2>0.2</k2>\
<k3>0.3</k3>\
<p1>0.4</p1>\
<p2>0.5</p2>\
<center>10 20</center>\
</distortion>\
</camera>\
</sensor>\
</sdf>", sdf));
msgs::Sensor msg = msgs::SensorFromSDF(sdf);
EXPECT_EQ(msg.name(), "camera");
EXPECT_EQ(msg.type(), "camera");
EXPECT_EQ(msg.topic(), "/gazebo/test");
EXPECT_TRUE(msg.always_on());
EXPECT_TRUE(msg.visualize());
EXPECT_NEAR(msg.update_rate(), 15.0, 1e-4);
EXPECT_EQ(msgs::ConvertIgn(msg.pose()),
ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
EXPECT_TRUE(msg.has_camera());
EXPECT_FALSE(msg.has_ray());
EXPECT_FALSE(msg.has_contact());
EXPECT_NEAR(msg.camera().horizontal_fov(), 0.123, 1e-4);
EXPECT_NEAR(msg.camera().image_size().x(), 320, 1e-4);
EXPECT_NEAR(msg.camera().image_size().y(), 240, 1e-4);
EXPECT_NEAR(msg.camera().near_clip(), 0.1, 1e-4);
EXPECT_NEAR(msg.camera().far_clip(), 10.5, 1e-4);
EXPECT_TRUE(msg.camera().save_enabled());
EXPECT_EQ(msg.camera().save_path(), "/tmp");
EXPECT_NEAR(msg.camera().distortion().k1(), 0.1, 1e-4);
EXPECT_NEAR(msg.camera().distortion().k2(), 0.2, 1e-4);
EXPECT_NEAR(msg.camera().distortion().k3(), 0.3, 1e-4);
EXPECT_NEAR(msg.camera().distortion().p1(), 0.4, 1e-4);
EXPECT_NEAR(msg.camera().distortion().p2(), 0.5, 1e-4);
EXPECT_NEAR(msg.camera().distortion().center().x(), 10, 1e-4);
EXPECT_NEAR(msg.camera().distortion().center().y(), 20, 1e-4);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, ContactSensorFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("sensor.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<sensor name='contact' type='contact'>\
<always_on>false</always_on>\
<update_rate>5</update_rate>\
<pose>1 2 3 0.1 0.2 0.3</pose>\
<visualize>false</visualize>\
<topic>/test</topic>\
<contact>\
<collision>my_collision</collision>\
</contact>\
</sensor>\
</sdf>", sdf));
msgs::Sensor msg = msgs::SensorFromSDF(sdf);
EXPECT_EQ(msg.name(), "contact");
EXPECT_EQ(msg.type(), "contact");
EXPECT_EQ(msg.topic(), "/test");
EXPECT_FALSE(msg.always_on());
EXPECT_FALSE(msg.visualize());
EXPECT_NEAR(msg.update_rate(), 5.0, 1e-4);
EXPECT_EQ(msgs::ConvertIgn(msg.pose()),
ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
EXPECT_FALSE(msg.has_camera());
EXPECT_FALSE(msg.has_ray());
EXPECT_TRUE(msg.has_contact());
EXPECT_EQ(msg.contact().collision_name(), "my_collision");
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, RaySensorFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("sensor.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<sensor name='ray' type='ray'>\
<always_on>false</always_on>\
<update_rate>5</update_rate>\
<pose>1 2 3 0.1 0.2 0.3</pose>\
<visualize>false</visualize>\
<topic>/test</topic>\
<ray>\
<scan>\
<horizontal>\
<samples>10</samples>\
<resolution>0.2</resolution>\
<min_angle>0</min_angle>\
<max_angle>1</max_angle>\
</horizontal>\
<vertical>\
<samples>20</samples>\
<resolution>0.4</resolution>\
<min_angle>-1</min_angle>\
<max_angle>0</max_angle>\
</vertical>\
</scan>\
<range>\
<min>0</min>\
<max>10</max>\
<resolution>0.1</resolution>\
</range>\
</ray>\
</sensor>\
</sdf>", sdf));
msgs::Sensor msg = msgs::SensorFromSDF(sdf);
EXPECT_EQ(msg.name(), "ray");
EXPECT_EQ(msg.type(), "ray");
EXPECT_EQ(msg.topic(), "/test");
EXPECT_FALSE(msg.always_on());
EXPECT_FALSE(msg.visualize());
EXPECT_NEAR(msg.update_rate(), 5.0, 1e-4);
EXPECT_EQ(msgs::ConvertIgn(msg.pose()),
ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
EXPECT_FALSE(msg.has_camera());
EXPECT_TRUE(msg.has_ray());
EXPECT_FALSE(msg.has_contact());
EXPECT_EQ(msg.ray().horizontal_samples(), 10);
EXPECT_NEAR(msg.ray().horizontal_resolution(), 0.2, 1e-4);
EXPECT_NEAR(msg.ray().horizontal_min_angle(), 0, 1e-4);
EXPECT_NEAR(msg.ray().horizontal_max_angle(), 1, 1e-4);
EXPECT_EQ(msg.ray().vertical_samples(), 20);
EXPECT_NEAR(msg.ray().vertical_resolution(), 0.4, 1e-4);
EXPECT_NEAR(msg.ray().vertical_min_angle(), -1, 1e-4);
EXPECT_NEAR(msg.ray().vertical_max_angle(), 0, 1e-4);
EXPECT_NEAR(msg.ray().range_min(), 0, 1e-4);
EXPECT_NEAR(msg.ray().range_max(), 10, 1e-4);
EXPECT_NEAR(msg.ray().range_resolution(), 0.1, 1e-4);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, GUIFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
<camera name='camera'>\
<view_controller>fps</view_controller>\
<pose>1 2 3 0 0 0</pose>\
<track_visual>\
<name>track</name>\
<min_dist>0.2</min_dist>\
<max_dist>1.0</max_dist>\
</track_visual>\
</camera>\
</gui>\
</sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
TEST_F(MsgsTest, GUIFromSDF_EmptyTrackVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
<camera name='camera'>\
<view_controller>fps</view_controller>\
<pose>1 2 3 0 0 0</pose>\
<track_visual>\
<name>visual_name</name>\
<min_dist>0.1</min_dist>\
<max_dist>1.0</max_dist>\
</track_visual>\
</camera>\
</gui>\
</sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
TEST_F(MsgsTest, GUIFromSDF_WithEmptyCamera)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
<camera name='camera'>\
</camera>\
</gui>\
</sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
TEST_F(MsgsTest, GUIFromSDF_WithoutCamera)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
</gui>\
</sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
TEST_F(MsgsTest, GUIFromSDF_WithPlugin)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
<plugin name='plugin_name' filename='plugin_filename'>\
</plugin>\
</gui>\
</sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
TEST_F(MsgsTest, PluginFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("plugin.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<plugin name='plugin_name' filename='plugin_filename'>\
<param1>1</param1>\
<param2>true</param2>\
</plugin>\
</sdf>", sdf));
msgs::Plugin msg = msgs::PluginFromSDF(sdf);
EXPECT_TRUE(msg.has_name());
EXPECT_EQ(msg.name(), "plugin_name");
EXPECT_TRUE(msg.has_filename());
EXPECT_EQ(msg.filename(), "plugin_filename");
EXPECT_TRUE(msg.has_innerxml());
EXPECT_EQ(msg.innerxml(), "<param1>1</param1>\n<param2>true</param2>\n");
}
TEST_F(MsgsTest, LightFromSDF_ListDirectional)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("light.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<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 0 0 1</specular>\
<attenuation>\
<range>20</range>\
<constant>0.8</constant>\
<linear>0.01</linear>\
<quadratic>0.0</quadratic>\
</attenuation>\
<direction>1.0 1.0 -1.0</direction>\
</light>\
</sdf>", sdf));
msgs::Light msg = msgs::LightFromSDF(sdf);
}
TEST_F(MsgsTest, LightFromSDF_LightSpot)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("light.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<light type='spot' name='lamp'>\
<pose>0 0 10 0 0 0</pose>\
<diffuse>0.8 0.8 0.8 1</diffuse>\
<specular>0 0 0 1</specular>\
<spot>\
<inner_angle>0</inner_angle>\
<outer_angle>1</outer_angle>\
<falloff>0.1</falloff>\
</spot>\
<attenuation>\
<range>20</range>\
<constant>0.8</constant>\
<linear>0.01</linear>\
<quadratic>0.0</quadratic>\
</attenuation>\
<direction>1.0 1.0 -1.0</direction>\
</light>\
</sdf>", sdf));
msgs::Light msg = msgs::LightFromSDF(sdf);
}
TEST_F(MsgsTest, LightFromSDF_LightPoint)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("light.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<light type='point' name='lamp'>\
<pose>0 0 10 0 0 0</pose>\
<diffuse>0.8 0.8 0.8 1</diffuse>\
<specular>0 0 0 1</specular>\
<attenuation>\
<range>20</range>\
<constant>0.8</constant>\
<linear>0.01</linear>\
<quadratic>0.0</quadratic>\
</attenuation>\
</light>\
</sdf>", sdf));
msgs::Light msg = msgs::LightFromSDF(sdf);
}
TEST_F(MsgsTest, LightFromSDF_LighBadType)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("light.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<light type='_bad_' name='lamp'>\
</light>\
</sdf>", sdf));
msgs::Light msg = msgs::LightFromSDF(sdf);
}
// Plane visual
TEST_F(MsgsTest, VisualFromSDF_PlaneVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
<geometry>\
<plane><normal>0 0 1</normal></plane>\
</geometry>\
<material><script>Gazebo/Grey</script></material>\
</visual>\
</sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
TEST_F(MsgsTest, VisualFromSDF_BoxVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
<geometry>\
<box><size>1 1 1</size></box>\
</geometry>\
<material><script>Gazebo/Grey'</script></material>\
</visual>\
</sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
TEST_F(MsgsTest, VisualFromSDF_SphereVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
<geometry>\
<sphere><radius>1</radius></sphere>\
</geometry>\
<material><script>Gazebo/Grey</script>\
<shader type='normal_map_tangent_space'>\
<normal_map>test.map</normal_map>\
</shader>\
</material>\
</visual>\
</sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
TEST_F(MsgsTest, VisualFromSDF_CylinderVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
<geometry>\
<cylinder><radius>1</radius><length>1.0</length></cylinder>\
</geometry>\
<material><script>Gazebo/Grey</script>\
<shader type='normal_map_object_space'/>\
</material>\
</visual>\
</sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
TEST_F(MsgsTest, VisualFromSDF_MeshVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
<geometry>\
<mesh><scale>1 1 1</scale><uri>test1.mesh</uri></mesh>\
</geometry>\
<material><script>Gazebo/Grey</script>\
<shader type='vertex'/>\
</material>\
</visual>\
</sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
TEST_F(MsgsTest, VisualFromSDF_ImageVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
<pose>1 1 1 1 2 3</pose>\
<geometry>\
<image>\
<scale>1</scale>\
<height>1</height>\
<threshold>255</threshold>\
<granularity>10</granularity>\
<uri>test2.mesh</uri>\
</image>\
</geometry>\
<material>\
<script>Gazebo/Grey</script>\
<shader type='pixel'/>\
<ambient>.1 .2 .3 1</ambient>\
<diffuse>.1 .2 .3 1</diffuse>\
<specular>.1 .2 .3 1</specular>\
<emissive>.1 .2 .3 1</emissive>\
</material>\
</visual>\
</sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
TEST_F(MsgsTest, VisualFromSDF_HeigthmapVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
<pose>1 1 1 1 2 3</pose>\
<geometry>\
<heightmap>\
<size>1 2 3</size>\
<uri>test3.mesh</uri>\
<pos>0 0 1</pos>\
</heightmap>\
</geometry>\
<material><script>Gazebo/Grey</script>\
<shader type='pixel'/>\
</material>\
</visual>\
</sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
TEST_F(MsgsTest, VisualFromSDF_NoGeometry)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
</visual>\
</sdf>", sdf));
EXPECT_THROW(msgs::Visual msg = msgs::VisualFromSDF(sdf),
common::Exception);
}
TEST_F(MsgsTest, VisualFromSDF_ShaderTypeThrow)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<pose>1 1 1 1 2 3</pose>\
<geometry>\
<heightmap>\
<size>1 2 3</size>\
<uri>test4.mesh</uri>\
<pos>0 0 0</pos>\
</heightmap>\
</geometry>\
<material><script>Gazebo/Grey</script>\
<shader type='throw'/>\
</material>\
</visual>\
</sdf>", sdf));
EXPECT_THROW(msgs::Visual msg = msgs::VisualFromSDF(sdf),
common::Exception);
}
TEST_F(MsgsTest, VisualFromSDF_BadGeometryVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<pose>1 1 1 1 2 3</pose>\
<geometry>\
</geometry>\
<material><script>Gazebo/Grey</script>\
<shader type='pixel'/>\
</material>\
</visual>\
</sdf>", sdf));
EXPECT_THROW(msgs::Visual msg = msgs::VisualFromSDF(sdf),
common::Exception);
}
TEST_F(MsgsTest, VisualFromSDF_BadGeometryType)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
// As of sdformat pull request 148 (released in version 2.3.1),
// unknown elements are now ignored with a warning message
// rather than causing an error.
EXPECT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<pose>1 1 1 1 2 3</pose>\
<geometry>\
<bad_type/>\
</geometry>\
<material><script>Gazebo/Grey</script>\
<shader type='pixel'/>\
</material>\
</visual>\
</sdf>", sdf));
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
</visual>\
</sdf>", sdf));
sdf::ElementPtr badElement(new sdf::Element());
badElement->SetName("bad_type");
sdf->GetElement("geometry")->InsertElement(badElement);
EXPECT_THROW(msgs::Visual msg = msgs::VisualFromSDF(sdf),
common::Exception);
}
TEST_F(MsgsTest, VisualFromSDF_BadFogType)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<ambient>0.1 0.1 0.1 1</ambient>\
<background>0 0 0 1</background>\
<shadows>true</shadows>\
<fog>\
<color>1 1 1 1</color>\
<type>throw</type>\
<start>0</start>\
<end>10</end>\
<density>1</density>\
</fog>\
<grid>false</grid>\
</scene>\
</sdf>", sdf));
EXPECT_THROW(msgs::Scene msg = msgs::SceneFromSDF(sdf), common::Exception);
}
TEST_F(MsgsTest, VisualSceneFromSDF_A)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<ambient>0.1 0.1 0.1 1</ambient>\
<background>0 0 0 1</background>\
<shadows>true</shadows>\
<fog>\
<color>1 1 1 1</color>\
<type>linear</type>\
<start>0</start>\
<end>10</end>\
<density>1</density>\
</fog>\
<grid>false</grid>\
</scene>\
</sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
TEST_F(MsgsTest, VisualSceneFromSDF_B)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<ambient>0.1 0.1 0.1 1</ambient>\
<background>0 0 0 1</background>\
<shadows>false</shadows>\
<fog>\
<color>1 1 1 1</color>\
<type>exp</type>\
<start>0</start>\
<end>10</end>\
<density>1</density>\
</fog>\
</scene>\
</sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
TEST_F(MsgsTest, VisualSceneFromSDF_C)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<ambient>0.1 0.1 0.1 1</ambient>\
<background>0 0 0 1</background>\
<shadows>false</shadows>\
<fog>\
<color>1 1 1 1</color>\
<type>exp2</type>\
<start>0</start>\
<end>10</end>\
<density>1</density>\
</fog>\
<grid>true</grid>\
</scene>\
</sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
TEST_F(MsgsTest, VisualSceneFromSDF_CEmpty)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
</scene>\
</sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
TEST_F(MsgsTest, VisualSceneFromSDF_CEmptyNoSky)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<background>0 0 0 1</background>\
</scene>\
</sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, MeshFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("geometry.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<geometry>\
<mesh>\
<uri>test/mesh.dae</uri>\
<scale>1 2 3</scale>\
<submesh>\
<name>test_name</name>\
<center>true</center>\
</submesh>\
</mesh>\
</geometry>\
</sdf>", sdf));
msgs::MeshGeom msg = msgs::MeshFromSDF(sdf->GetElement("mesh"));
EXPECT_TRUE(msg.has_filename());
EXPECT_STREQ("test/mesh.dae", msg.filename().c_str());
EXPECT_TRUE(msg.has_scale());
EXPECT_DOUBLE_EQ(msg.scale().x(), 1.0);
EXPECT_DOUBLE_EQ(msg.scale().y(), 2.0);
EXPECT_DOUBLE_EQ(msg.scale().z(), 3.0);
EXPECT_TRUE(msg.has_submesh());
EXPECT_STREQ("test_name", msg.submesh().c_str());
EXPECT_TRUE(msg.has_center_submesh());
EXPECT_TRUE(msg.center_submesh());
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, CollisionFromSDF_Friction)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("collision.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<collision name='collision_name'>\
<geometry>\
<cylinder>\
<radius>1.23</radius>\
<length>4.56</length>\
</cylinder>\
</geometry>\
<surface>\
<friction>\
<ode>\
<mu>0.1</mu>\
<mu2>0.2</mu2>\
<fdir1>0.3 0.4 0.5</fdir1>\
<slip1>0.6</slip1>\
<slip2>0.7</slip2>\
</ode>\
<torsional>\
<coefficient>0.8</coefficient>\
<patch_radius>0.9</patch_radius>\
<surface_radius>1.0</surface_radius>\
<use_patch_radius>0</use_patch_radius>\
<ode>\
<slip>1.1</slip>\
</ode>\
</torsional>\
</friction>\
</surface>\
</collision>\
</sdf>", sdf));
msgs::Collision msg = msgs::CollisionFromSDF(sdf);
// Translational friction
EXPECT_TRUE(msg.has_surface());
EXPECT_TRUE(msg.surface().has_friction());
msgs::Friction friction = msg.surface().friction();
EXPECT_TRUE(friction.has_mu());
EXPECT_DOUBLE_EQ(0.1, friction.mu());
EXPECT_TRUE(friction.has_mu2());
EXPECT_DOUBLE_EQ(0.2, friction.mu2());
EXPECT_TRUE(friction.has_fdir1());
EXPECT_EQ(ignition::math::Vector3d(0.3, 0.4, 0.5),
msgs::ConvertIgn(friction.fdir1()));
EXPECT_TRUE(friction.has_slip1());
EXPECT_DOUBLE_EQ(0.6, friction.slip1());
EXPECT_TRUE(friction.has_slip2());
EXPECT_DOUBLE_EQ(0.7, friction.slip2());
// Torsional friction
EXPECT_TRUE(friction.has_torsional());
msgs::Friction::Torsional torsional = friction.torsional();
EXPECT_TRUE(torsional.has_coefficient());
EXPECT_DOUBLE_EQ(0.8, torsional.coefficient());
EXPECT_TRUE(torsional.has_patch_radius());
EXPECT_DOUBLE_EQ(0.9, torsional.patch_radius());
EXPECT_TRUE(torsional.has_surface_radius());
EXPECT_DOUBLE_EQ(1.0, torsional.surface_radius());
EXPECT_TRUE(torsional.has_patch_radius());
EXPECT_FALSE(torsional.use_patch_radius());
EXPECT_TRUE(torsional.has_ode());
EXPECT_TRUE(torsional.ode().has_slip());
EXPECT_DOUBLE_EQ(1.1, torsional.ode().slip());
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, CollisionFromSDF_Contact)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("collision.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<collision name='collision_name'>\
<geometry>\
<cylinder>\
<radius>1.23</radius>\
<length>4.56</length>\
</cylinder>\
</geometry>\
<laser_retro>0.7</laser_retro>\
<max_contacts>8</max_contacts>\
<surface>\
<bounce>\
<restitution_coefficient>0.9</restitution_coefficient>\
<threshold>1.0</threshold>\
</bounce>\
<contact>\
<collide_without_contact>1</collide_without_contact>\
<collide_without_contact_bitmask>\
2\
</collide_without_contact_bitmask>\
<collide_bitmask>3</collide_bitmask>\
<ode>\
<soft_cfm>0.4</soft_cfm>\
<soft_erp>0.5</soft_erp>\
<kp>0.6</kp>\
<kd>0.7</kd>\
<max_vel>0.8</max_vel>\
<min_depth>0.9</min_depth>\
</ode>\
</contact>\
</surface>\
</collision>\
</sdf>", sdf));
msgs::Collision msg = msgs::CollisionFromSDF(sdf);
EXPECT_TRUE(msg.has_surface());
msgs::Surface surface = msg.surface();
EXPECT_TRUE(surface.has_restitution_coefficient());
EXPECT_DOUBLE_EQ(0.9, surface.restitution_coefficient());
EXPECT_TRUE(surface.has_bounce_threshold());
EXPECT_DOUBLE_EQ(1.0, surface.bounce_threshold());
EXPECT_TRUE(surface.has_soft_cfm());
EXPECT_DOUBLE_EQ(0.4, surface.soft_cfm());
EXPECT_TRUE(surface.has_soft_erp());
EXPECT_DOUBLE_EQ(0.5, surface.soft_erp());
EXPECT_TRUE(surface.has_kp());
EXPECT_DOUBLE_EQ(0.6, surface.kp());
EXPECT_TRUE(surface.has_kd());
EXPECT_DOUBLE_EQ(0.7, surface.kd());
EXPECT_TRUE(surface.has_max_vel());
EXPECT_DOUBLE_EQ(0.8, surface.max_vel());
EXPECT_TRUE(surface.has_min_depth());
EXPECT_DOUBLE_EQ(0.9, surface.min_depth());
EXPECT_TRUE(surface.has_collide_without_contact());
EXPECT_TRUE(surface.collide_without_contact());
EXPECT_TRUE(surface.has_collide_without_contact_bitmask());
EXPECT_EQ(2u, surface.collide_without_contact_bitmask());
EXPECT_TRUE(surface.has_collide_bitmask());
EXPECT_EQ(3u, surface.collide_bitmask());
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, SurfaceToFromSDF)
{
// Surface
double restitution_coefficient = 0.1;
double threshold = 0.2;
// Friction
double mu = 0.1;
double mu2 = 0.2;
ignition::math::Vector3d fdir1(-0.3, 0.4, -0.5);
double slip1 = 0.6;
double slip2 = 0.7;
double mu3 = 0.8;
double patch_radius = 0.9;
double surface_radius = 1.0;
bool use_patch_radius = false;
double slip3 = 1.1;
// Contact
bool collide_without_contact = true;
int32_t collide_without_contact_bitmask = 123;
int32_t collide_bitmask = 456;
double soft_cfm = 0.1;
double soft_erp = 0.2;
double kp = 0.3;
double kd = 0.4;
double max_vel = 0.5;
double min_depth = 0.6;
// Create message
msgs::Surface surfaceMsg1;
surfaceMsg1.set_restitution_coefficient(restitution_coefficient);
surfaceMsg1.set_bounce_threshold(threshold);
surfaceMsg1.set_soft_cfm(soft_cfm);
surfaceMsg1.set_soft_erp(soft_erp);
surfaceMsg1.set_kp(kp);
surfaceMsg1.set_kd(kd);
surfaceMsg1.set_max_vel(max_vel);
surfaceMsg1.set_min_depth(min_depth);
surfaceMsg1.set_collide_without_contact(collide_without_contact);
surfaceMsg1.set_collide_without_contact_bitmask(
collide_without_contact_bitmask);
surfaceMsg1.set_collide_bitmask(collide_bitmask);
msgs::Friction frictionMsg1;
frictionMsg1.set_mu(mu);
frictionMsg1.set_mu2(mu2);
msgs::Set(frictionMsg1.mutable_fdir1(), fdir1);
frictionMsg1.set_slip1(slip1);
frictionMsg1.set_slip2(slip2);
msgs::Friction::Torsional torsionalMsg1;
torsionalMsg1.set_coefficient(mu3);
torsionalMsg1.set_patch_radius(patch_radius);
torsionalMsg1.set_surface_radius(surface_radius);
torsionalMsg1.set_use_patch_radius(use_patch_radius);
msgs::Friction::Torsional::ODE torsionalOdeMsg1;
torsionalOdeMsg1.set_slip(slip3);
auto torsionalODE = torsionalMsg1.mutable_ode();
torsionalODE->CopyFrom(torsionalOdeMsg1);
auto torsional = frictionMsg1.mutable_torsional();
torsional->CopyFrom(torsionalMsg1);
auto friction = surfaceMsg1.mutable_friction();
friction->CopyFrom(frictionMsg1);
// To SDF
sdf::ElementPtr sdf1 = msgs::SurfaceToSDF(surfaceMsg1);
// Back to Msg
msgs::Surface surfaceMsg2 = msgs::SurfaceFromSDF(sdf1);
EXPECT_DOUBLE_EQ(surfaceMsg2.restitution_coefficient(),
restitution_coefficient);
EXPECT_DOUBLE_EQ(surfaceMsg2.bounce_threshold(), threshold);
EXPECT_DOUBLE_EQ(surfaceMsg2.soft_cfm(), soft_cfm);
EXPECT_DOUBLE_EQ(surfaceMsg2.soft_erp(), soft_erp);
EXPECT_DOUBLE_EQ(surfaceMsg2.kp(), kp);
EXPECT_DOUBLE_EQ(surfaceMsg2.kd(), kd);
EXPECT_DOUBLE_EQ(surfaceMsg2.max_vel(), max_vel);
EXPECT_DOUBLE_EQ(surfaceMsg2.min_depth(), min_depth);
EXPECT_EQ(surfaceMsg2.collide_without_contact(), collide_without_contact);
EXPECT_EQ(surfaceMsg2.collide_without_contact_bitmask(),
static_cast<unsigned int>(collide_without_contact_bitmask));
EXPECT_EQ(surfaceMsg2.collide_bitmask(),
static_cast<unsigned int>(collide_bitmask));
ASSERT_TRUE(surfaceMsg2.has_friction());
msgs::Friction frictionMsg2 = surfaceMsg2.friction();
EXPECT_DOUBLE_EQ(frictionMsg2.mu(), mu);
EXPECT_DOUBLE_EQ(frictionMsg2.mu2(), mu2);
EXPECT_EQ(msgs::ConvertIgn(frictionMsg2.fdir1()), fdir1);
EXPECT_DOUBLE_EQ(frictionMsg2.slip1(), slip1);
EXPECT_DOUBLE_EQ(frictionMsg2.slip2(), slip2);
ASSERT_TRUE(frictionMsg2.has_torsional());
EXPECT_DOUBLE_EQ(frictionMsg2.torsional().coefficient(), mu3);
EXPECT_DOUBLE_EQ(frictionMsg2.torsional().patch_radius(), patch_radius);
EXPECT_DOUBLE_EQ(frictionMsg2.torsional().surface_radius(), surface_radius);
EXPECT_EQ(frictionMsg2.torsional().use_patch_radius(), use_patch_radius);
ASSERT_TRUE(frictionMsg2.torsional().has_ode());
EXPECT_DOUBLE_EQ(frictionMsg2.torsional().ode().slip(), slip3);
// Back to SDF
sdf::ElementPtr sdf2;
sdf2.reset(new sdf::Element);
sdf::initFile("surface.sdf", sdf2);
msgs::SurfaceToSDF(surfaceMsg2, sdf2);
ASSERT_TRUE(sdf2 != nullptr);
ASSERT_TRUE(sdf2->HasElement("bounce"));
EXPECT_DOUBLE_EQ(
sdf2->GetElement("bounce")->Get<double>("restitution_coefficient"),
restitution_coefficient);
EXPECT_DOUBLE_EQ(
sdf2->GetElement("bounce")->Get<double>("threshold"), threshold);
ASSERT_TRUE(sdf2->HasElement("contact"));
sdf::ElementPtr contactSdf2 = sdf2->GetElement("contact");
EXPECT_EQ(contactSdf2->Get<bool>("collide_without_contact"),
collide_without_contact);
EXPECT_EQ(contactSdf2->Get<int>("collide_without_contact_bitmask"),
collide_without_contact_bitmask);
EXPECT_EQ(contactSdf2->Get<int>("collide_bitmask"), collide_bitmask);
ASSERT_TRUE(sdf2->HasElement("friction"));
ASSERT_TRUE(sdf2->GetElement("friction")->HasElement("ode"));
sdf::ElementPtr odeSdf2 =
sdf2->GetElement("friction")->GetElement("ode");
EXPECT_DOUBLE_EQ(odeSdf2->Get<double>("mu"), mu);
EXPECT_DOUBLE_EQ(odeSdf2->Get<double>("mu2"), mu2);
EXPECT_EQ(odeSdf2->Get<ignition::math::Vector3d>("fdir1"), fdir1);
EXPECT_DOUBLE_EQ(odeSdf2->Get<double>("slip1"), slip1);
EXPECT_DOUBLE_EQ(odeSdf2->Get<double>("slip2"), slip2);
ASSERT_TRUE(sdf2->GetElement("friction")->HasElement("torsional"));
sdf::ElementPtr torsionalSdf2 =
sdf2->GetElement("friction")->GetElement("torsional");
EXPECT_DOUBLE_EQ(torsionalSdf2->Get<double>("coefficient"), mu3);
EXPECT_DOUBLE_EQ(torsionalSdf2->Get<double>("patch_radius"), patch_radius);
EXPECT_DOUBLE_EQ(torsionalSdf2->Get<double>("surface_radius"),
surface_radius);
EXPECT_EQ(torsionalSdf2->Get<bool>("use_patch_radius"),
use_patch_radius);
ASSERT_TRUE(torsionalSdf2->HasElement("ode"));
EXPECT_DOUBLE_EQ(torsionalSdf2->GetElement("ode")->Get<double>("slip"),
slip3);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, AxisFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("joint.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<joint name='arm' type='revolute'>\
<parent>arm_base</parent>\
<child>arm_shoulder</child>\
<axis>\
<xyz>0 0 1</xyz>\
<limit>\
<lower>0.01</lower>\
<upper>9</upper>\
<effort>2.2</effort>\
<velocity>0.1</velocity>\
</limit>\
<use_parent_model_frame>false</use_parent_model_frame>\
<dynamics>\
<damping>0.1</damping>\
<friction>0.2</friction>\
</dynamics>\
</axis>\
</joint>\
</sdf>", sdf));
msgs::Axis msg = msgs::AxisFromSDF(sdf->GetElement("axis"));
EXPECT_TRUE(msg.has_xyz());
EXPECT_EQ(msgs::ConvertIgn(msg.xyz()), ignition::math::Vector3d(0, 0, 1));
EXPECT_TRUE(msg.has_limit_lower());
EXPECT_NEAR(msg.limit_lower(), 0.01, 1e-6);
EXPECT_TRUE(msg.has_limit_upper());
EXPECT_NEAR(msg.limit_upper(), 9, 1e-6);
EXPECT_TRUE(msg.has_limit_effort());
EXPECT_NEAR(msg.limit_effort(), 2.2, 1e-6);
EXPECT_TRUE(msg.has_limit_velocity());
EXPECT_NEAR(msg.limit_velocity(), 0.1, 1e-6);
EXPECT_TRUE(msg.has_use_parent_model_frame());
EXPECT_EQ(msg.use_parent_model_frame(), false);
EXPECT_TRUE(msg.has_damping());
EXPECT_NEAR(msg.damping(), 0.1, 1e-6);
EXPECT_TRUE(msg.has_friction());
EXPECT_NEAR(msg.friction(), 0.2, 1e-6);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, JointFromSDF)
{
// revolute
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("joint.sdf", sdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<joint name='arm' type='revolute'>\
<pose>1 2 3 0 1.57 0</pose>\
<parent>arm_base</parent>\
<child>arm_shoulder</child>\
<axis>\
<xyz>1 0 0</xyz>\
<limit>\
<lower>0.1</lower>\
<upper>3.14</upper>\
<effort>2.4</effort>\
<velocity>0.4</velocity>\
</limit>\
<use_parent_model_frame>true</use_parent_model_frame>\
<dynamics>\
<damping>1.0</damping>\
<friction>0.1</friction>\
</dynamics>\
</axis>\
<physics>\
<ode>\
<cfm>0.2</cfm>\
<bounce>0.1</bounce>\
<velocity>1.1</velocity>\
<fudge_factor>0.4</fudge_factor>\
<limit>\
<cfm>0.0</cfm>\
<erp>0.9</erp>\
</limit>\
<suspension>\
<cfm>0.1</cfm>\
<erp>0.3</erp>\
</suspension>\
</ode>\
</physics>\
</joint>\
</sdf>", sdf));
msgs::Joint msg = msgs::JointFromSDF(sdf);
EXPECT_TRUE(msg.has_name());
EXPECT_EQ(msg.name(), "arm");
EXPECT_TRUE(msg.has_type());
EXPECT_EQ(msgs::ConvertJointType(msg.type()), "revolute");
EXPECT_TRUE(msg.has_pose());
EXPECT_EQ(msgs::ConvertIgn(msg.pose()),
ignition::math::Pose3d(1, 2, 3, 0, 1.57, 0));
EXPECT_TRUE(msg.has_parent());
EXPECT_EQ(msg.parent(), "arm_base");
EXPECT_TRUE(msg.has_child());
EXPECT_EQ(msg.child(), "arm_shoulder");
EXPECT_TRUE(msg.has_cfm());
EXPECT_NEAR(msg.cfm(), 0.2, 1e-6);
EXPECT_TRUE(msg.has_bounce());
EXPECT_NEAR(msg.bounce(), 0.1, 1e-6);
EXPECT_TRUE(msg.has_velocity());
EXPECT_NEAR(msg.velocity(), 1.1, 1e-6);
EXPECT_TRUE(msg.has_fudge_factor());
EXPECT_NEAR(msg.fudge_factor(), 0.4, 1e-6);
EXPECT_TRUE(msg.has_limit_cfm());
EXPECT_NEAR(msg.limit_cfm(), 0.0, 1e-6);
EXPECT_TRUE(msg.has_limit_erp());
EXPECT_NEAR(msg.limit_erp(), 0.9, 1e-6);
EXPECT_TRUE(msg.has_suspension_cfm());
EXPECT_NEAR(msg.suspension_cfm(), 0.1, 1e-6);
EXPECT_TRUE(msg.has_suspension_erp());
EXPECT_NEAR(msg.suspension_erp(), 0.3, 1e-6);
EXPECT_TRUE(msg.has_axis1());
EXPECT_TRUE(!msg.has_axis2());
const msgs::Axis axisMsg = msg.axis1();
EXPECT_TRUE(axisMsg.has_xyz());
EXPECT_EQ(msgs::ConvertIgn(axisMsg.xyz()), ignition::math::Vector3d(1, 0, 0));
EXPECT_TRUE(axisMsg.has_limit_lower());
EXPECT_NEAR(axisMsg.limit_lower(), 0.1, 1e-6);
EXPECT_TRUE(axisMsg.has_limit_upper());
EXPECT_NEAR(axisMsg.limit_upper(), 3.14, 1e-6);
EXPECT_TRUE(axisMsg.has_limit_effort());
EXPECT_NEAR(axisMsg.limit_effort(), 2.4, 1e-6);
EXPECT_TRUE(axisMsg.has_limit_velocity());
EXPECT_NEAR(axisMsg.limit_velocity(), 0.4, 1e-6);
EXPECT_TRUE(axisMsg.has_use_parent_model_frame());
EXPECT_EQ(axisMsg.use_parent_model_frame(), true);
EXPECT_TRUE(axisMsg.has_damping());
EXPECT_NEAR(axisMsg.damping(), 1.0, 1e-6);
EXPECT_TRUE(axisMsg.has_friction());
EXPECT_NEAR(axisMsg.friction(), 0.1, 1e-6);
// gearbox
sdf::ElementPtr gearboxSdf(new sdf::Element());
sdf::initFile("joint.sdf", gearboxSdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<joint name='axle_wheel' type='gearbox'>\
<pose>5 2 1 1.57 0 0</pose>\
<parent>axle</parent>\
<child>wheel</child>\
<axis>\
<xyz>0 0 1</xyz>\
<limit>\
<lower>0.01</lower>\
<upper>3.0</upper>\
<effort>2.1</effort>\
<velocity>0.2</velocity>\
</limit>\
<use_parent_model_frame>true</use_parent_model_frame>\
<dynamics>\
<damping>0.8</damping>\
<friction>0.1</friction>\
</dynamics>\
</axis>\
<axis2>\
<xyz>0 0 1</xyz>\
<limit>\
<lower>0.02</lower>\
<upper>3.01</upper>\
<effort>2.3</effort>\
<velocity>0.1</velocity>\
</limit>\
<use_parent_model_frame>true</use_parent_model_frame>\
<dynamics>\
<damping>0.9</damping>\
<friction>0.1</friction>\
</dynamics>\
</axis2>\
<gearbox_reference_body>chassis</gearbox_reference_body>\
<gearbox_ratio>0.2</gearbox_ratio>\
<physics>\
<ode>\
<cfm>0.3</cfm>\
<bounce>0.12</bounce>\
<velocity>1.12</velocity>\
<fudge_factor>0.4</fudge_factor>\
<limit>\
<cfm>0.1</cfm>\
<erp>0.9</erp>\
</limit>\
<suspension>\
<cfm>0.2</cfm>\
<erp>0.4</erp>\
</suspension>\
</ode>\
</physics>\
</joint>\
</sdf>", gearboxSdf));
msgs::Joint gearboxMsg = msgs::JointFromSDF(gearboxSdf);
EXPECT_TRUE(gearboxMsg.has_name());
EXPECT_EQ(gearboxMsg.name(), "axle_wheel");
EXPECT_TRUE(gearboxMsg.has_type());
EXPECT_EQ(msgs::ConvertJointType(gearboxMsg.type()), "gearbox");
EXPECT_TRUE(gearboxMsg.has_pose());
EXPECT_EQ(msgs::ConvertIgn(gearboxMsg.pose()),
ignition::math::Pose3d(5, 2, 1, 1.57, 0, 0));
EXPECT_TRUE(gearboxMsg.has_parent());
EXPECT_EQ(gearboxMsg.parent(), "axle");
EXPECT_TRUE(gearboxMsg.has_child());
EXPECT_EQ(gearboxMsg.child(), "wheel");
EXPECT_TRUE(gearboxMsg.has_cfm());
EXPECT_NEAR(gearboxMsg.cfm(), 0.3, 1e-6);
EXPECT_TRUE(gearboxMsg.has_bounce());
EXPECT_NEAR(gearboxMsg.bounce(), 0.12, 1e-6);
EXPECT_TRUE(gearboxMsg.has_velocity());
EXPECT_NEAR(gearboxMsg.velocity(), 1.12, 1e-6);
EXPECT_TRUE(gearboxMsg.has_fudge_factor());
EXPECT_NEAR(gearboxMsg.fudge_factor(), 0.4, 1e-6);
EXPECT_TRUE(gearboxMsg.has_limit_cfm());
EXPECT_NEAR(gearboxMsg.limit_cfm(), 0.1, 1e-6);
EXPECT_TRUE(gearboxMsg.has_limit_erp());
EXPECT_NEAR(gearboxMsg.limit_erp(), 0.9, 1e-6);
EXPECT_TRUE(gearboxMsg.has_suspension_cfm());
EXPECT_NEAR(gearboxMsg.suspension_cfm(), 0.2, 1e-6);
EXPECT_TRUE(gearboxMsg.has_suspension_erp());
EXPECT_NEAR(gearboxMsg.suspension_erp(), 0.4, 1e-6);
EXPECT_TRUE(gearboxMsg.has_axis1());
const msgs::Axis axisGearboxMsg = gearboxMsg.axis1();
EXPECT_TRUE(axisGearboxMsg.has_xyz());
EXPECT_EQ(msgs::ConvertIgn(axisGearboxMsg.xyz()),
ignition::math::Vector3d(0, 0, 1));
EXPECT_TRUE(axisGearboxMsg.has_limit_lower());
EXPECT_NEAR(axisGearboxMsg.limit_lower(), 0.01, 1e-6);
EXPECT_TRUE(axisGearboxMsg.has_limit_upper());
EXPECT_NEAR(axisGearboxMsg.limit_upper(), 3.0, 1e-6);
EXPECT_TRUE(axisGearboxMsg.has_limit_effort());
EXPECT_NEAR(axisGearboxMsg.limit_effort(), 2.1, 1e-6);
EXPECT_TRUE(axisGearboxMsg.has_limit_velocity());
EXPECT_NEAR(axisGearboxMsg.limit_velocity(), 0.2, 1e-6);
EXPECT_TRUE(axisGearboxMsg.has_use_parent_model_frame());
EXPECT_EQ(axisGearboxMsg.use_parent_model_frame(), true);
EXPECT_TRUE(axisGearboxMsg.has_damping());
EXPECT_NEAR(axisGearboxMsg.damping(), 0.8, 1e-6);
EXPECT_TRUE(axisGearboxMsg.has_friction());
EXPECT_NEAR(axisGearboxMsg.friction(), 0.1, 1e-6);
EXPECT_TRUE(gearboxMsg.has_axis2());
const msgs::Axis axisGearboxMsg2 = gearboxMsg.axis2();
EXPECT_TRUE(axisGearboxMsg2.has_xyz());
EXPECT_EQ(msgs::ConvertIgn(axisGearboxMsg2.xyz()),
ignition::math::Vector3d(0, 0, 1));
EXPECT_TRUE(axisGearboxMsg2.has_limit_lower());
EXPECT_NEAR(axisGearboxMsg2.limit_lower(), 0.02, 1e-6);
EXPECT_TRUE(axisGearboxMsg2.has_limit_upper());
EXPECT_NEAR(axisGearboxMsg2.limit_upper(), 3.01, 1e-6);
EXPECT_TRUE(axisGearboxMsg2.has_limit_effort());
EXPECT_NEAR(axisGearboxMsg2.limit_effort(), 2.3, 1e-6);
EXPECT_TRUE(axisGearboxMsg2.has_limit_velocity());
EXPECT_NEAR(axisGearboxMsg2.limit_velocity(), 0.1, 1e-6);
EXPECT_TRUE(axisGearboxMsg2.has_use_parent_model_frame());
EXPECT_EQ(axisGearboxMsg2.use_parent_model_frame(), true);
EXPECT_TRUE(axisGearboxMsg2.has_damping());
EXPECT_NEAR(axisGearboxMsg2.damping(), 0.9, 1e-6);
EXPECT_TRUE(axisGearboxMsg2.has_friction());
EXPECT_NEAR(axisGearboxMsg2.friction(), 0.1, 1e-6);
EXPECT_TRUE(gearboxMsg.has_gearbox());
EXPECT_EQ(gearboxMsg.mutable_gearbox()->gearbox_reference_body(), "chassis");
EXPECT_NEAR(gearboxMsg.mutable_gearbox()->gearbox_ratio(), 0.2, 1e-6);
// screw
sdf::ElementPtr screwSdf(new sdf::Element());
sdf::initFile("joint.sdf", screwSdf);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<joint name='box_cylinder' type='screw'>\
<pose>1 1 0 0 0 1.57</pose>\
<parent>box</parent>\
<child>cylinder</child>\
<axis>\
<xyz>0 1 0</xyz>\
<limit>\
<lower>0.0</lower>\
<upper>2.0</upper>\
<effort>1.21</effort>\
<velocity>0.12</velocity>\
</limit>\
<use_parent_model_frame>true</use_parent_model_frame>\
<dynamics>\
<damping>0.5</damping>\
<friction>0.12</friction>\
</dynamics>\
</axis>\
<thread_pitch>0.2</thread_pitch>\
<physics>\
<ode>\
<cfm>0.13</cfm>\
<bounce>0.02</bounce>\
<velocity>1.1</velocity>\
<fudge_factor>0.42</fudge_factor>\
<limit>\
<cfm>0.11</cfm>\
<erp>0.91</erp>\
</limit>\
<suspension>\
<cfm>0.22</cfm>\
<erp>0.42</erp>\
</suspension>\
</ode>\
</physics>\
</joint>\
</sdf>", screwSdf));
msgs::Joint screwMsg = msgs::JointFromSDF(screwSdf);
EXPECT_TRUE(screwMsg.has_name());
EXPECT_EQ(screwMsg.name(), "box_cylinder");
EXPECT_TRUE(screwMsg.has_type());
EXPECT_EQ(msgs::ConvertJointType(screwMsg.type()), "screw");
EXPECT_TRUE(screwMsg.has_pose());
EXPECT_EQ(msgs::ConvertIgn(screwMsg.pose()),
ignition::math::Pose3d(1, 1, 0, 0, 0, 1.57));
EXPECT_TRUE(screwMsg.has_parent());
EXPECT_EQ(screwMsg.parent(), "box");
EXPECT_TRUE(screwMsg.has_child());
EXPECT_EQ(screwMsg.child(), "cylinder");
EXPECT_TRUE(screwMsg.has_cfm());
EXPECT_NEAR(screwMsg.cfm(), 0.13, 1e-6);
EXPECT_TRUE(screwMsg.has_bounce());
EXPECT_NEAR(screwMsg.bounce(), 0.02, 1e-6);
EXPECT_TRUE(screwMsg.has_velocity());
EXPECT_NEAR(screwMsg.velocity(), 1.1, 1e-6);
EXPECT_TRUE(screwMsg.has_fudge_factor());
EXPECT_NEAR(screwMsg.fudge_factor(), 0.42, 1e-6);
EXPECT_TRUE(screwMsg.has_limit_cfm());
EXPECT_NEAR(screwMsg.limit_cfm(), 0.11, 1e-6);
EXPECT_TRUE(screwMsg.has_limit_erp());
EXPECT_NEAR(screwMsg.limit_erp(), 0.91, 1e-6);
EXPECT_TRUE(screwMsg.has_suspension_cfm());
EXPECT_NEAR(screwMsg.suspension_cfm(), 0.22, 1e-6);
EXPECT_TRUE(screwMsg.has_suspension_erp());
EXPECT_NEAR(screwMsg.suspension_erp(), 0.42, 1e-6);
EXPECT_TRUE(screwMsg.has_axis1());
EXPECT_TRUE(!screwMsg.has_axis2());
const msgs::Axis axisScrewMsg = screwMsg.axis1();
EXPECT_TRUE(axisScrewMsg.has_xyz());
EXPECT_EQ(msgs::ConvertIgn(axisScrewMsg.xyz()),
ignition::math::Vector3d(0, 1, 0));
EXPECT_TRUE(axisScrewMsg.has_limit_lower());
EXPECT_NEAR(axisScrewMsg.limit_lower(), 0.0, 1e-6);
EXPECT_TRUE(axisScrewMsg.has_limit_upper());
EXPECT_NEAR(axisScrewMsg.limit_upper(), 2.0, 1e-6);
EXPECT_TRUE(axisScrewMsg.has_limit_effort());
EXPECT_NEAR(axisScrewMsg.limit_effort(), 1.21, 1e-6);
EXPECT_TRUE(axisScrewMsg.has_limit_velocity());
EXPECT_NEAR(axisScrewMsg.limit_velocity(), 0.12, 1e-6);
EXPECT_TRUE(axisScrewMsg.has_use_parent_model_frame());
EXPECT_EQ(axisScrewMsg.use_parent_model_frame(), true);
EXPECT_TRUE(axisScrewMsg.has_damping());
EXPECT_NEAR(axisScrewMsg.damping(), 0.5, 1e-6);
EXPECT_TRUE(axisScrewMsg.has_friction());
EXPECT_NEAR(axisScrewMsg.friction(), 0.12, 1e-6);
EXPECT_TRUE(screwMsg.has_screw());
EXPECT_NEAR(screwMsg.mutable_screw()->thread_pitch(), 0.2, 1e-6);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, LinkToSDF)
{
const std::string name("test_link");
const ignition::math::Pose3d pose(
ignition::math::Vector3d(3, 2, 1),
ignition::math::Quaterniond(0.5, -0.5, -0.5, 0.5));
msgs::Link linkMsg;
linkMsg.set_name(name);
linkMsg.set_self_collide(false);
linkMsg.set_gravity(true);
linkMsg.set_kinematic(false);
msgs::Set(linkMsg.mutable_pose(), pose);
const double laserRetro1 = 0.4;
const double laserRetro2 = 0.5;
// collision - see CollisionToSDF for a more detailed test
auto collisionMsg1 = linkMsg.add_collision();
collisionMsg1->set_laser_retro(laserRetro1);
collisionMsg1->set_max_contacts(100);
auto collisionMsg2 = linkMsg.add_collision();
collisionMsg2->set_laser_retro(laserRetro2);
collisionMsg2->set_max_contacts(300);
// visual - see VisualToSDF for a more detailed test
auto visualMsg1 = linkMsg.add_visual();
visualMsg1->set_laser_retro(laserRetro1);
auto visualMsg2 = linkMsg.add_visual();
visualMsg2->set_laser_retro(laserRetro2);
// inertial - see InertialToSDF for a more detailed test
auto inertialMsg = linkMsg.mutable_inertial();
inertialMsg->set_mass(3.5);
sdf::ElementPtr linkSDF = msgs::LinkToSDF(linkMsg);
EXPECT_EQ(linkSDF->Get<std::string>("name"), name);
EXPECT_FALSE(linkSDF->Get<bool>("self_collide"));
EXPECT_TRUE(linkSDF->Get<bool>("gravity"));
EXPECT_FALSE(linkSDF->Get<bool>("kinematic"));
EXPECT_EQ(pose, linkSDF->Get<ignition::math::Pose3d>("pose"));
sdf::ElementPtr collisionElem1 = linkSDF->GetElement("collision");
EXPECT_DOUBLE_EQ(collisionElem1->Get<double>("laser_retro"), laserRetro1);
EXPECT_DOUBLE_EQ(collisionElem1->Get<double>("max_contacts"), 100);
sdf::ElementPtr collisionElem2 = collisionElem1->GetNextElement("collision");
EXPECT_DOUBLE_EQ(collisionElem2->Get<double>("laser_retro"), laserRetro2);
EXPECT_DOUBLE_EQ(collisionElem2->Get<double>("max_contacts"), 300);
sdf::ElementPtr visualElem1 = linkSDF->GetElement("visual");
EXPECT_DOUBLE_EQ(visualElem1->Get<double>("laser_retro"), laserRetro1);
sdf::ElementPtr visualElem2 = visualElem1->GetNextElement("visual");
EXPECT_DOUBLE_EQ(visualElem2->Get<double>("laser_retro"), laserRetro2);
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
EXPECT_DOUBLE_EQ(inertialElem->Get<double>("mass"), 3.5);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, CollisionToSDF)
{
const std::string name("collision");
msgs::Collision collisionMsg;
collisionMsg.set_name(name);
collisionMsg.set_laser_retro(0.2);
collisionMsg.set_max_contacts(5);
const ignition::math::Pose3d pose(
ignition::math::Vector3d(1, 2, 3),
ignition::math::Quaterniond(0, 0, 1, 0));
msgs::Set(collisionMsg.mutable_pose(), pose);
// geometry - see GeometryToSDF for a more detailed test
msgs::Geometry *geomMsg = collisionMsg.mutable_geometry();
geomMsg->set_type(msgs::Geometry::CYLINDER);
msgs::CylinderGeom *cylinderMsg = geomMsg->mutable_cylinder();
cylinderMsg->set_radius(3.3);
cylinderMsg->set_length(1.0);
// surface - see SurfaceToSDF for a more detailed test
msgs::Surface *surfaceMsg = collisionMsg.mutable_surface();
surfaceMsg->set_restitution_coefficient(5.1);
surfaceMsg->set_bounce_threshold(1300);
sdf::ElementPtr collisionSDF = msgs::CollisionToSDF(collisionMsg);
EXPECT_TRUE(collisionSDF->HasAttribute("name"));
EXPECT_EQ(name, collisionSDF->Get<std::string>("name"));
EXPECT_DOUBLE_EQ(collisionSDF->Get<double>("laser_retro"), 0.2);
EXPECT_DOUBLE_EQ(collisionSDF->Get<double>("max_contacts"), 5);
EXPECT_EQ(collisionSDF->Get<ignition::math::Pose3d>("pose"), pose);
sdf::ElementPtr geomElem = collisionSDF->GetElement("geometry");
sdf::ElementPtr cylinderElem = geomElem->GetElement("cylinder");
EXPECT_DOUBLE_EQ(cylinderElem->Get<double>("radius"), 3.3);
EXPECT_DOUBLE_EQ(cylinderElem->Get<double>("length"), 1.0);
sdf::ElementPtr surfaceElem = collisionSDF->GetElement("surface");
sdf::ElementPtr bounceElem = surfaceElem->GetElement("bounce");
EXPECT_DOUBLE_EQ(bounceElem->Get<double>("restitution_coefficient"), 5.1);
EXPECT_DOUBLE_EQ(bounceElem->Get<double>("threshold"), 1300);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, VisualToSDF)
{
const std::string name("visual");
const double laserRetro = 0.2;
const ignition::math::Pose3d pose(
ignition::math::Vector3d(1, 2, 3),
ignition::math::Quaterniond(0, 0, 1, 0));
const double radius = 3.3;
const std::string materialName("Gazebo/Grey");
const std::string uri("pretend_this_is_a_URI");
msgs::Visual visualMsg;
visualMsg.set_name(name);
visualMsg.set_laser_retro(laserRetro);
msgs::Set(visualMsg.mutable_pose(), pose);
// geometry - see GeometryToSDF for a more detailed test
auto geomMsg = visualMsg.mutable_geometry();
geomMsg->set_type(msgs::Geometry::SPHERE);
geomMsg->mutable_sphere()->set_radius(radius);
// material - see MaterialToSDF for a more detailed test
auto scriptMsg = visualMsg.mutable_material()->mutable_script();
scriptMsg->set_name(materialName);
scriptMsg->add_uri();
scriptMsg->set_uri(0, uri);
sdf::ElementPtr visualSDF = msgs::VisualToSDF(visualMsg);
EXPECT_TRUE(visualSDF->HasAttribute("name"));
EXPECT_EQ(name, visualSDF->Get<std::string>("name"));
EXPECT_DOUBLE_EQ(visualSDF->Get<double>("laser_retro"), laserRetro);
EXPECT_EQ(pose, visualSDF->Get<ignition::math::Pose3d>("pose"));
ASSERT_TRUE(visualSDF->HasElement("geometry"));
sdf::ElementPtr geomElem = visualSDF->GetElement("geometry");
EXPECT_TRUE(geomElem->HasElement("sphere"));
sdf::ElementPtr sphereElem = geomElem->GetElement("sphere");
EXPECT_TRUE(sphereElem->HasElement("radius"));
EXPECT_DOUBLE_EQ(sphereElem->Get<double>("radius"), radius);
ASSERT_TRUE(visualSDF->HasElement("material"));
sdf::ElementPtr materialElem = visualSDF->GetElement("material");
EXPECT_TRUE(materialElem->HasElement("script"));
sdf::ElementPtr scriptElem = materialElem->GetElement("script");
EXPECT_TRUE(scriptElem->HasElement("name"));
EXPECT_EQ(materialName, scriptElem->Get<std::string>("name"));
EXPECT_TRUE(scriptElem->HasElement("uri"));
EXPECT_EQ(uri, scriptElem->Get<std::string>("uri"));
// Test the meta.layer property
{
msgs::Visual msg;
auto metaMsg = msg.mutable_meta();
metaMsg->set_layer(1);
sdf::ElementPtr visSdf = msgs::VisualToSDF(msg);
EXPECT_TRUE(visSdf->HasElement("meta"));
EXPECT_TRUE(visSdf->GetElement("meta")->HasElement("layer"));
EXPECT_EQ(visSdf->GetElement("meta")->Get<int32_t>("layer"), 1);
msgs::Visual msg2 = msgs::VisualFromSDF(visSdf);
EXPECT_TRUE(msg2.has_meta());
EXPECT_TRUE(msg2.meta().has_layer());
EXPECT_EQ(msg2.meta().layer(), 1);
}
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, GeometryToSDF)
{
// box
const ignition::math::Vector3d boxSize(0.5, 0.75, 1.0);
msgs::Geometry boxMsg;
boxMsg.set_type(msgs::Geometry::BOX);
msgs::BoxGeom *boxGeom = boxMsg.mutable_box();
msgs::Set(boxGeom->mutable_size(), boxSize);
sdf::ElementPtr boxSDF = msgs::GeometryToSDF(boxMsg);
sdf::ElementPtr boxElem = boxSDF->GetElement("box");
EXPECT_EQ(boxElem->Get<ignition::math::Vector3d>("size"), boxSize);
// cylinder
msgs::Geometry cylinderMsg;
cylinderMsg.set_type(msgs::Geometry::CYLINDER);
msgs::CylinderGeom *cylinderGeom = cylinderMsg.mutable_cylinder();
cylinderGeom->set_radius(0.3);
cylinderGeom->set_length(1.0);
sdf::ElementPtr cylinderSDF = msgs::GeometryToSDF(cylinderMsg);
sdf::ElementPtr cylinderElem = cylinderSDF->GetElement("cylinder");
EXPECT_DOUBLE_EQ(cylinderElem->Get<double>("radius"), 0.3);
EXPECT_DOUBLE_EQ(cylinderElem->Get<double>("length"), 1.0);
// sphere
msgs::Geometry sphereMsg;
sphereMsg.set_type(msgs::Geometry::SPHERE);
msgs::SphereGeom *sphereGeom = sphereMsg.mutable_sphere();
sphereGeom->set_radius(3.0);
sdf::ElementPtr sphereSDF = msgs::GeometryToSDF(sphereMsg);
sdf::ElementPtr sphereElem = sphereSDF->GetElement("sphere");
EXPECT_DOUBLE_EQ(sphereElem->Get<double>("radius"), 3.0);
// plane
msgs::Geometry planeMsg;
planeMsg.set_type(msgs::Geometry::PLANE);
msgs::PlaneGeom *planeGeom = planeMsg.mutable_plane();
msgs::Set(planeGeom->mutable_normal(), ignition::math::Vector3d(0, 0, 1.0));
msgs::Set(planeGeom->mutable_size(), ignition::math::Vector2d(0.5, 0.8));
sdf::ElementPtr planeSDF = msgs::GeometryToSDF(planeMsg);
sdf::ElementPtr planeElem = planeSDF->GetElement("plane");
EXPECT_TRUE(planeElem->Get<ignition::math::Vector3d>("normal") ==
ignition::math::Vector3d(0, 0, 1.0));
EXPECT_TRUE(planeElem->Get<ignition::math::Vector2d>("size") ==
ignition::math::Vector2d(0.5, 0.8));
// image
msgs::Geometry imageMsg;
imageMsg.set_type(msgs::Geometry::IMAGE);
msgs::ImageGeom *imageGeom = imageMsg.mutable_image();
imageGeom->set_uri("test_uri");
imageGeom->set_scale(1.8);
imageGeom->set_threshold(255);
imageGeom->set_height(1.3);
imageGeom->set_granularity(2);
sdf::ElementPtr imageSDF = msgs::GeometryToSDF(imageMsg);
sdf::ElementPtr imageElem = imageSDF->GetElement("image");
EXPECT_STREQ(imageElem->Get<std::string>("uri").c_str(), "test_uri");
EXPECT_DOUBLE_EQ(imageElem->Get<double>("scale"), 1.8);
EXPECT_DOUBLE_EQ(imageElem->Get<double>("threshold"), 255);
EXPECT_DOUBLE_EQ(imageElem->Get<double>("height"), 1.3);
EXPECT_DOUBLE_EQ(imageElem->Get<int>("granularity"), 2);
// heightmap
msgs::Geometry heightmapMsg;
heightmapMsg.set_type(msgs::Geometry::HEIGHTMAP);
msgs::HeightmapGeom *heightmapGeom = heightmapMsg.mutable_heightmap();
heightmapGeom->set_filename("test_heightmap_filename");
msgs::Set(heightmapGeom->mutable_size(),
ignition::math::Vector3d(100, 200, 30));
msgs::Set(heightmapGeom->mutable_origin(),
ignition::math::Vector3d(50, 100, 15));
heightmapGeom->set_sampling(1);
heightmapGeom->set_use_terrain_paging(true);
msgs::HeightmapGeom_Texture *texture1 = heightmapGeom->add_texture();
texture1->set_diffuse("test_diffuse1");
texture1->set_normal("test_normal1");
texture1->set_size(10);
msgs::HeightmapGeom_Texture *texture2 = heightmapGeom->add_texture();
texture2->set_diffuse("test_diffuse2");
texture2->set_normal("test_normal2");
texture2->set_size(20);
msgs::HeightmapGeom_Blend *blend = heightmapGeom->add_blend();
blend->set_min_height(25);
blend->set_fade_dist(5);
sdf::ElementPtr heightmapSDF = msgs::GeometryToSDF(heightmapMsg);
sdf::ElementPtr heightmapElem = heightmapSDF->GetElement("heightmap");
EXPECT_STREQ(heightmapElem->Get<std::string>("uri").c_str(),
"test_heightmap_filename");
EXPECT_TRUE(heightmapElem->Get<ignition::math::Vector3d>("size") ==
ignition::math::Vector3d(100, 200, 30));
EXPECT_TRUE(heightmapElem->Get<ignition::math::Vector3d>("pos") ==
ignition::math::Vector3d(50, 100, 15));
// fix test while we wait for new sdformat4 version to be released
if (heightmapElem->HasElementDescription("sampling"))
{
EXPECT_EQ(heightmapElem->Get<unsigned int>("sampling"), 1u);
}
EXPECT_TRUE(heightmapElem->Get<bool>("use_terrain_paging"));
sdf::ElementPtr textureElem1 = heightmapElem->GetElement("texture");
EXPECT_STREQ(textureElem1->Get<std::string>("diffuse").c_str(),
"test_diffuse1");
EXPECT_STREQ(textureElem1->Get<std::string>("normal").c_str(),
"test_normal1");
EXPECT_DOUBLE_EQ(textureElem1->Get<double>("size"), 10);
sdf::ElementPtr textureElem2 = textureElem1->GetNextElement("texture");
EXPECT_STREQ(textureElem2->Get<std::string>("diffuse").c_str(),
"test_diffuse2");
EXPECT_STREQ(textureElem2->Get<std::string>("normal").c_str(),
"test_normal2");
EXPECT_DOUBLE_EQ(textureElem2->Get<double>("size"), 20);
sdf::ElementPtr blendElem = heightmapElem->GetElement("blend");
EXPECT_DOUBLE_EQ(blendElem->Get<double>("min_height"), 25);
EXPECT_DOUBLE_EQ(blendElem->Get<double>("fade_dist"), 5);
// mesh
msgs::Geometry meshMsg;
meshMsg.set_type(msgs::Geometry::MESH);
msgs::MeshGeom *meshGeom = meshMsg.mutable_mesh();
meshGeom->set_filename("test_mesh_filename");
msgs::Set(meshGeom->mutable_scale(), ignition::math::Vector3d(2.3, 1.2, 2.9));
meshGeom->set_submesh("test_mesh_submesh");
meshGeom->set_center_submesh(false);
sdf::ElementPtr meshSDF = msgs::GeometryToSDF(meshMsg);
sdf::ElementPtr meshElem = meshSDF->GetElement("mesh");
EXPECT_STREQ(meshElem->Get<std::string>("uri").c_str(),
"test_mesh_filename");
EXPECT_TRUE(meshElem->Get<ignition::math::Vector3d>("scale") ==
ignition::math::Vector3d(2.3, 1.2, 2.9));
sdf::ElementPtr submeshElem = meshElem->GetElement("submesh");
EXPECT_STREQ(submeshElem->Get<std::string>("name").c_str(),
"test_mesh_submesh");
EXPECT_TRUE(!submeshElem->Get<bool>("center"));
// polyline
msgs::Geometry polylineMsg;
polylineMsg.set_type(msgs::Geometry::POLYLINE);
msgs::Polyline *polylineGeom = polylineMsg.add_polyline();
polylineGeom->set_height(2.33);
const ignition::math::Vector2d point1(0.5, 0.7);
const ignition::math::Vector2d point2(3.5, 4.7);
const ignition::math::Vector2d point3(1000, 2000);
msgs::Set(polylineGeom->add_point(), point1);
msgs::Set(polylineGeom->add_point(), point2);
msgs::Set(polylineGeom->add_point(), point3);
sdf::ElementPtr polylineSDF = msgs::GeometryToSDF(polylineMsg);
sdf::ElementPtr polylineElem = polylineSDF->GetElement("polyline");
EXPECT_DOUBLE_EQ(polylineElem->Get<double>("height"), 2.33);
sdf::ElementPtr pointElem1 = polylineElem->GetElement("point");
sdf::ElementPtr pointElem2 = pointElem1->GetNextElement("point");
sdf::ElementPtr pointElem3 = pointElem2->GetNextElement("point");
EXPECT_EQ(pointElem1->Get<ignition::math::Vector2d>(), point1);
EXPECT_EQ(pointElem2->Get<ignition::math::Vector2d>(), point2);
EXPECT_EQ(pointElem3->Get<ignition::math::Vector2d>(), point3);
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, MeshToSDF)
{
const ignition::math::Vector3d meshScale(0.1, 0.2, 0.3);
msgs::MeshGeom msg;
msg.set_filename("test_filename");
msgs::Set(msg.mutable_scale(), meshScale);
msg.set_submesh("test_submesh");
msg.set_center_submesh(true);
sdf::ElementPtr meshSDF = msgs::MeshToSDF(msg);
EXPECT_STREQ(meshSDF->Get<std::string>("uri").c_str(), "test_filename");
EXPECT_TRUE(meshSDF->HasElement("scale"));
ignition::math::Vector3d scale =
meshSDF->Get<ignition::math::Vector3d>("scale");
EXPECT_DOUBLE_EQ(scale.X(), meshScale.X());
EXPECT_DOUBLE_EQ(scale.Y(), meshScale.Y());
EXPECT_DOUBLE_EQ(scale.Z(), meshScale.Z());
sdf::ElementPtr submeshElem = meshSDF->GetElement("submesh");
EXPECT_STREQ(submeshElem->Get<std::string>("name").c_str(), "test_submesh");
EXPECT_TRUE(submeshElem->Get<bool>("center"));
// no submesh
const ignition::math::Vector3d meshScale2(1, 2, 3);
msgs::MeshGeom msg2;
msg2.set_filename("test_filename2");
msgs::Set(msg2.mutable_scale(), meshScale2);
sdf::ElementPtr meshSDF2 = msgs::MeshToSDF(msg2);
EXPECT_STREQ(meshSDF2->Get<std::string>("uri").c_str(), "test_filename2");
EXPECT_TRUE(meshSDF2->HasElement("scale"));
ignition::math::Vector3d scale2 =
meshSDF2->Get<ignition::math::Vector3d>("scale");
EXPECT_DOUBLE_EQ(scale2.X(), meshScale2.X());
EXPECT_DOUBLE_EQ(scale2.Y(), meshScale2.Y());
EXPECT_DOUBLE_EQ(scale2.Z(), meshScale2.Z());
EXPECT_FALSE(meshSDF2->HasElement("submesh"));
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, InertialToSDF)
{
const double mass = 3.4;
const ignition::math::Pose3d pose(
ignition::math::Vector3d(1.2, 3.4, 5.6),
ignition::math::Quaterniond(0.7071, 0.0, 0.7071, 0.0));
const double ixx = 0.0133;
const double ixy = -0.0003;
const double ixz = -0.0004;
const double iyy = 0.0116;
const double iyz = 0.0008;
const double izz = 0.0038;
msgs::Inertial msg;
msg.set_mass(mass);
msgs::Set(msg.mutable_pose(), pose);
msg.set_ixx(ixx);
msg.set_ixy(ixy);
msg.set_ixz(ixz);
msg.set_iyy(iyy);
msg.set_iyz(iyz);
msg.set_izz(izz);
sdf::ElementPtr inertialSDF = msgs::InertialToSDF(msg);
EXPECT_TRUE(inertialSDF->HasElement("mass"));
EXPECT_DOUBLE_EQ(inertialSDF->Get<double>("mass"), mass);
EXPECT_TRUE(inertialSDF->HasElement("pose"));
EXPECT_EQ(inertialSDF->Get<ignition::math::Pose3d>("pose"), pose);
{
ASSERT_TRUE(inertialSDF->HasElement("inertia"));
sdf::ElementPtr inertiaElem = inertialSDF->GetElement("inertia");
EXPECT_TRUE(inertiaElem->HasElement("ixx"));
EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixx"), ixx);
EXPECT_TRUE(inertiaElem->HasElement("ixy"));
EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixy"), ixy);
EXPECT_TRUE(inertiaElem->HasElement("ixz"));
EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixz"), ixz);
EXPECT_TRUE(inertiaElem->HasElement("iyy"));
EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyy"), iyy);
EXPECT_TRUE(inertiaElem->HasElement("iyz"));
EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyz"), iyz);
EXPECT_TRUE(inertiaElem->HasElement("izz"));
EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("izz"), izz);
}
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, MaterialToSDF)
{
msgs::Material msg;
const std::string name("Gazebo/Grey");
const std::string uri("file://media/materials/scripts/gazebo.material");
const msgs::Material::ShaderType type = msgs::Material::VERTEX;
const std::string normalMap("normalMap");
const bool lighting = true;
const common::Color ambient(.1, .2, .3, 1.0);
const common::Color diffuse(.4, .5, .6, 1.0);
const common::Color emissive(.5, .5, .5, 0.5);
const common::Color specular(.7, .8, .9, 1.0);
msg.mutable_script()->set_name(name);
msg.mutable_script()->add_uri();
msg.mutable_script()->set_uri(0, uri);
msg.set_shader_type(type);
msg.set_normal_map(normalMap);
msg.set_lighting(lighting);
msgs::Set(msg.mutable_ambient(), ambient);
msgs::Set(msg.mutable_diffuse(), diffuse);
msgs::Set(msg.mutable_emissive(), emissive);
msgs::Set(msg.mutable_specular(), specular);
sdf::ElementPtr materialSDF = msgs::MaterialToSDF(msg);
{
ASSERT_TRUE(materialSDF->HasElement("script"));
sdf::ElementPtr scriptElem = materialSDF->GetElement("script");
EXPECT_TRUE(scriptElem->HasElement("name"));
EXPECT_EQ(name, scriptElem->Get<std::string>("name"));
EXPECT_TRUE(scriptElem->HasElement("uri"));
EXPECT_EQ(uri, scriptElem->Get<std::string>("uri"));
}
{
ASSERT_TRUE(materialSDF->HasElement("shader"));
sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
EXPECT_TRUE(shaderElem->HasAttribute("type"));
EXPECT_EQ(msgs::ConvertShaderType(type),
shaderElem->Get<std::string>("type"));
EXPECT_TRUE(shaderElem->HasElement("normal_map"));
EXPECT_EQ(normalMap, shaderElem->Get<std::string>("normal_map"));
}
EXPECT_TRUE(materialSDF->HasElement("ambient"));
EXPECT_EQ(ambient, materialSDF->Get<common::Color>("ambient"));
EXPECT_TRUE(materialSDF->HasElement("diffuse"));
EXPECT_EQ(diffuse, materialSDF->Get<common::Color>("diffuse"));
EXPECT_TRUE(materialSDF->HasElement("emissive"));
EXPECT_EQ(emissive, materialSDF->Get<common::Color>("emissive"));
EXPECT_TRUE(materialSDF->HasElement("specular"));
EXPECT_EQ(specular, materialSDF->Get<common::Color>("specular"));
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, SurfaceToSDF)
{
msgs::Surface msg;
// friction
const double mu = 0.1;
const double mu2 = 0.2;
const ignition::math::Vector3d fdir1(0.3, 0.4, 0.5);
const double slip1 = 0.6;
const double slip2 = 0.7;
msgs::Friction *friction = msg.mutable_friction();
friction->set_mu(mu);
friction->set_mu2(mu2);
msgs::Set(friction->mutable_fdir1(), fdir1);
friction->set_slip1(slip1);
friction->set_slip2(slip2);
// bounce
msg.set_restitution_coefficient(1.1);
msg.set_bounce_threshold(1000);
// other ode surface properties
msg.set_soft_cfm(0.9);
msg.set_soft_erp(0.3);
msg.set_kp(0.4);
msg.set_kd(0.8);
msg.set_max_vel(3.8);
msg.set_min_depth(0.0001);
msg.set_collide_without_contact(true);
msg.set_collide_without_contact_bitmask(0x0004);
msg.set_collide_bitmask(0x01);
sdf::ElementPtr surfaceSDF = msgs::SurfaceToSDF(msg);
sdf::ElementPtr frictionElem = surfaceSDF->GetElement("friction");
sdf::ElementPtr frictionPhysicsElem = frictionElem->GetElement("ode");
EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu"), mu);
EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu2"), mu2);
EXPECT_EQ(frictionPhysicsElem->Get<ignition::math::Vector3d>("fdir1"), fdir1);
EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip1"), slip1);
EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip2"), slip2);
sdf::ElementPtr bounceElem = surfaceSDF->GetElement("bounce");
EXPECT_DOUBLE_EQ(bounceElem->Get<double>("restitution_coefficient"), 1.1);
EXPECT_DOUBLE_EQ(bounceElem->Get<double>("threshold"), 1000);
sdf::ElementPtr contactElem = surfaceSDF->GetElement("contact");
sdf::ElementPtr contactPhysicsElem = contactElem->GetElement("ode");
EXPECT_DOUBLE_EQ(contactPhysicsElem->Get<double>("soft_cfm"), 0.9);
EXPECT_DOUBLE_EQ(contactPhysicsElem->Get<double>("soft_erp"), 0.3);
EXPECT_DOUBLE_EQ(contactPhysicsElem->Get<double>("kp"), 0.4);
EXPECT_DOUBLE_EQ(contactPhysicsElem->Get<double>("kd"), 0.8);
EXPECT_DOUBLE_EQ(contactPhysicsElem->Get<double>("max_vel"), 3.8);
EXPECT_DOUBLE_EQ(contactPhysicsElem->Get<double>("min_depth"), 0.0001);
EXPECT_TRUE(contactElem->Get<bool>("collide_without_contact"));
EXPECT_EQ(contactElem->Get<unsigned int>("collide_without_contact_bitmask"),
static_cast<unsigned int>(0x0004));
EXPECT_EQ(contactElem->Get<unsigned int>("collide_bitmask"),
static_cast<unsigned int>(0x01));
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, JointToSDF)
{
// universal
{
const std::string name("test_joint");
const msgs::Joint::Type type = msgs::Joint::UNIVERSAL;
const ignition::math::Pose3d pose(
ignition::math::Vector3d(9, 1, 1),
ignition::math::Quaterniond(0, 1, 0, 0));
const std::string parent("parent_link");
const std::string child("child_link");
const double cfm = 0.1;
const double bounce = 0.2;
const double velocity = 0.6;
const double fudge_factor = 0.7;
const double limit_cfm = 0.3;
const double limit_erp = 0.4;
const double suspension_cfm = 0.8;
const double suspension_erp = 0.9;
const ignition::math::Vector3d xyz1(0.6, 0.8, 0.0);
const ignition::math::Vector3d xyz2(0.0, 0.0, 1.0);
const double limit_lower1 = -2.0;
const double limit_lower2 = -4.0;
const double limit_upper1 = 12.0;
const double limit_upper2 = 24.0;
const double limit_effort1 = 1e3;
const double limit_effort2 = 1e4;
const double limit_velocity1 = 33;
const double limit_velocity2 = 44;
const double damping1 = 1e-2;
const double damping2 = 3e-2;
const double friction1 = 1e2;
const double friction2 = 3e2;
const bool useParentModelFrame1 = true;
// don't set use_parent_model_frame for axis2
// expect it to match sdformat default (false)
msgs::Joint jointMsg;
jointMsg.set_name(name);
jointMsg.set_type(type);
jointMsg.set_parent(parent);
jointMsg.set_child(child);
msgs::Set(jointMsg.mutable_pose(), pose);
jointMsg.set_cfm(cfm);
jointMsg.set_bounce(bounce);
jointMsg.set_velocity(velocity);
jointMsg.set_fudge_factor(fudge_factor);
jointMsg.set_limit_cfm(limit_cfm);
jointMsg.set_limit_erp(limit_erp);
jointMsg.set_suspension_cfm(suspension_cfm);
jointMsg.set_suspension_erp(suspension_erp);
{
auto axis1 = jointMsg.mutable_axis1();
msgs::Set(axis1->mutable_xyz(), xyz1);
axis1->set_limit_lower(limit_lower1);
axis1->set_limit_upper(limit_upper1);
axis1->set_limit_effort(limit_effort1);
axis1->set_limit_velocity(limit_velocity1);
axis1->set_damping(damping1);
axis1->set_friction(friction1);
axis1->set_use_parent_model_frame(useParentModelFrame1);
}
{
auto axis2 = jointMsg.mutable_axis2();
msgs::Set(axis2->mutable_xyz(), xyz2);
axis2->set_limit_lower(limit_lower2);
axis2->set_limit_upper(limit_upper2);
axis2->set_limit_effort(limit_effort2);
axis2->set_limit_velocity(limit_velocity2);
axis2->set_damping(damping2);
axis2->set_friction(friction2);
}
sdf::ElementPtr jointSDF = msgs::JointToSDF(jointMsg);
EXPECT_TRUE(jointSDF->HasAttribute("name"));
EXPECT_EQ(jointSDF->Get<std::string>("name"), name);
EXPECT_TRUE(jointSDF->HasAttribute("type"));
EXPECT_STREQ(jointSDF->Get<std::string>("type").c_str(), "universal");
EXPECT_TRUE(jointSDF->HasElement("parent"));
EXPECT_EQ(jointSDF->Get<std::string>("parent"), parent);
EXPECT_TRUE(jointSDF->HasElement("child"));
EXPECT_EQ(jointSDF->Get<std::string>("child"), child);
EXPECT_TRUE(jointSDF->HasElement("pose"));
EXPECT_EQ(pose, jointSDF->Get<ignition::math::Pose3d>("pose"));
EXPECT_TRUE(jointSDF->HasElement("axis"));
{
auto axisElem = jointSDF->GetElement("axis");
EXPECT_TRUE(axisElem->HasElement("xyz"));
EXPECT_EQ(xyz1, axisElem->Get<ignition::math::Vector3d>("xyz"));
EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
EXPECT_EQ(useParentModelFrame1,
axisElem->Get<bool>("use_parent_model_frame"));
EXPECT_TRUE(axisElem->HasElement("dynamics"));
auto axisDynamics = axisElem->GetElement("dynamics");
EXPECT_TRUE(axisDynamics->HasElement("damping"));
EXPECT_DOUBLE_EQ(damping1, axisDynamics->Get<double>("damping"));
EXPECT_TRUE(axisDynamics->HasElement("friction"));
EXPECT_DOUBLE_EQ(friction1, axisDynamics->Get<double>("friction"));
EXPECT_TRUE(axisElem->HasElement("limit"));
auto axisLimit = axisElem->GetElement("limit");
EXPECT_TRUE(axisLimit->HasElement("lower"));
EXPECT_DOUBLE_EQ(limit_lower1, axisLimit->Get<double>("lower"));
EXPECT_TRUE(axisLimit->HasElement("upper"));
EXPECT_DOUBLE_EQ(limit_upper1, axisLimit->Get<double>("upper"));
EXPECT_TRUE(axisLimit->HasElement("effort"));
EXPECT_DOUBLE_EQ(limit_effort1, axisLimit->Get<double>("effort"));
EXPECT_TRUE(axisLimit->HasElement("velocity"));
EXPECT_DOUBLE_EQ(limit_velocity1, axisLimit->Get<double>("velocity"));
}
EXPECT_TRUE(jointSDF->HasElement("axis2"));
{
auto axisElem = jointSDF->GetElement("axis2");
EXPECT_TRUE(axisElem->HasElement("xyz"));
EXPECT_EQ(xyz2, axisElem->Get<ignition::math::Vector3d>("xyz"));
// use_parent_model_frame is required in axis.proto
// so expect to to exist even if we don't set it
EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
// expect false (default sdformat value)
EXPECT_FALSE(axisElem->Get<bool>("use_parent_model_frame"));
EXPECT_TRUE(axisElem->HasElement("dynamics"));
auto axisDynamics = axisElem->GetElement("dynamics");
EXPECT_TRUE(axisDynamics->HasElement("damping"));
EXPECT_DOUBLE_EQ(damping2, axisDynamics->Get<double>("damping"));
EXPECT_TRUE(axisDynamics->HasElement("friction"));
EXPECT_DOUBLE_EQ(friction2, axisDynamics->Get<double>("friction"));
EXPECT_TRUE(axisElem->HasElement("limit"));
auto axisLimit = axisElem->GetElement("limit");
EXPECT_TRUE(axisLimit->HasElement("lower"));
EXPECT_DOUBLE_EQ(limit_lower2, axisLimit->Get<double>("lower"));
EXPECT_TRUE(axisLimit->HasElement("upper"));
EXPECT_DOUBLE_EQ(limit_upper2, axisLimit->Get<double>("upper"));
EXPECT_TRUE(axisLimit->HasElement("effort"));
EXPECT_DOUBLE_EQ(limit_effort2, axisLimit->Get<double>("effort"));
EXPECT_TRUE(axisLimit->HasElement("velocity"));
EXPECT_DOUBLE_EQ(limit_velocity2, axisLimit->Get<double>("velocity"));
}
EXPECT_TRUE(jointSDF->HasElement("physics"));
auto physicsElem = jointSDF->GetElement("physics");
EXPECT_TRUE(physicsElem->HasElement("ode"));
auto odePhysics = physicsElem->GetElement("ode");
EXPECT_TRUE(odePhysics->HasElement("cfm"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("cfm"), cfm);
EXPECT_TRUE(odePhysics->HasElement("bounce"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("bounce"), bounce);
EXPECT_TRUE(odePhysics->HasElement("velocity"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("velocity"), velocity);
EXPECT_TRUE(odePhysics->HasElement("fudge_factor"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("fudge_factor"), fudge_factor);
EXPECT_TRUE(odePhysics->HasElement("limit"));
auto limitElem = odePhysics->GetElement("limit");
EXPECT_TRUE(limitElem->HasElement("cfm"));
EXPECT_DOUBLE_EQ(limitElem->Get<double>("cfm"), limit_cfm);
EXPECT_TRUE(limitElem->HasElement("erp"));
EXPECT_DOUBLE_EQ(limitElem->Get<double>("erp"), limit_erp);
EXPECT_TRUE(odePhysics->HasElement("suspension"));
auto suspensionElem = odePhysics->GetElement("suspension");
EXPECT_TRUE(suspensionElem->HasElement("cfm"));
EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("cfm"), suspension_cfm);
EXPECT_TRUE(suspensionElem->HasElement("erp"));
EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("erp"), suspension_erp);
}
// gearbox
{
const std::string name("test_gearbox_joint");
const msgs::Joint::Type type = msgs::Joint::GEARBOX;
const ignition::math::Pose3d pose(
ignition::math::Vector3d(2, 1, 3),
ignition::math::Quaterniond(0, 0, 1, 0));
const std::string parent("parent_gearbox_link");
const std::string child("child_gearbox_link");
const double cfm = 0.1;
const double bounce = 0.1;
const double velocity = 0.2;
const double fudge_factor = 0.4;
const double limit_cfm = 0.2;
const double limit_erp = 0.6;
const double suspension_cfm = 0.7;
const double suspension_erp = 0.4;
const ignition::math::Vector3d xyz1(0.0, 1.0, 0.0);
const ignition::math::Vector3d xyz2(0.0, 1.0, 0.0);
const double limit_lower1 = -1.0;
const double limit_lower2 = -2.0;
const double limit_upper1 = 2.0;
const double limit_upper2 = 4.0;
const double limit_effort1 = 1e2;
const double limit_effort2 = 1e3;
const double limit_velocity1 = 23;
const double limit_velocity2 = 54;
const double damping1 = 2e-2;
const double damping2 = 4e-2;
const double friction1 = 2e2;
const double friction2 = 1e2;
const bool useParentModelFrame1 = true;
// don't set use_parent_model_frame for axis2
// expect it to match sdformat default (false)
const std::string gearbox_reference_body = "child_gearbox_link";
const double gearbox_ratio = 0.2;
msgs::Joint jointMsg;
jointMsg.set_name(name);
jointMsg.set_type(type);
jointMsg.set_parent(parent);
jointMsg.set_child(child);
msgs::Set(jointMsg.mutable_pose(), pose);
jointMsg.set_cfm(cfm);
jointMsg.set_bounce(bounce);
jointMsg.set_velocity(velocity);
jointMsg.set_fudge_factor(fudge_factor);
jointMsg.set_limit_cfm(limit_cfm);
jointMsg.set_limit_erp(limit_erp);
jointMsg.set_suspension_cfm(suspension_cfm);
jointMsg.set_suspension_erp(suspension_erp);
{
auto axis1 = jointMsg.mutable_axis1();
msgs::Set(axis1->mutable_xyz(), xyz1);
axis1->set_limit_lower(limit_lower1);
axis1->set_limit_upper(limit_upper1);
axis1->set_limit_effort(limit_effort1);
axis1->set_limit_velocity(limit_velocity1);
axis1->set_damping(damping1);
axis1->set_friction(friction1);
axis1->set_use_parent_model_frame(useParentModelFrame1);
}
{
auto axis2 = jointMsg.mutable_axis2();
msgs::Set(axis2->mutable_xyz(), xyz2);
axis2->set_limit_lower(limit_lower2);
axis2->set_limit_upper(limit_upper2);
axis2->set_limit_effort(limit_effort2);
axis2->set_limit_velocity(limit_velocity2);
axis2->set_damping(damping2);
axis2->set_friction(friction2);
}
msgs::Joint::Gearbox *gearboxMsg = jointMsg.mutable_gearbox();
gearboxMsg->set_gearbox_reference_body(gearbox_reference_body);
gearboxMsg->set_gearbox_ratio(gearbox_ratio);
sdf::ElementPtr jointSDF = msgs::JointToSDF(jointMsg);
EXPECT_TRUE(jointSDF->HasAttribute("name"));
EXPECT_EQ(jointSDF->Get<std::string>("name"), name);
EXPECT_TRUE(jointSDF->HasAttribute("type"));
EXPECT_STREQ(jointSDF->Get<std::string>("type").c_str(), "gearbox");
EXPECT_TRUE(jointSDF->HasElement("parent"));
EXPECT_EQ(jointSDF->Get<std::string>("parent"), parent);
EXPECT_TRUE(jointSDF->HasElement("child"));
EXPECT_EQ(jointSDF->Get<std::string>("child"), child);
EXPECT_TRUE(jointSDF->HasElement("pose"));
EXPECT_EQ(pose, jointSDF->Get<ignition::math::Pose3d>("pose"));
EXPECT_TRUE(jointSDF->HasElement("axis"));
{
auto axisElem = jointSDF->GetElement("axis");
EXPECT_TRUE(axisElem->HasElement("xyz"));
EXPECT_EQ(xyz1, axisElem->Get<ignition::math::Vector3d>("xyz"));
EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
EXPECT_EQ(useParentModelFrame1,
axisElem->Get<bool>("use_parent_model_frame"));
EXPECT_TRUE(axisElem->HasElement("dynamics"));
auto axisDynamics = axisElem->GetElement("dynamics");
EXPECT_TRUE(axisDynamics->HasElement("damping"));
EXPECT_DOUBLE_EQ(damping1, axisDynamics->Get<double>("damping"));
EXPECT_TRUE(axisDynamics->HasElement("friction"));
EXPECT_DOUBLE_EQ(friction1, axisDynamics->Get<double>("friction"));
EXPECT_TRUE(axisElem->HasElement("limit"));
auto axisLimit = axisElem->GetElement("limit");
EXPECT_TRUE(axisLimit->HasElement("lower"));
EXPECT_DOUBLE_EQ(limit_lower1, axisLimit->Get<double>("lower"));
EXPECT_TRUE(axisLimit->HasElement("upper"));
EXPECT_DOUBLE_EQ(limit_upper1, axisLimit->Get<double>("upper"));
EXPECT_TRUE(axisLimit->HasElement("effort"));
EXPECT_DOUBLE_EQ(limit_effort1, axisLimit->Get<double>("effort"));
EXPECT_TRUE(axisLimit->HasElement("velocity"));
EXPECT_DOUBLE_EQ(limit_velocity1, axisLimit->Get<double>("velocity"));
}
EXPECT_TRUE(jointSDF->HasElement("axis2"));
{
auto axisElem = jointSDF->GetElement("axis2");
EXPECT_TRUE(axisElem->HasElement("xyz"));
EXPECT_EQ(xyz2, axisElem->Get<ignition::math::Vector3d>("xyz"));
// use_parent_model_frame is required in axis.proto
// so expect to to exist even if we don't set it
EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
// expect false (default sdformat value)
EXPECT_FALSE(axisElem->Get<bool>("use_parent_model_frame"));
EXPECT_TRUE(axisElem->HasElement("dynamics"));
auto axisDynamics = axisElem->GetElement("dynamics");
EXPECT_TRUE(axisDynamics->HasElement("damping"));
EXPECT_DOUBLE_EQ(damping2, axisDynamics->Get<double>("damping"));
EXPECT_TRUE(axisDynamics->HasElement("friction"));
EXPECT_DOUBLE_EQ(friction2, axisDynamics->Get<double>("friction"));
EXPECT_TRUE(axisElem->HasElement("limit"));
auto axisLimit = axisElem->GetElement("limit");
EXPECT_TRUE(axisLimit->HasElement("lower"));
EXPECT_DOUBLE_EQ(limit_lower2, axisLimit->Get<double>("lower"));
EXPECT_TRUE(axisLimit->HasElement("upper"));
EXPECT_DOUBLE_EQ(limit_upper2, axisLimit->Get<double>("upper"));
EXPECT_TRUE(axisLimit->HasElement("effort"));
EXPECT_DOUBLE_EQ(limit_effort2, axisLimit->Get<double>("effort"));
EXPECT_TRUE(axisLimit->HasElement("velocity"));
EXPECT_DOUBLE_EQ(limit_velocity2, axisLimit->Get<double>("velocity"));
}
EXPECT_TRUE(jointSDF->HasElement("physics"));
auto physicsElem = jointSDF->GetElement("physics");
EXPECT_TRUE(physicsElem->HasElement("ode"));
auto odePhysics = physicsElem->GetElement("ode");
EXPECT_TRUE(odePhysics->HasElement("cfm"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("cfm"), cfm);
EXPECT_TRUE(odePhysics->HasElement("bounce"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("bounce"), bounce);
EXPECT_TRUE(odePhysics->HasElement("velocity"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("velocity"), velocity);
EXPECT_TRUE(odePhysics->HasElement("fudge_factor"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("fudge_factor"), fudge_factor);
EXPECT_TRUE(odePhysics->HasElement("limit"));
auto limitElem = odePhysics->GetElement("limit");
EXPECT_TRUE(limitElem->HasElement("cfm"));
EXPECT_DOUBLE_EQ(limitElem->Get<double>("cfm"), limit_cfm);
EXPECT_TRUE(limitElem->HasElement("erp"));
EXPECT_DOUBLE_EQ(limitElem->Get<double>("erp"), limit_erp);
EXPECT_TRUE(odePhysics->HasElement("suspension"));
auto suspensionElem = odePhysics->GetElement("suspension");
EXPECT_TRUE(suspensionElem->HasElement("cfm"));
EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("cfm"), suspension_cfm);
EXPECT_TRUE(suspensionElem->HasElement("erp"));
EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("erp"), suspension_erp);
EXPECT_TRUE(jointSDF->HasElement("gearbox_reference_body"));
EXPECT_EQ(jointSDF->Get<std::string>("gearbox_reference_body"),
gearbox_reference_body);
EXPECT_TRUE(jointSDF->HasElement("gearbox_ratio"));
EXPECT_DOUBLE_EQ(jointSDF->Get<double>("gearbox_ratio"), gearbox_ratio);
}
// screw
{
const std::string name("test_screw_joint");
const msgs::Joint::Type type = msgs::Joint::SCREW;
const ignition::math::Pose3d pose(
ignition::math::Vector3d(2, 1, 3),
ignition::math::Quaterniond(0, 0, 0, 1));
const std::string parent("parent_screw_link");
const std::string child("child_screw_link");
const double cfm = 0.15;
const double bounce = 0.15;
const double velocity = 0.25;
const double fudge_factor = 0.45;
const double limit_cfm = 0.25;
const double limit_erp = 0.65;
const double suspension_cfm = 0.76;
const double suspension_erp = 0.46;
const ignition::math::Vector3d xyz1(0.0, 1.0, 1.0);
const double limit_lower1 = -1.2;
const double limit_upper1 = 2.2;
const double limit_effort1 = 1.2e2;
const double limit_velocity1 = 22;
const double damping1 = 2.1e-2;
const double friction1 = 2.6e2;
const bool useParentModelFrame1 = false;
const double thread_pitch = 0.9;
msgs::Joint jointMsg;
jointMsg.set_name(name);
jointMsg.set_type(type);
jointMsg.set_parent(parent);
jointMsg.set_child(child);
msgs::Set(jointMsg.mutable_pose(), pose);
jointMsg.set_cfm(cfm);
jointMsg.set_bounce(bounce);
jointMsg.set_velocity(velocity);
jointMsg.set_fudge_factor(fudge_factor);
jointMsg.set_limit_cfm(limit_cfm);
jointMsg.set_limit_erp(limit_erp);
jointMsg.set_suspension_cfm(suspension_cfm);
jointMsg.set_suspension_erp(suspension_erp);
{
auto axis1 = jointMsg.mutable_axis1();
msgs::Set(axis1->mutable_xyz(), xyz1);
axis1->set_limit_lower(limit_lower1);
axis1->set_limit_upper(limit_upper1);
axis1->set_limit_effort(limit_effort1);
axis1->set_limit_velocity(limit_velocity1);
axis1->set_damping(damping1);
axis1->set_friction(friction1);
axis1->set_use_parent_model_frame(useParentModelFrame1);
}
msgs::Joint::Screw *screwMsg = jointMsg.mutable_screw();
screwMsg->set_thread_pitch(thread_pitch);
sdf::ElementPtr jointSDF = msgs::JointToSDF(jointMsg);
EXPECT_TRUE(jointSDF->HasAttribute("name"));
EXPECT_EQ(jointSDF->Get<std::string>("name"), name);
EXPECT_TRUE(jointSDF->HasAttribute("type"));
EXPECT_STREQ(jointSDF->Get<std::string>("type").c_str(), "screw");
EXPECT_TRUE(jointSDF->HasElement("parent"));
EXPECT_EQ(jointSDF->Get<std::string>("parent"), parent);
EXPECT_TRUE(jointSDF->HasElement("child"));
EXPECT_EQ(jointSDF->Get<std::string>("child"), child);
EXPECT_TRUE(jointSDF->HasElement("pose"));
EXPECT_EQ(pose, jointSDF->Get<ignition::math::Pose3d>("pose"));
EXPECT_TRUE(jointSDF->HasElement("axis"));
{
auto axisElem = jointSDF->GetElement("axis");
EXPECT_TRUE(axisElem->HasElement("xyz"));
EXPECT_EQ(xyz1, axisElem->Get<ignition::math::Vector3d>("xyz"));
EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
EXPECT_EQ(useParentModelFrame1,
axisElem->Get<bool>("use_parent_model_frame"));
EXPECT_TRUE(axisElem->HasElement("dynamics"));
auto axisDynamics = axisElem->GetElement("dynamics");
EXPECT_TRUE(axisDynamics->HasElement("damping"));
EXPECT_DOUBLE_EQ(damping1, axisDynamics->Get<double>("damping"));
EXPECT_TRUE(axisDynamics->HasElement("friction"));
EXPECT_DOUBLE_EQ(friction1, axisDynamics->Get<double>("friction"));
EXPECT_TRUE(axisElem->HasElement("limit"));
auto axisLimit = axisElem->GetElement("limit");
EXPECT_TRUE(axisLimit->HasElement("lower"));
EXPECT_DOUBLE_EQ(limit_lower1, axisLimit->Get<double>("lower"));
EXPECT_TRUE(axisLimit->HasElement("upper"));
EXPECT_DOUBLE_EQ(limit_upper1, axisLimit->Get<double>("upper"));
EXPECT_TRUE(axisLimit->HasElement("effort"));
EXPECT_DOUBLE_EQ(limit_effort1, axisLimit->Get<double>("effort"));
EXPECT_TRUE(axisLimit->HasElement("velocity"));
EXPECT_DOUBLE_EQ(limit_velocity1, axisLimit->Get<double>("velocity"));
}
EXPECT_TRUE(jointSDF->HasElement("physics"));
auto physicsElem = jointSDF->GetElement("physics");
EXPECT_TRUE(physicsElem->HasElement("ode"));
auto odePhysics = physicsElem->GetElement("ode");
EXPECT_TRUE(odePhysics->HasElement("cfm"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("cfm"), cfm);
EXPECT_TRUE(odePhysics->HasElement("bounce"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("bounce"), bounce);
EXPECT_TRUE(odePhysics->HasElement("velocity"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("velocity"), velocity);
EXPECT_TRUE(odePhysics->HasElement("fudge_factor"));
EXPECT_DOUBLE_EQ(odePhysics->Get<double>("fudge_factor"), fudge_factor);
EXPECT_TRUE(odePhysics->HasElement("limit"));
auto limitElem = odePhysics->GetElement("limit");
EXPECT_TRUE(limitElem->HasElement("cfm"));
EXPECT_DOUBLE_EQ(limitElem->Get<double>("cfm"), limit_cfm);
EXPECT_TRUE(limitElem->HasElement("erp"));
EXPECT_DOUBLE_EQ(limitElem->Get<double>("erp"), limit_erp);
EXPECT_TRUE(odePhysics->HasElement("suspension"));
auto suspensionElem = odePhysics->GetElement("suspension");
EXPECT_TRUE(suspensionElem->HasElement("cfm"));
EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("cfm"), suspension_cfm);
EXPECT_TRUE(suspensionElem->HasElement("erp"));
EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("erp"), suspension_erp);
EXPECT_TRUE(jointSDF->HasElement("thread_pitch"));
EXPECT_DOUBLE_EQ(jointSDF->Get<double>("thread_pitch"), thread_pitch);
}
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, AddBoxLink)
{
msgs::Model model;
EXPECT_EQ(model.link_size(), 0);
const double mass = 1.0;
const ignition::math::Vector3d size(1, 1, 1);
msgs::AddBoxLink(model, mass, size);
EXPECT_EQ(model.link_size(), 1);
{
auto link = model.link(0);
EXPECT_EQ(link.name(), std::string("link_1"));
auto inertial = link.inertial();
EXPECT_DOUBLE_EQ(inertial.mass(), mass);
double ixx = inertial.ixx();
double iyy = inertial.iyy();
double izz = inertial.izz();
double ixy = inertial.ixy();
double ixz = inertial.ixz();
double iyz = inertial.iyz();
EXPECT_GT(ixx, 0.0);
EXPECT_GT(iyy, 0.0);
EXPECT_GT(izz, 0.0);
EXPECT_DOUBLE_EQ(ixy, 0.0);
EXPECT_DOUBLE_EQ(ixz, 0.0);
EXPECT_DOUBLE_EQ(iyz, 0.0);
// triangle inequality
EXPECT_GE(ixx + iyy, izz);
EXPECT_GE(iyy + izz, ixx);
EXPECT_GE(izz + ixx, iyy);
EXPECT_EQ(link.collision_size(), 1);
{
auto collision = link.collision(0);
auto geometry = collision.geometry();
EXPECT_EQ(geometry.type(), msgs::Geometry_Type_BOX);
EXPECT_EQ(msgs::ConvertIgn(geometry.box().size()), size);
}
EXPECT_EQ(link.visual_size(), 1);
{
auto visual = link.visual(0);
auto geometry = visual.geometry();
EXPECT_EQ(geometry.type(), msgs::Geometry_Type_BOX);
EXPECT_EQ(msgs::ConvertIgn(geometry.box().size()), size);
}
}
const double massRatio = 2.0;
msgs::AddBoxLink(model, mass*massRatio, size);
EXPECT_EQ(model.link_size(), 2);
{
auto link1 = model.link(0);
auto link2 = model.link(1);
EXPECT_EQ(link2.name(), std::string("link_2"));
auto inertial1 = link1.inertial();
auto inertial2 = link2.inertial();
EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
}
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, AddCylinderLink)
{
msgs::Model model;
EXPECT_EQ(model.link_size(), 0);
const double mass = 1.0;
const double radius = 0.5;
const double length = 2.5;
msgs::AddCylinderLink(model, mass, radius, length);
EXPECT_EQ(model.link_size(), 1);
{
auto link = model.link(0);
EXPECT_EQ(link.name(), std::string("link_1"));
auto inertial = link.inertial();
EXPECT_DOUBLE_EQ(inertial.mass(), mass);
double ixx = inertial.ixx();
double iyy = inertial.iyy();
double izz = inertial.izz();
double ixy = inertial.ixy();
double ixz = inertial.ixz();
double iyz = inertial.iyz();
EXPECT_GT(ixx, 0.0);
EXPECT_GT(iyy, 0.0);
EXPECT_GT(izz, 0.0);
EXPECT_DOUBLE_EQ(ixy, 0.0);
EXPECT_DOUBLE_EQ(ixz, 0.0);
EXPECT_DOUBLE_EQ(iyz, 0.0);
// triangle inequality
EXPECT_GE(ixx + iyy, izz);
EXPECT_GE(iyy + izz, ixx);
EXPECT_GE(izz + ixx, iyy);
EXPECT_EQ(link.collision_size(), 1);
{
auto collision = link.collision(0);
auto geometry = collision.geometry();
EXPECT_EQ(geometry.type(), msgs::Geometry_Type_CYLINDER);
EXPECT_DOUBLE_EQ(geometry.cylinder().radius(), radius);
EXPECT_DOUBLE_EQ(geometry.cylinder().length(), length);
}
EXPECT_EQ(link.visual_size(), 1);
{
auto visual = link.visual(0);
auto geometry = visual.geometry();
EXPECT_EQ(geometry.type(), msgs::Geometry_Type_CYLINDER);
EXPECT_DOUBLE_EQ(geometry.cylinder().radius(), radius);
EXPECT_DOUBLE_EQ(geometry.cylinder().length(), length);
}
}
const double massRatio = 2.0;
msgs::AddCylinderLink(model, mass*massRatio, radius, length);
EXPECT_EQ(model.link_size(), 2);
{
auto link1 = model.link(0);
auto link2 = model.link(1);
EXPECT_EQ(link2.name(), std::string("link_2"));
auto inertial1 = link1.inertial();
auto inertial2 = link2.inertial();
EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
}
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, AddSphereLink)
{
msgs::Model model;
EXPECT_EQ(model.link_size(), 0);
const double mass = 1.0;
const double radius = 0.5;
msgs::AddSphereLink(model, mass, radius);
EXPECT_EQ(model.link_size(), 1);
{
auto link = model.link(0);
EXPECT_EQ(link.name(), std::string("link_1"));
auto inertial = link.inertial();
EXPECT_DOUBLE_EQ(inertial.mass(), mass);
double ixx = inertial.ixx();
double iyy = inertial.iyy();
double izz = inertial.izz();
double ixy = inertial.ixy();
double ixz = inertial.ixz();
double iyz = inertial.iyz();
EXPECT_GT(ixx, 0.0);
EXPECT_GT(iyy, 0.0);
EXPECT_GT(izz, 0.0);
EXPECT_DOUBLE_EQ(ixy, 0.0);
EXPECT_DOUBLE_EQ(ixz, 0.0);
EXPECT_DOUBLE_EQ(iyz, 0.0);
// triangle inequality
EXPECT_GE(ixx + iyy, izz);
EXPECT_GE(iyy + izz, ixx);
EXPECT_GE(izz + ixx, iyy);
EXPECT_EQ(link.collision_size(), 1);
{
auto collision = link.collision(0);
auto geometry = collision.geometry();
EXPECT_EQ(geometry.type(), msgs::Geometry_Type_SPHERE);
EXPECT_DOUBLE_EQ(geometry.sphere().radius(), radius);
}
EXPECT_EQ(link.visual_size(), 1);
{
auto visual = link.visual(0);
auto geometry = visual.geometry();
EXPECT_EQ(geometry.type(), msgs::Geometry_Type_SPHERE);
EXPECT_DOUBLE_EQ(geometry.sphere().radius(), radius);
}
}
const double massRatio = 2.0;
msgs::AddSphereLink(model, mass*massRatio, radius);
EXPECT_EQ(model.link_size(), 2);
{
auto link1 = model.link(0);
auto link2 = model.link(1);
EXPECT_EQ(link2.name(), std::string("link_2"));
auto inertial1 = link1.inertial();
auto inertial2 = link2.inertial();
EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
}
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, ModelToSDF)
{
const std::string name("test_bicycle");
const ignition::math::Pose3d pose(
ignition::math::Vector3d(6, 1, 7),
ignition::math::Quaterniond(0.5, 0.5, 0.5, 0.5));
msgs::Model model;
model.set_name(name);
model.set_is_static(false);
msgs::Set(model.mutable_pose(), pose);
EXPECT_EQ(model.link_size(), 0);
EXPECT_EQ(model.joint_size(), 0);
// This will be a bicycle with two wheels.
// The frame is a box.
const double length = 1.5;
const double height = 0.9;
const double width = 0.1;
const ignition::math::Vector3d boxSize(length, width, height);
const double boxMass = 4.0;
AddBoxLink(model, boxMass, boxSize);
ASSERT_EQ(model.link_size(), 1);
EXPECT_EQ(model.joint_size(), 0);
model.mutable_link(0)->set_name("frame");
// The rear wheel is a cylinder.
const double radius = height / 2;
AddCylinderLink(model, 0.5, radius, radius);
ASSERT_EQ(model.link_size(), 2);
EXPECT_EQ(model.joint_size(), 0);
const ignition::math::Pose3d cylinderPose(
-length/2, 0, -height/2, M_PI/2, 0, 0);
{
auto link = model.mutable_link(1);
msgs::Set(link->mutable_pose(), cylinderPose);
link->set_name("rear_wheel");
}
// The front wheel is a sphere.
AddSphereLink(model, 0.5, radius);
ASSERT_EQ(model.link_size(), 3);
EXPECT_EQ(model.joint_size(), 0);
const ignition::math::Pose3d spherePose(length/2, 0, -height/2, 0, 0, 0);
{
auto link = model.mutable_link(2);
msgs::Set(link->mutable_pose(), spherePose);
link->set_name("front_wheel");
}
// Add revolute joints for the wheels.
// Front wheel joint
model.add_joint();
ASSERT_EQ(model.joint_size(), 1);
auto frontJoint = model.mutable_joint(0);
frontJoint->set_name("front_hinge");
frontJoint->set_type(msgs::ConvertJointType("revolute"));
frontJoint->set_parent("frame");
frontJoint->set_child("front_wheel");
const ignition::math::Vector3d frontAxis(0, 1, 0);
msgs::Set(frontJoint->mutable_axis1()->mutable_xyz(), frontAxis);
// Rear wheel joint
model.add_joint();
ASSERT_EQ(model.joint_size(), 2);
auto rearJoint = model.mutable_joint(1);
rearJoint->set_name("rear_hinge");
rearJoint->set_type(msgs::ConvertJointType("revolute"));
rearJoint->set_parent("frame");
rearJoint->set_child("rear_wheel");
const ignition::math::Vector3d rearAxis(0, 0, 1);
msgs::Set(rearJoint->mutable_axis1()->mutable_xyz(), rearAxis);
sdf::ElementPtr modelSDF = msgs::ModelToSDF(model);
EXPECT_EQ(modelSDF->Get<std::string>("name"), name);
EXPECT_FALSE(modelSDF->Get<bool>("static"));
EXPECT_EQ(pose, modelSDF->Get<ignition::math::Pose3d>("pose"));
sdf::ElementPtr linkElem1 = modelSDF->GetElement("link");
EXPECT_EQ(linkElem1->Get<std::string>("name"), "frame");
EXPECT_EQ(linkElem1->Get<ignition::math::Pose3d>("pose"),
ignition::math::Pose3d());
sdf::ElementPtr linkElem2 = linkElem1->GetNextElement("link");
EXPECT_EQ(linkElem2->Get<std::string>("name"), "rear_wheel");
EXPECT_EQ(linkElem2->Get<ignition::math::Pose3d>("pose"), cylinderPose);
sdf::ElementPtr linkElem3 = linkElem2->GetNextElement("link");
EXPECT_EQ(linkElem3->Get<std::string>("name"), "front_wheel");
EXPECT_EQ(linkElem3->Get<ignition::math::Pose3d>("pose"), spherePose);
sdf::ElementPtr jointElem1 = modelSDF->GetElement("joint");
EXPECT_EQ(jointElem1->Get<std::string>("name"), "front_hinge");
EXPECT_EQ(jointElem1->Get<std::string>("type"), "revolute");
EXPECT_EQ(jointElem1->Get<ignition::math::Pose3d>("pose"),
ignition::math::Pose3d());
sdf::ElementPtr jointElem2 = jointElem1->GetNextElement("joint");
EXPECT_EQ(jointElem2->Get<std::string>("name"), "rear_hinge");
EXPECT_EQ(jointElem2->Get<std::string>("type"), "revolute");
EXPECT_EQ(jointElem2->Get<ignition::math::Pose3d>("pose"),
ignition::math::Pose3d());
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, PluginToSDF)
{
std::string name = "plugin_name";
std::string filename = "plugin_filename";
std::string innerxml = "<plugin_param>param</plugin_param>";
msgs::Plugin msg;
msg.set_name(name);
msg.set_filename(filename);
msg.set_innerxml(innerxml);
sdf::ElementPtr pluginSDF = msgs::PluginToSDF(msg);
EXPECT_TRUE(pluginSDF->HasAttribute("name"));
EXPECT_EQ(pluginSDF->Get<std::string>("name"), name);
EXPECT_TRUE(pluginSDF->HasAttribute("filename"));
EXPECT_EQ(pluginSDF->Get<std::string>("filename"), filename);
EXPECT_TRUE(pluginSDF->HasElement("plugin_param"));
EXPECT_EQ(pluginSDF->Get<std::string>("plugin_param"), "param");
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, PluginToFromSDF)
{
std::string name = "plugin_name";
std::string filename = "plugin_filename";
std::string innerxml = "<plugin_param>param</plugin_param>\n";
// Create message
msgs::Plugin msg1;
msg1.set_name(name);
msg1.set_filename(filename);
msg1.set_innerxml(innerxml);
// To SDF
sdf::ElementPtr sdf1 = msgs::PluginToSDF(msg1);
// Back to Msg
msgs::Plugin msg2 = msgs::PluginFromSDF(sdf1);
EXPECT_EQ(msg2.name(), name);
EXPECT_EQ(msg2.filename(), filename);
EXPECT_EQ(msg2.innerxml(), innerxml);
// Back to SDF
sdf::ElementPtr sdf2;
sdf2.reset(new sdf::Element);
sdf::initFile("plugin.sdf", sdf2);
msgs::PluginToSDF(msg2, sdf2);
EXPECT_TRUE(sdf2 != nullptr);
EXPECT_EQ(sdf2->Get<std::string>("name"), name);
EXPECT_EQ(sdf2->Get<std::string>("filename"), filename);
EXPECT_TRUE(sdf2->HasElement("plugin_param"));
EXPECT_EQ(sdf2->Get<std::string>("plugin_param"), "param");
}
/////////////////////////////////////////////////
TEST_F(MsgsTest, VisualPluginToFromSDF)
{
const std::string visName("visual_name");
const double radius = 3.3;
std::string pluginName("plugin_name");
std::string filename("plugin_filename");
std::string innerxml("<plugin_param>param</plugin_param>\n");
// Create SDF
sdf::ElementPtr sdf1(new sdf::Element());
sdf::initFile("visual.sdf", sdf1);
ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='" + visName + "'>\
<geometry>\
<sphere><radius>" + std::to_string(radius) + "</radius></sphere>\
</geometry>\
<plugin name='" + pluginName + "' filename='" + filename + "'>\
" + innerxml + "\
</plugin>\
</visual>\
</sdf>", sdf1));
// To msg
auto msg1 = msgs::VisualFromSDF(sdf1);
// Back to SDF
auto sdf2 = msgs::VisualToSDF(msg1);
ASSERT_TRUE(sdf2 != nullptr);
// DEPRECATED: Remove this test in Gazebo8
{
EXPECT_TRUE(sdf2->HasElement("plugin"));
EXPECT_TRUE(sdf2->GetElement("plugin")->HasElement("plugin_param"));
EXPECT_TRUE(sdf2->GetElement("plugin")->HasElement("sdf"));
EXPECT_TRUE(sdf2->GetElement("plugin")->GetElement("sdf")->
HasElement("plugin_param"));
}
// Back to Msg
auto msg2 = msgs::VisualFromSDF(sdf2);
// Back to SDF
auto sdf3 = msgs::VisualToSDF(msg2);
ASSERT_TRUE(sdf3 != nullptr);
ASSERT_TRUE(sdf3->HasAttribute("name"));
EXPECT_EQ(sdf3->Get<std::string>("name"), visName);
ASSERT_TRUE(sdf3->HasElement("geometry"));
auto geomElem = sdf3->GetElement("geometry");
ASSERT_TRUE(geomElem->HasElement("sphere"));
auto sphereElem = geomElem->GetElement("sphere");
EXPECT_TRUE(sphereElem->HasElement("radius"));
EXPECT_DOUBLE_EQ(sphereElem->Get<double>("radius"), radius);
ASSERT_TRUE(sdf3->HasElement("plugin"));
auto pluginElem = sdf3->GetElement("plugin");
EXPECT_EQ(pluginElem->Get<std::string>("name"), pluginName);
EXPECT_EQ(pluginElem->Get<std::string>("filename"), filename);
EXPECT_TRUE(pluginElem->HasElement("plugin_param"));
EXPECT_EQ(pluginElem->Get<std::string>("plugin_param"), "param");
}