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

2838 lines
86 KiB
C++
Raw Permalink Blame History

/*
* 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 <google/protobuf/descriptor.h>
#include <algorithm>
#include <ignition/math/MassMatrix3.hh>
#include <ignition/math/Rand.hh>
#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Image.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/msgs/msgs.hh"
namespace gazebo
{
namespace msgs
{
/// \internal
/// \brief Internal function to create an SDF element from msgs::Axis.
/// It is only intended to be used by JointToSDF.
/// \param[in] _msg The msgs::Axis object.
/// \param[in] _sdf sdf::ElementPtr to fill with data.
void AxisToSDF(const msgs::Axis &_msg, sdf::ElementPtr _sdf);
/////////////////////////////////////////////
/// Create a request message
msgs::Request *CreateRequest(const std::string &_request,
const std::string &_data)
{
msgs::Request *request = new msgs::Request;
request->set_request(_request);
request->set_data(_data);
request->set_id(ignition::math::Rand::IntUniform(1, 10000));
return request;
}
/////////////////////////////////////////////
const google::protobuf::FieldDescriptor *GetFD(
google::protobuf::Message &message, const std::string &name)
{
return message.GetDescriptor()->FindFieldByName(name);
}
/////////////////////////////////////////////
msgs::Header *GetHeader(google::protobuf::Message &message)
{
google::protobuf::Message *msg = NULL;
if (GetFD(message, "str_id"))
msg = &message;
else
{
const google::protobuf::FieldDescriptor *fd;
fd = GetFD(message, "header");
if (fd)
msg = message.GetReflection()->MutableMessage(&message, fd);
}
return (msgs::Header*)msg;
}
/////////////////////////////////////////////
void Init(google::protobuf::Message &_message, const std::string &_id)
{
msgs::Header *header = GetHeader(_message);
if (header)
{
if (!_id.empty())
header->set_str_id(_id);
Stamp(header->mutable_stamp());
}
}
/////////////////////////////////////////////
void Stamp(msgs::Header *_hdr)
{
Stamp(_hdr->mutable_stamp());
}
/////////////////////////////////////////////
void Stamp(msgs::Time *_time)
{
common::Time tm = common::Time::GetWallTime();
_time->set_sec(tm.sec);
_time->set_nsec(tm.nsec);
}
/////////////////////////////////////////////
std::string Package(const std::string &type,
const google::protobuf::Message &message)
{
std::string data;
msgs::Packet pkg;
Stamp(pkg.mutable_stamp());
pkg.set_type(type);
std::string *serialized_data = pkg.mutable_serialized_data();
if (!message.IsInitialized())
gzthrow("Can't serialize message of type[" + message.GetTypeName() +
"] because it is missing required fields");
if (!message.SerializeToString(serialized_data))
gzthrow("Failed to serialized message");
if (!pkg.SerializeToString(&data))
gzthrow("Failed to serialized message");
return data;
}
/////////////////////////////////////////////
void Set(msgs::Vector3d *_pt, const ignition::math::Vector3d &_v)
{
_pt->set_x(_v.X());
_pt->set_y(_v.Y());
_pt->set_z(_v.Z());
}
/////////////////////////////////////////////
void Set(msgs::Vector2d *_pt, const ignition::math::Vector2d &_v)
{
_pt->set_x(_v.X());
_pt->set_y(_v.Y());
}
/////////////////////////////////////////////
void Set(msgs::Quaternion *_q, const ignition::math::Quaterniond &_v)
{
_q->set_x(_v.X());
_q->set_y(_v.Y());
_q->set_z(_v.Z());
_q->set_w(_v.W());
}
/////////////////////////////////////////////
void Set(msgs::Pose *_p, const ignition::math::Pose3d &_v)
{
Set(_p->mutable_position(), _v.Pos());
Set(_p->mutable_orientation(), _v.Rot());
}
/////////////////////////////////////////////
void Set(msgs::Color *_c, const common::Color &_v)
{
_c->set_r(_v.r);
_c->set_g(_v.g);
_c->set_b(_v.b);
_c->set_a(_v.a);
}
/////////////////////////////////////////////
void Set(msgs::Time *_t, const common::Time &_v)
{
_t->set_sec(_v.sec);
_t->set_nsec(_v.nsec);
}
/////////////////////////////////////////////////
void Set(msgs::SphericalCoordinates *_s,
const common::SphericalCoordinates &_v)
{
switch (_v.GetSurfaceType())
{
case common::SphericalCoordinates::EARTH_WGS84:
_s->set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84);
break;
default:
gzerr << "Unable to map surface type[" << _v.GetSurfaceType()
<< "] to a SphericalCoordinates message.\n";
_s->set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84);
break;
};
_s->set_latitude_deg(_v.LatitudeReference().Degree());
_s->set_longitude_deg(_v.LongitudeReference().Degree());
_s->set_heading_deg(_v.HeadingOffset().Degree());
_s->set_elevation(_v.GetElevationReference());
}
/////////////////////////////////////////////////
void Set(msgs::Inertial *_i, const ignition::math::MassMatrix3d &_m)
{
_i->set_mass(_m.Mass());
_i->set_ixx(_m.IXX());
_i->set_iyy(_m.IYY());
_i->set_izz(_m.IZZ());
_i->set_ixy(_m.IXY());
_i->set_ixz(_m.IXZ());
_i->set_iyz(_m.IYZ());
}
/////////////////////////////////////////////////
void Set(msgs::Inertial *_i, const ignition::math::Inertiald &_m)
{
msgs::Set(_i, _m.MassMatrix());
msgs::Set(_i->mutable_pose(), _m.Pose());
}
/////////////////////////////////////////////////
void Set(msgs::PlaneGeom *_p, const ignition::math::Planed &_v)
{
Set(_p->mutable_normal(), _v.Normal());
_p->mutable_size()->set_x(_v.Size().X());
_p->mutable_size()->set_y(_v.Size().Y());
_p->set_d(_v.Offset());
}
/////////////////////////////////////////////////
void Set(common::Image &_img, const msgs::Image &_msg)
{
_img.SetFromData(
(const unsigned char*)_msg.data().data(),
_msg.width(),
_msg.height(),
(common::Image::PixelFormat)(_msg.pixel_format()));
}
/////////////////////////////////////////////////
void Set(msgs::Image *_msg, const common::Image &_i)
{
_msg->set_width(_i.GetWidth());
_msg->set_height(_i.GetHeight());
_msg->set_pixel_format(_i.GetPixelFormat());
_msg->set_step(_i.GetPitch());
unsigned char *data = NULL;
unsigned int size;
_i.GetData(&data, size);
_msg->set_data(data, size);
if (data)
{
delete[] data;
}
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const double _d)
{
msgs::Any result;
result.set_type(msgs::Any::DOUBLE);
result.set_double_value(_d);
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const int _i)
{
msgs::Any result;
result.set_type(msgs::Any::INT32);
result.set_int_value(_i);
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const std::string &_s)
{
msgs::Any result;
result.set_type(msgs::Any::STRING);
result.set_string_value(_s);
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const char *_s)
{
msgs::Any result;
result.set_type(msgs::Any::STRING);
result.set_string_value(std::string(_s));
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const bool _b)
{
msgs::Any result;
result.set_type(msgs::Any::BOOLEAN);
result.set_bool_value(_b);
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const ignition::math::Vector3d &_v)
{
msgs::Any result;
result.set_type(msgs::Any::VECTOR3D);
result.mutable_vector3d_value()->CopyFrom(Convert(_v));
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const common::Color &_c)
{
msgs::Any result;
result.set_type(msgs::Any::COLOR);
result.mutable_color_value()->CopyFrom(Convert(_c));
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const ignition::math::Pose3d &_p)
{
msgs::Any result;
result.set_type(msgs::Any::POSE3D);
result.mutable_pose3d_value()->CopyFrom(Convert(_p));
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const ignition::math::Quaterniond &_q)
{
msgs::Any result;
result.set_type(msgs::Any::QUATERNIOND);
result.mutable_quaternion_value()->CopyFrom(Convert(_q));
return result;
}
/////////////////////////////////////////////////
msgs::Any ConvertAny(const common::Time &_t)
{
msgs::Any result;
result.set_type(msgs::Any::TIME);
result.mutable_time_value()->CopyFrom(Convert(_t));
return result;
}
/////////////////////////////////////////////////
msgs::Vector3d Convert(const ignition::math::Vector3d &_v)
{
msgs::Vector3d result;
result.set_x(_v.X());
result.set_y(_v.Y());
result.set_z(_v.Z());
return result;
}
/////////////////////////////////////////////////
msgs::Vector2d Convert(const ignition::math::Vector2d &_v)
{
msgs::Vector2d result;
result.set_x(_v.X());
result.set_y(_v.Y());
return result;
}
/////////////////////////////////////////////
msgs::Quaternion Convert(const ignition::math::Quaterniond &_q)
{
msgs::Quaternion result;
result.set_x(_q.X());
result.set_y(_q.Y());
result.set_z(_q.Z());
result.set_w(_q.W());
return result;
}
/////////////////////////////////////////////
msgs::Pose Convert(const ignition::math::Pose3d &_p)
{
msgs::Pose result;
result.mutable_position()->CopyFrom(Convert(_p.Pos()));
result.mutable_orientation()->CopyFrom(Convert(_p.Rot()));
return result;
}
/////////////////////////////////////////////
msgs::Color Convert(const common::Color &_c)
{
msgs::Color result;
result.set_r(_c.r);
result.set_g(_c.g);
result.set_b(_c.b);
result.set_a(_c.a);
return result;
}
/////////////////////////////////////////////
msgs::Time Convert(const common::Time &_t)
{
msgs::Time result;
result.set_sec(_t.sec);
result.set_nsec(_t.nsec);
return result;
}
/////////////////////////////////////////////
msgs::Inertial Convert(const ignition::math::Inertiald &_i)
{
msgs::Inertial result;
msgs::Set(&result, _i);
return result;
}
/////////////////////////////////////////////
msgs::Inertial Convert(const ignition::math::MassMatrix3d &_m)
{
msgs::Inertial result;
msgs::Set(&result, _m);
return result;
}
/////////////////////////////////////////////
msgs::PlaneGeom Convert(const ignition::math::Planed &_p)
{
msgs::PlaneGeom result;
result.mutable_normal()->CopyFrom(Convert(_p.Normal()));
result.mutable_size()->set_x(_p.Size().X());
result.mutable_size()->set_y(_p.Size().Y());
result.set_d(_p.Offset());
return result;
}
/////////////////////////////////////////////
msgs::Joint::Type ConvertJointType(const std::string &_str)
{
msgs::Joint::Type result = msgs::Joint::REVOLUTE;
if (_str == "revolute")
{
result = msgs::Joint::REVOLUTE;
}
else if (_str == "revolute2")
{
result = msgs::Joint::REVOLUTE2;
}
else if (_str == "prismatic")
{
result = msgs::Joint::PRISMATIC;
}
else if (_str == "universal")
{
result = msgs::Joint::UNIVERSAL;
}
else if (_str == "ball")
{
result = msgs::Joint::BALL;
}
else if (_str == "screw")
{
result = msgs::Joint::SCREW;
}
else if (_str == "gearbox")
{
result = msgs::Joint::GEARBOX;
}
else if (_str == "fixed")
{
result = msgs::Joint::FIXED;
}
else
{
gzerr << "Unrecognized JointType ["
<< _str
<< "], returning REVOLUTE"
<< std::endl;
}
return result;
}
/////////////////////////////////////////////
std::string ConvertJointType(const msgs::Joint::Type &_type)
{
std::string result;
switch (_type)
{
case msgs::Joint::REVOLUTE:
{
result = "revolute";
break;
}
case msgs::Joint::REVOLUTE2:
{
result = "revolute2";
break;
}
case msgs::Joint::PRISMATIC:
{
result = "prismatic";
break;
}
case msgs::Joint::UNIVERSAL:
{
result = "universal";
break;
}
case msgs::Joint::BALL:
{
result = "ball";
break;
}
case msgs::Joint::SCREW:
{
result = "screw";
break;
}
case msgs::Joint::GEARBOX:
{
result = "gearbox";
break;
}
case msgs::Joint::FIXED:
{
result = "fixed";
break;
}
default:
{
result = "unknown";
gzerr << "Unrecognized JointType [" << _type << "]"
<< std::endl;
break;
}
}
return result;
}
/////////////////////////////////////////////////
msgs::Geometry::Type ConvertGeometryType(const std::string &_str)
{
msgs::Geometry::Type result = msgs::Geometry::BOX;
if (_str == "box")
{
result = msgs::Geometry::BOX;
}
else if (_str == "cylinder")
{
result = msgs::Geometry::CYLINDER;
}
else if (_str == "sphere")
{
result = msgs::Geometry::SPHERE;
}
else if (_str == "plane")
{
result = msgs::Geometry::PLANE;
}
else if (_str == "image")
{
result = msgs::Geometry::IMAGE;
}
else if (_str == "heightmap")
{
result = msgs::Geometry::HEIGHTMAP;
}
else if (_str == "mesh")
{
result = msgs::Geometry::MESH;
}
else if (_str == "polyline")
{
result = msgs::Geometry::POLYLINE;
}
else
{
gzwarn << "Geometry: '" << _str << "' is not recognized, "
<< " returning type as msgs::Geometry::BOX." << std::endl;
}
return result;
}
/////////////////////////////////////////////////
std::string ConvertGeometryType(const msgs::Geometry::Type _type)
{
std::string result;
switch (_type)
{
case msgs::Geometry::BOX:
{
result = "box";
break;
}
case msgs::Geometry::CYLINDER:
{
result = "cylinder";
break;
}
case msgs::Geometry::SPHERE:
{
result = "sphere";
break;
}
case msgs::Geometry::PLANE:
{
result = "plane";
break;
}
case msgs::Geometry::IMAGE:
{
result = "image";
break;
}
case msgs::Geometry::HEIGHTMAP:
{
result = "heightmap";
break;
}
case msgs::Geometry::MESH:
{
result = "mesh";
break;
}
case msgs::Geometry::POLYLINE:
{
result = "polyline";
break;
}
default:
{
result = "unknown";
break;
}
}
return result;
}
/////////////////////////////////////////////
ignition::math::Vector3d ConvertIgn(const msgs::Vector3d &_v)
{
return ignition::math::Vector3d(_v.x(), _v.y(), _v.z());
}
/////////////////////////////////////////////
ignition::math::Vector2d ConvertIgn(const msgs::Vector2d &_v)
{
return ignition::math::Vector2d(_v.x(), _v.y());
}
/////////////////////////////////////////////
ignition::math::Quaterniond ConvertIgn(const msgs::Quaternion &_q)
{
return ignition::math::Quaterniond(_q.w(), _q.x(), _q.y(), _q.z());
}
/////////////////////////////////////////////
ignition::math::Pose3d ConvertIgn(const msgs::Pose &_p)
{
return ignition::math::Pose3d(ConvertIgn(_p.position()),
ConvertIgn(_p.orientation()));
}
/////////////////////////////////////////////
ignition::math::Inertiald Convert(const msgs::Inertial &_i)
{
auto pose = msgs::ConvertIgn(_i.pose());
return ignition::math::Inertiald(
ignition::math::MassMatrix3d(
_i.mass(),
ignition::math::Vector3d(_i.ixx(), _i.iyy(), _i.izz()),
ignition::math::Vector3d(_i.ixy(), _i.ixz(), _i.iyz())),
pose);
}
/////////////////////////////////////////////
common::Color Convert(const msgs::Color &_c)
{
return common::Color(_c.r(), _c.g(), _c.b(), _c.a());
}
/////////////////////////////////////////////
common::Time Convert(const msgs::Time &_t)
{
return common::Time(_t.sec(), _t.nsec());
}
/////////////////////////////////////////////
ignition::math::Planed ConvertIgn(const msgs::PlaneGeom &_p)
{
return ignition::math::Planed(ConvertIgn(_p.normal()),
ignition::math::Vector2d(_p.size().x(), _p.size().y()),
_p.d());
}
/////////////////////////////////////////////
msgs::GUI GUIFromSDF(sdf::ElementPtr _sdf)
{
msgs::GUI result;
result.set_fullscreen(_sdf->Get<bool>("fullscreen"));
// Set gui plugins
if (_sdf->HasElement("plugin"))
{
sdf::ElementPtr pluginElem = _sdf->GetElement("plugin");
while (pluginElem)
{
msgs::Plugin *pluginMsg = result.add_plugin();
pluginMsg->CopyFrom(PluginFromSDF(pluginElem));
pluginElem = pluginElem->GetNextElement("plugin");
}
}
if (_sdf->HasElement("camera"))
{
sdf::ElementPtr camSDF = _sdf->GetElement("camera");
msgs::GUICamera *guiCam = result.mutable_camera();
guiCam->set_name(camSDF->Get<std::string>("name"));
if (camSDF->HasElement("pose"))
{
msgs::Set(guiCam->mutable_pose(),
camSDF->Get<ignition::math::Pose3d>("pose"));
}
if (camSDF->HasElement("view_controller"))
{
guiCam->set_view_controller(
camSDF->Get<std::string>("view_controller"));
}
if (camSDF->HasElement("projection_type"))
{
guiCam->set_projection_type(
camSDF->Get<std::string>("projection_type"));
}
if (camSDF->HasElement("track_visual"))
{
guiCam->mutable_track()->CopyFrom(
TrackVisualFromSDF(camSDF->GetElement("track_visual")));
}
}
return result;
}
/////////////////////////////////////////////
msgs::Plugin PluginFromSDF(const sdf::ElementPtr _sdf)
{
msgs::Plugin result;
result.set_name(_sdf->Get<std::string>("name"));
result.set_filename(_sdf->Get<std::string>("filename"));
std::stringstream ss;
for (sdf::ElementPtr innerElem = _sdf->GetFirstElement();
innerElem; innerElem = innerElem->GetNextElement(""))
{
ss << innerElem->ToString("");
}
result.set_innerxml(ss.str());
return result;
}
/////////////////////////////////////////////////
msgs::TrackVisual TrackVisualFromSDF(sdf::ElementPtr _sdf)
{
msgs::TrackVisual result;
result.set_name(_sdf->Get<std::string>("name"));
if (_sdf->HasElement("min_dist"))
result.set_min_dist(_sdf->GetElement("min_dist")->Get<double>());
if (_sdf->HasElement("max_dist"))
result.set_max_dist(_sdf->GetElement("max_dist")->Get<double>());
return result;
}
/////////////////////////////////////////////////
msgs::Light LightFromSDF(sdf::ElementPtr _sdf)
{
msgs::Light result;
std::string type = _sdf->Get<std::string>("type");
std::transform(type.begin(), type.end(), type.begin(), ::tolower);
result.set_name(_sdf->Get<std::string>("name"));
result.set_cast_shadows(_sdf->Get<bool>("cast_shadows"));
if (type == "point")
result.set_type(msgs::Light::POINT);
else if (type == "spot")
result.set_type(msgs::Light::SPOT);
else if (type == "directional")
result.set_type(msgs::Light::DIRECTIONAL);
if (_sdf->HasElement("pose"))
{
result.mutable_pose()->CopyFrom(
Convert(_sdf->Get<ignition::math::Pose3d>("pose")));
}
if (_sdf->HasElement("diffuse"))
{
result.mutable_diffuse()->CopyFrom(
Convert(_sdf->Get<common::Color>("diffuse")));
}
if (_sdf->HasElement("specular"))
{
result.mutable_specular()->CopyFrom(
Convert(_sdf->Get<common::Color>("specular")));
}
if (_sdf->HasElement("attenuation"))
{
sdf::ElementPtr elem = _sdf->GetElement("attenuation");
result.set_attenuation_constant(elem->Get<double>("constant"));
result.set_attenuation_linear(elem->Get<double>("linear"));
result.set_attenuation_quadratic(elem->Get<double>("quadratic"));
result.set_range(elem->Get<double>("range"));
}
if (_sdf->HasElement("direction"))
{
result.mutable_direction()->CopyFrom(
Convert(_sdf->Get<ignition::math::Vector3d>("direction")));
}
if (_sdf->HasElement("spot"))
{
sdf::ElementPtr elem = _sdf->GetElement("spot");
result.set_spot_inner_angle(elem->Get<double>("inner_angle"));
result.set_spot_outer_angle(elem->Get<double>("outer_angle"));
result.set_spot_falloff(elem->Get<double>("falloff"));
}
return result;
}
/////////////////////////////////////////////////
msgs::MeshGeom MeshFromSDF(sdf::ElementPtr _sdf)
{
msgs::MeshGeom result;
if (_sdf->GetName() != "mesh")
{
gzerr << "Cannot create a mesh message from an "
<< _sdf->GetName() << " SDF element.\n";
return result;
}
msgs::Set(result.mutable_scale(),
_sdf->Get<ignition::math::Vector3d>("scale"));
result.set_filename(_sdf->Get<std::string>("uri"));
if (_sdf->HasElement("submesh"))
{
sdf::ElementPtr submeshElem = _sdf->GetElement("submesh");
if (submeshElem->HasElement("name") &&
submeshElem->Get<std::string>("name") != "__default__")
{
result.set_submesh(submeshElem->Get<std::string>("name"));
if (submeshElem->HasElement("center"))
result.set_center_submesh(submeshElem->Get<bool>("center"));
}
}
return result;
}
/////////////////////////////////////////////////
msgs::Geometry GeometryFromSDF(sdf::ElementPtr _sdf)
{
msgs::Geometry result;
if (_sdf->GetName() != "geometry")
{
gzerr << "Cannot create a geometry message from an "
<< _sdf->GetName() << " SDF element.\n";
return result;
}
// Load the geometry
sdf::ElementPtr geomElem = _sdf->GetFirstElement();
if (!geomElem)
gzthrow("Invalid geometry element");
if (geomElem->GetName() == "box")
{
result.set_type(msgs::Geometry::BOX);
msgs::Set(result.mutable_box()->mutable_size(),
geomElem->Get<ignition::math::Vector3d>("size"));
}
else if (geomElem->GetName() == "cylinder")
{
result.set_type(msgs::Geometry::CYLINDER);
result.mutable_cylinder()->set_radius(
geomElem->Get<double>("radius"));
result.mutable_cylinder()->set_length(
geomElem->Get<double>("length"));
}
else if (geomElem->GetName() == "sphere")
{
result.set_type(msgs::Geometry::SPHERE);
result.mutable_sphere()->set_radius(
geomElem->Get<double>("radius"));
}
else if (geomElem->GetName() == "plane")
{
result.set_type(msgs::Geometry::PLANE);
msgs::Set(result.mutable_plane()->mutable_normal(),
geomElem->Get<ignition::math::Vector3d>("normal"));
msgs::Set(result.mutable_plane()->mutable_size(),
geomElem->Get<ignition::math::Vector2d>("size"));
}
else if (geomElem->GetName() == "polyline")
{
sdf::ElementPtr polylineElem = geomElem;
result.set_type(msgs::Geometry::POLYLINE);
while (polylineElem)
{
msgs::Polyline *polylineMsg = result.add_polyline();
polylineMsg->set_height(polylineElem->Get<double>("height"));
sdf::ElementPtr pointElem = polylineElem->GetElement("point");
while (pointElem)
{
ignition::math::Vector2d point =
pointElem->Get<ignition::math::Vector2d>();
pointElem = pointElem->GetNextElement("point");
msgs::Vector2d *ptMsg = polylineMsg->add_point();
msgs::Set(ptMsg, point);
}
polylineElem = polylineElem->GetNextElement("polyline");
}
}
else if (geomElem->GetName() == "image")
{
result.set_type(msgs::Geometry::IMAGE);
result.mutable_image()->set_scale(
geomElem->Get<double>("scale"));
result.mutable_image()->set_height(
geomElem->Get<double>("height"));
result.mutable_image()->set_uri(
geomElem->Get<std::string>("uri"));
}
else if (geomElem->GetName() == "heightmap")
{
result.set_type(msgs::Geometry::HEIGHTMAP);
// We do not want to set the size field to be the default values of
// [1, 1, 1] if not specified (size is optional for DEMs). So mark it as
// zero for now.
// TODO remove the required rule in heightmapgeom.proto's size field
ignition::math::Vector3d size;
if (geomElem->HasElement("size"))
size = geomElem->Get<ignition::math::Vector3d>("size");
msgs::Set(result.mutable_heightmap()->mutable_size(), size);
msgs::Set(result.mutable_heightmap()->mutable_origin(),
geomElem->Get<ignition::math::Vector3d>("pos"));
if (geomElem->HasElement("sampling"))
{
result.mutable_heightmap()->set_sampling(
geomElem->Get<unsigned int>("sampling"));
}
sdf::ElementPtr textureElem = geomElem->GetElement("texture");
while (textureElem)
{
msgs::HeightmapGeom::Texture *tex =
result.mutable_heightmap()->add_texture();
tex->set_diffuse(textureElem->Get<std::string>("diffuse"));
tex->set_normal(textureElem->Get<std::string>("normal"));
tex->set_size(textureElem->Get<double>("size"));
textureElem = textureElem->GetNextElement("texture");
}
sdf::ElementPtr blendElem = geomElem->GetElement("blend");
while (blendElem)
{
msgs::HeightmapGeom::Blend *blend =
result.mutable_heightmap()->add_blend();
blend->set_min_height(blendElem->Get<double>("min_height"));
blend->set_fade_dist(blendElem->Get<double>("fade_dist"));
blendElem = blendElem->GetNextElement("blend");
}
// Set if the rendering engine uses terrain paging
bool useTerrainPaging =
geomElem->Get<bool>("use_terrain_paging");
result.mutable_heightmap()->set_use_terrain_paging(useTerrainPaging);
result.mutable_heightmap()->set_filename(
geomElem->Get<std::string>("uri"));
}
else if (geomElem->GetName() == "mesh")
{
result.set_type(msgs::Geometry::MESH);
result.mutable_mesh()->CopyFrom(MeshFromSDF(geomElem));
}
else if (geomElem->GetName() == "empty")
{
result.set_type(msgs::Geometry::EMPTY);
}
else
gzthrow("Unknown geometry type\n");
return result;
}
/////////////////////////////////////////////////
msgs::Visual VisualFromSDF(sdf::ElementPtr _sdf)
{
msgs::Visual result;
result.set_name(_sdf->Get<std::string>("name"));
if (_sdf->HasElement("cast_shadows"))
result.set_cast_shadows(_sdf->Get<bool>("cast_shadows"));
if (_sdf->HasElement("transparency"))
result.set_transparency(_sdf->Get<double>("transparency"));
if (_sdf->HasElement("laser_retro"))
result.set_laser_retro(_sdf->Get<double>("laser_retro"));
// Set the meta information
if (_sdf->HasElement("meta"))
{
auto metaElem = _sdf->GetElement("meta");
auto meta = result.mutable_meta();
if (metaElem->HasElement("layer"))
meta->set_layer(metaElem->Get<int32_t>("layer"));
}
// Load the geometry
if (_sdf->HasElement("geometry"))
{
auto geomMsg = result.mutable_geometry();
geomMsg->CopyFrom(GeometryFromSDF(_sdf->GetElement("geometry")));
}
/// Load the material
if (_sdf->HasElement("material"))
{
sdf::ElementPtr elem = _sdf->GetElement("material");
auto matMsg = result.mutable_material();
if (elem->HasElement("script"))
{
sdf::ElementPtr scriptElem = elem->GetElement("script");
matMsg->mutable_script()->set_name(
scriptElem->Get<std::string>("name"));
sdf::ElementPtr uriElem = scriptElem->GetElement("uri");
while (uriElem)
{
matMsg->mutable_script()->add_uri(uriElem->Get<std::string>());
uriElem = uriElem->GetNextElement("uri");
}
}
if (elem->HasElement("lighting"))
{
matMsg->set_lighting(elem->Get<bool>("lighting"));
}
if (elem->HasElement("shader"))
{
sdf::ElementPtr shaderElem = elem->GetElement("shader");
if (shaderElem->Get<std::string>("type") == "pixel")
matMsg->set_shader_type(msgs::Material::PIXEL);
else if (shaderElem->Get<std::string>("type") == "vertex")
matMsg->set_shader_type(msgs::Material::VERTEX);
else if (shaderElem->Get<std::string>("type") ==
"normal_map_object_space")
matMsg->set_shader_type(msgs::Material::NORMAL_MAP_OBJECT_SPACE);
else if (shaderElem->Get<std::string>("type") ==
"normal_map_tangent_space")
matMsg->set_shader_type(msgs::Material::NORMAL_MAP_TANGENT_SPACE);
else
gzthrow(std::string("Unknown shader type[") +
shaderElem->Get<std::string>("type") + "]");
if (shaderElem->HasElement("normal_map"))
matMsg->set_normal_map(
shaderElem->GetElement("normal_map")->Get<std::string>());
}
if (elem->HasElement("ambient"))
msgs::Set(matMsg->mutable_ambient(),
elem->Get<common::Color>("ambient"));
if (elem->HasElement("diffuse"))
msgs::Set(matMsg->mutable_diffuse(),
elem->Get<common::Color>("diffuse"));
if (elem->HasElement("specular"))
msgs::Set(matMsg->mutable_specular(),
elem->Get<common::Color>("specular"));
if (elem->HasElement("emissive"))
msgs::Set(matMsg->mutable_emissive(),
elem->Get<common::Color>("emissive"));
}
// Set the origin of the visual
if (_sdf->HasElement("pose"))
{
msgs::Set(result.mutable_pose(),
_sdf->Get<ignition::math::Pose3d>("pose"));
}
// Set plugins of the visual
if (_sdf->HasElement("plugin"))
{
sdf::ElementPtr pluginElem = _sdf->GetElement("plugin");
while (pluginElem)
{
msgs::Plugin *pluginMsg = result.add_plugin();
pluginMsg->CopyFrom(PluginFromSDF(pluginElem));
// DEPRECATED in Gazebo7, remove in Gazebo8
// duplicate innerxml contents into an <sdf> tag to keep backwards
// compatibility
pluginMsg->set_innerxml(pluginMsg->innerxml() +
"\n<sdf>" + pluginMsg->innerxml() + "</sdf>");
pluginElem = pluginElem->GetNextElement("plugin");
}
}
return result;
}
/////////////////////////////////////////////////
msgs::Collision CollisionFromSDF(sdf::ElementPtr _sdf)
{
msgs::Collision result;
if (_sdf->GetName() != "collision")
{
gzerr << "Cannot create a collision message from a ["
<< _sdf->GetName() << "] SDF element." << std::endl;
return result;
}
result.set_name(_sdf->Get<std::string>("name"));
if (_sdf->HasElement("laser_retro"))
result.set_laser_retro(_sdf->Get<double>("laser_retro"));
if (_sdf->HasElement("max_contacts"))
result.set_max_contacts(_sdf->Get<double>("max_contacts"));
if (_sdf->HasElement("pose"))
{
msgs::Set(result.mutable_pose(),
_sdf->Get<ignition::math::Pose3d>("pose"));
}
// Load the geometry
if (_sdf->HasElement("geometry"))
{
auto geomMsg = result.mutable_geometry();
geomMsg->CopyFrom(GeometryFromSDF(_sdf->GetElement("geometry")));
}
// Load the surface
if (_sdf->HasElement("surface"))
{
auto surfaceMsg = result.mutable_surface();
surfaceMsg->CopyFrom(SurfaceFromSDF(_sdf->GetElement("surface")));
}
return result;
}
/////////////////////////////////////////////////
msgs::Surface SurfaceFromSDF(sdf::ElementPtr _sdf)
{
msgs::Surface result;
if (_sdf->GetName() != "surface")
{
gzerr << "Cannot create a surface message from a ["
<< _sdf->GetName() << "] SDF element." << std::endl;
return result;
}
// Load the friction
if (_sdf->HasElement("friction"))
{
auto frictionMsg = result.mutable_friction();
frictionMsg->CopyFrom(FrictionFromSDF(_sdf->GetElement("friction")));
}
// Load bounce elements
if (_sdf->HasElement("bounce"))
{
sdf::ElementPtr bounceElem = _sdf->GetElement("bounce");
if (bounceElem->HasElement("restitution_coefficient"))
{
result.set_restitution_coefficient(
bounceElem->Get<double>("restitution_coefficient"));
}
if (bounceElem->HasElement("threshold"))
result.set_bounce_threshold(bounceElem->Get<double>("threshold"));
}
// Load contact elements. Note the hierarchy differs in SDF and msg
if (_sdf->HasElement("contact"))
{
sdf::ElementPtr contactElem = _sdf->GetElement("contact");
/// \todo Getting only ODE elements, find a way to get others too
if (contactElem->HasElement("ode"))
{
sdf::ElementPtr odeElem = contactElem->GetElement("ode");
if (odeElem->HasElement("soft_cfm"))
result.set_soft_cfm(odeElem->Get<double>("soft_cfm"));
if (odeElem->HasElement("soft_erp"))
result.set_soft_erp(odeElem->Get<double>("soft_erp"));
if (odeElem->HasElement("kp"))
result.set_kp(odeElem->Get<double>("kp"));
if (odeElem->HasElement("kd"))
result.set_kd(odeElem->Get<double>("kd"));
if (odeElem->HasElement("max_vel"))
result.set_max_vel(odeElem->Get<double>("max_vel"));
if (odeElem->HasElement("min_depth"))
result.set_min_depth(odeElem->Get<double>("min_depth"));
}
if (contactElem->HasElement("collide_without_contact"))
{
result.set_collide_without_contact(
contactElem->Get<bool>("collide_without_contact"));
}
if (contactElem->HasElement("collide_without_contact_bitmask"))
{
result.set_collide_without_contact_bitmask(
contactElem->Get<int>("collide_without_contact_bitmask"));
}
if (contactElem->HasElement("collide_bitmask"))
{
result.set_collide_bitmask(contactElem->Get<int>("collide_bitmask"));
}
}
return result;
}
/////////////////////////////////////////////////
msgs::Friction FrictionFromSDF(sdf::ElementPtr _sdf)
{
msgs::Friction result;
if (_sdf->GetName() != "friction")
{
gzerr << "Cannot create a friction message from a ["
<< _sdf->GetName() << "] SDF element." << std::endl;
return result;
}
/// \todo Getting only ODE elements, find a way to get others too
if (_sdf->HasElement("ode"))
{
sdf::ElementPtr odeElem = _sdf->GetElement("ode");
if (odeElem->HasElement("mu"))
result.set_mu(odeElem->Get<double>("mu"));
if (odeElem->HasElement("mu2"))
result.set_mu2(odeElem->Get<double>("mu2"));
if (odeElem->HasElement("fdir1"))
{
msgs::Set(result.mutable_fdir1(),
odeElem->Get<ignition::math::Vector3d>("fdir1"));
}
if (odeElem->HasElement("slip1"))
result.set_slip1(odeElem->Get<double>("slip1"));
if (odeElem->HasElement("slip2"))
result.set_slip2(odeElem->Get<double>("slip2"));
}
// Load torsional friction
if (_sdf->HasElement("torsional"))
{
sdf::ElementPtr torsionalElem = _sdf->GetElement("torsional");
msgs::Friction::Torsional torsionalMsg;
if (torsionalElem->HasElement("coefficient"))
{
torsionalMsg.set_coefficient(
torsionalElem->Get<double>("coefficient"));
}
if (torsionalElem->HasElement("use_patch_radius"))
{
torsionalMsg.set_use_patch_radius(
torsionalElem->Get<bool>("use_patch_radius"));
}
if (torsionalElem->HasElement("patch_radius"))
{
torsionalMsg.set_patch_radius(
torsionalElem->Get<double>("patch_radius"));
}
if (torsionalElem->HasElement("surface_radius"))
{
torsionalMsg.set_surface_radius(
torsionalElem->Get<double>("surface_radius"));
}
if (torsionalElem->HasElement("ode"))
{
sdf::ElementPtr odeElem = torsionalElem->GetElement("ode");
if (odeElem->HasElement("slip"))
{
msgs::Friction::Torsional::ODE torsionalOdeMsg;
torsionalOdeMsg.set_slip(odeElem->Get<double>("slip"));
auto torsionalODE = torsionalMsg.mutable_ode();
torsionalODE->CopyFrom(torsionalOdeMsg);
}
}
auto torsional = result.mutable_torsional();
torsional->CopyFrom(torsionalMsg);
}
return result;
}
/////////////////////////////////////////////////
msgs::Axis AxisFromSDF(sdf::ElementPtr _sdf)
{
msgs::Axis result;
sdf::ElementPtr limitElem = _sdf->GetElement("limit");
result.set_limit_lower(limitElem->Get<double>("lower"));
result.set_limit_upper(limitElem->Get<double>("upper"));
result.set_limit_effort(limitElem->Get<double>("effort"));
result.set_limit_velocity(limitElem->Get<double>("velocity"));
result.set_use_parent_model_frame(
_sdf->Get<bool>("use_parent_model_frame"));
sdf::ElementPtr dynamicsElem = _sdf->GetElement("dynamics");
result.set_damping(dynamicsElem->Get<double>("damping"));
result.set_friction(dynamicsElem->Get<double>("friction"));
msgs::Set(result.mutable_xyz(),
_sdf->Get<ignition::math::Vector3d>("xyz"));
return result;
}
/////////////////////////////////////////////////
msgs::Joint JointFromSDF(sdf::ElementPtr _sdf)
{
msgs::Joint result;
// Name
result.set_name(_sdf->Get<std::string>("name"));
// parent
if (_sdf->HasElement("parent"))
result.set_parent(_sdf->Get<std::string>("parent"));
// child
if (_sdf->HasElement("child"))
result.set_child(_sdf->Get<std::string>("child"));
// Pose
ignition::math::Pose3d jointPose;
if (_sdf->HasElement("pose"))
{
msgs::Set(result.mutable_pose(),
_sdf->Get<ignition::math::Pose3d>("pose"));
}
// Type
std::string type = _sdf->Get<std::string>("type");
result.set_type(msgs::ConvertJointType(type));
// axis1
if (_sdf->HasElement("axis"))
{
msgs::Axis *axis = result.mutable_axis1();
axis->CopyFrom(AxisFromSDF(_sdf->GetElement("axis")));
result.add_angle(0);
}
// axis2
if (_sdf->HasElement("axis2"))
{
msgs::Axis *axis = result.mutable_axis2();
axis->CopyFrom(AxisFromSDF(_sdf->GetElement("axis2")));
result.add_angle(0);
}
// physics
if (_sdf->HasElement("physics"))
{
sdf::ElementPtr physicsElem = _sdf->GetElement("physics");
if (physicsElem->HasElement("ode"))
{
sdf::ElementPtr odeElem = physicsElem->GetElement("ode");
if (odeElem->HasElement("cfm"))
result.set_cfm(odeElem->Get<double>("cfm"));
if (odeElem->HasElement("bounce"))
result.set_bounce(odeElem->Get<double>("bounce"));
if (odeElem->HasElement("velocity"))
result.set_velocity(odeElem->Get<double>("velocity"));
if (odeElem->HasElement("fudge_factor"))
result.set_fudge_factor(odeElem->Get<double>("fudge_factor"));
if (odeElem->HasElement("limit"))
{
sdf::ElementPtr odeLimitElem = odeElem->GetElement("limit");
if (odeLimitElem->HasElement("cfm"))
result.set_limit_cfm(odeLimitElem->Get<double>("cfm"));
if (odeLimitElem->HasElement("erp"))
result.set_limit_erp(odeLimitElem->Get<double>("erp"));
}
if (odeElem->HasElement("suspension"))
{
sdf::ElementPtr odeSuspensionElem =
odeElem->GetElement("suspension");
if (odeSuspensionElem->HasElement("cfm"))
{
result.set_suspension_cfm(
odeSuspensionElem->Get<double>("cfm"));
}
if (odeSuspensionElem->HasElement("erp"))
{
result.set_suspension_erp(
odeSuspensionElem->Get<double>("erp"));
}
}
}
}
// gearbox
if (_sdf->HasElement("gearbox_reference_body"))
{
msgs::Joint::Gearbox *gearboxMsg = result.mutable_gearbox();
gearboxMsg->set_gearbox_reference_body(
_sdf->Get<std::string>("gearbox_reference_body"));
}
if (_sdf->HasElement("gearbox_ratio"))
{
msgs::Joint::Gearbox *gearboxMsg = result.mutable_gearbox();
gearboxMsg->set_gearbox_ratio(_sdf->Get<double>("gearbox_ratio"));
}
// screw
if (_sdf->HasElement("thread_pitch"))
{
msgs::Joint::Screw *screwMsg = result.mutable_screw();
screwMsg->set_thread_pitch(_sdf->Get<double>("thread_pitch"));
}
return result;
}
/////////////////////////////////////////////////
sdf::ElementPtr VisualToSDF(const msgs::Visual &_msg,
sdf::ElementPtr _sdf)
{
sdf::ElementPtr visualSDF;
if (_sdf)
{
visualSDF = _sdf;
}
else
{
visualSDF.reset(new sdf::Element);
sdf::initFile("visual.sdf", visualSDF);
}
// Set the meta information
if (_msg.has_meta())
{
if (_msg.meta().has_layer())
{
visualSDF->GetElement("meta")->GetElement("layer")->Set(
_msg.meta().layer());
}
}
if (_msg.has_name())
visualSDF->GetAttribute("name")->Set(_msg.name());
if (_msg.has_cast_shadows())
visualSDF->GetElement("cast_shadows")->Set(_msg.cast_shadows());
if (_msg.has_transparency())
visualSDF->GetElement("transparency")->Set(_msg.transparency());
if (_msg.has_laser_retro())
visualSDF->GetElement("laser_retro")->Set(_msg.laser_retro());
if (_msg.has_pose())
visualSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
// Load the geometry
if (_msg.has_geometry())
{
sdf::ElementPtr geomElem = visualSDF->GetElement("geometry");
geomElem = GeometryToSDF(_msg.geometry(), geomElem);
}
/// Load the material
if (_msg.has_material())
{
sdf::ElementPtr materialElem = visualSDF->GetElement("material");
materialElem = MaterialToSDF(_msg.material(), materialElem);
}
// Set plugins of the visual
for (int i = 0; i < _msg.plugin_size(); ++i)
{
sdf::ElementPtr pluginElem = visualSDF->AddElement("plugin");
pluginElem = PluginToSDF(_msg.plugin(i), pluginElem);
}
return visualSDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr MaterialToSDF(const msgs::Material &_msg,
sdf::ElementPtr _sdf)
{
sdf::ElementPtr materialSDF;
if (_sdf)
{
materialSDF = _sdf;
}
else
{
materialSDF.reset(new sdf::Element);
sdf::initFile("material.sdf", materialSDF);
}
if (_msg.has_script())
{
sdf::ElementPtr scriptElem = materialSDF->GetElement("script");
msgs::Material::Script script = _msg.script();
if (script.has_name())
scriptElem->GetElement("name")->Set(script.name());
if (script.uri_size() > 0)
while (scriptElem->HasElement("uri"))
scriptElem->GetElement("uri")->RemoveFromParent();
for (int i = 0; i < script.uri_size(); ++i)
{
sdf::ElementPtr uriElem = scriptElem->AddElement("uri");
uriElem->Set(script.uri(i));
}
}
if (_msg.has_shader_type())
{
sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
shaderElem->GetAttribute("type")->Set(
ConvertShaderType(_msg.shader_type()));
}
if (_msg.has_normal_map())
{
sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
shaderElem->GetElement("normal_map")->Set(_msg.normal_map());
}
if (_msg.has_lighting())
materialSDF->GetElement("lighting")->Set(_msg.lighting());
if (_msg.has_ambient())
materialSDF->GetElement("ambient")->Set(Convert(_msg.ambient()));
if (_msg.has_diffuse())
materialSDF->GetElement("diffuse")->Set(Convert(_msg.diffuse()));
if (_msg.has_emissive())
materialSDF->GetElement("emissive")->Set(Convert(_msg.emissive()));
if (_msg.has_specular())
materialSDF->GetElement("specular")->Set(Convert(_msg.specular()));
return materialSDF;
}
/////////////////////////////////////////////////
msgs::Material::ShaderType ConvertShaderType(const std::string &_str)
{
auto result = msgs::Material::VERTEX;
if (_str == "vertex")
{
result = msgs::Material::VERTEX;
}
else if (_str == "pixel")
{
result = msgs::Material::PIXEL;
}
else if (_str == "normal_map_object_space")
{
result = msgs::Material::NORMAL_MAP_OBJECT_SPACE;
}
else if (_str == "normal_map_tangent_space")
{
result = msgs::Material::NORMAL_MAP_TANGENT_SPACE;
}
else
{
gzerr << "Unrecognized ShaderType ["
<< _str
<< "], returning VERTEX"
<< std::endl;
}
return result;
}
/////////////////////////////////////////////////
std::string ConvertShaderType(const msgs::Material::ShaderType &_type)
{
std::string result;
switch (_type)
{
case msgs::Material::VERTEX:
{
result = "vertex";
break;
}
case msgs::Material::PIXEL:
{
result = "pixel";
break;
}
case msgs::Material::NORMAL_MAP_OBJECT_SPACE:
{
result = "normal_map_object_space";
break;
}
case msgs::Material::NORMAL_MAP_TANGENT_SPACE:
{
result = "normal_map_tangent_space";
break;
}
default:
{
result = "unknown";
gzerr << "Unrecognized ShaderType [" << _type << "]"
<< std::endl;
break;
}
}
return result;
}
/////////////////////////////////////////////////
msgs::Fog FogFromSDF(sdf::ElementPtr _sdf)
{
msgs::Fog result;
std::string type = _sdf->Get<std::string>("type");
if (type == "linear")
result.set_type(msgs::Fog::LINEAR);
else if (type == "exp")
result.set_type(msgs::Fog::EXPONENTIAL);
else if (type == "exp2")
result.set_type(msgs::Fog::EXPONENTIAL2);
else if (type == "none")
result.set_type(msgs::Fog::NONE);
else
gzthrow(std::string("Unknown fog type[") + type + "]");
result.mutable_color()->CopyFrom(
Convert(_sdf->Get<common::Color>("color")));
result.set_density(_sdf->Get<double>("density"));
result.set_start(_sdf->Get<double>("start"));
result.set_end(_sdf->Get<double>("end"));
return result;
}
/////////////////////////////////////////////////
msgs::Scene SceneFromSDF(sdf::ElementPtr _sdf)
{
msgs::Scene result;
Init(result, "scene");
if (_sdf->HasElement("grid"))
result.set_grid(_sdf->Get<bool>("grid"));
else
result.set_grid(true);
if (_sdf->HasElement("origin_visual"))
result.set_origin_visual(_sdf->Get<bool>("origin_visual"));
else
result.set_origin_visual(true);
if (_sdf->HasElement("ambient"))
result.mutable_ambient()->CopyFrom(
Convert(_sdf->Get<common::Color>("ambient")));
if (_sdf->HasElement("background"))
{
result.mutable_background()->CopyFrom(
Convert(_sdf->Get<common::Color>("background")));
}
if (_sdf->HasElement("sky"))
{
msgs::Sky *skyMsg = result.mutable_sky();
skyMsg->set_time(_sdf->GetElement("sky")->Get<double>("time"));
skyMsg->set_sunrise(_sdf->GetElement("sky")->Get<double>("sunrise"));
skyMsg->set_sunset(_sdf->GetElement("sky")->Get<double>("sunset"));
if (_sdf->GetElement("sky")->HasElement("clouds"))
{
sdf::ElementPtr cloudsElem =
_sdf->GetElement("sky")->GetElement("clouds");
skyMsg->set_wind_speed(cloudsElem->Get<double>("speed"));
skyMsg->set_wind_direction(cloudsElem->Get<double>("direction"));
skyMsg->set_humidity(cloudsElem->Get<double>("humidity"));
skyMsg->set_mean_cloud_size(cloudsElem->Get<double>("mean_size"));
msgs::Set(skyMsg->mutable_cloud_ambient(),
cloudsElem->Get<common::Color>("ambient"));
}
}
if (_sdf->HasElement("fog"))
result.mutable_fog()->CopyFrom(FogFromSDF(_sdf->GetElement("fog")));
if (_sdf->HasElement("shadows"))
result.set_shadows(_sdf->Get<bool>("shadows"));
return result;
}
/////////////////////////////////////////////////
msgs::Sensor SensorFromSDF(sdf::ElementPtr _sdf)
{
msgs::Sensor result;
std::string type = _sdf->Get<std::string>("type");
result.set_name(_sdf->Get<std::string>("name"));
result.set_type(type);
if (_sdf->HasElement("always_on"))
result.set_always_on(_sdf->Get<bool>("always_on"));
if (_sdf->HasElement("update_rate"))
result.set_update_rate(_sdf->Get<double>("update_rate"));
if (_sdf->HasElement("pose"))
{
msgs::Set(result.mutable_pose(),
_sdf->Get<ignition::math::Pose3d>("pose"));
}
if (_sdf->HasElement("visualize"))
result.set_visualize(_sdf->Get<bool>("visualize"));
if (_sdf->HasElement("topic"))
result.set_topic(_sdf->Get<std::string>("topic"));
if (type == "camera")
{
result.mutable_camera()->CopyFrom(
msgs::CameraSensorFromSDF(_sdf->GetElement("camera")));
}
else if (type == "ray")
{
result.mutable_ray()->CopyFrom(msgs::RaySensorFromSDF(
_sdf->GetElement("ray")));
}
else if (type == "contact")
{
result.mutable_contact()->CopyFrom(
msgs::ContactSensorFromSDF(_sdf->GetElement("contact")));
}
else
{
gzwarn << "Conversion of sensor type[" << type << "] not suppported."
<< std::endl;
}
return result;
}
/////////////////////////////////////////////////
msgs::CameraSensor CameraSensorFromSDF(sdf::ElementPtr _sdf)
{
msgs::CameraSensor result;
result.set_horizontal_fov(_sdf->Get<double>("horizontal_fov"));
result.mutable_image_size()->set_x(
_sdf->GetElement("image")->Get<int>("width"));
result.mutable_image_size()->set_y(
_sdf->GetElement("image")->Get<int>("height"));
if (_sdf->GetElement("image")->HasElement("format"))
{
result.set_image_format(
_sdf->GetElement("image")->Get<std::string>("format"));
}
result.set_near_clip(_sdf->GetElement("clip")->Get<double>("near"));
result.set_far_clip(_sdf->GetElement("clip")->Get<double>("far"));
if (_sdf->HasElement("save"))
{
result.set_save_enabled(_sdf->GetElement("save")->Get<bool>("enabled"));
result.set_save_path(
_sdf->GetElement("save")->Get<std::string>("path"));
}
if (_sdf->HasElement("distortion"))
{
sdf::ElementPtr distElem = _sdf->GetElement("distortion");
msgs::Distortion *distortionMsg = result.mutable_distortion();
if (distElem->HasElement("k1"))
distortionMsg->set_k1(distElem->Get<double>("k1"));
if (distElem->HasElement("k2"))
distortionMsg->set_k2(distElem->Get<double>("k2"));
if (distElem->HasElement("k3"))
distortionMsg->set_k3(distElem->Get<double>("k3"));
if (distElem->HasElement("p1"))
distortionMsg->set_p1(distElem->Get<double>("p1"));
if (distElem->HasElement("p2"))
distortionMsg->set_p2(distElem->Get<double>("p2"));
if (distElem->HasElement("center"))
{
distortionMsg->mutable_center()->set_x(
distElem->Get<ignition::math::Vector2d>("center").X());
distortionMsg->mutable_center()->set_y(
distElem->Get<ignition::math::Vector2d>("center").Y());
}
}
return result;
}
/////////////////////////////////////////////////
msgs::RaySensor RaySensorFromSDF(sdf::ElementPtr _sdf)
{
msgs::RaySensor result;
sdf::ElementPtr rangeElem = _sdf->GetElement("range");
sdf::ElementPtr scanElem = _sdf->GetElement("scan");
sdf::ElementPtr hscanElem = scanElem->GetElement("horizontal");
sdf::ElementPtr vscanElem = scanElem->GetElement("vertical");
result.set_horizontal_samples(hscanElem->Get<int>("samples"));
result.set_horizontal_resolution(hscanElem->Get<double>("resolution"));
result.set_horizontal_min_angle(hscanElem->Get<double>("min_angle"));
result.set_horizontal_max_angle(hscanElem->Get<double>("max_angle"));
result.set_vertical_samples(vscanElem->Get<int>("samples"));
result.set_vertical_resolution(vscanElem->Get<double>("resolution"));
result.set_vertical_min_angle(vscanElem->Get<double>("min_angle"));
result.set_vertical_max_angle(vscanElem->Get<double>("max_angle"));
result.set_range_min(rangeElem->Get<double>("min"));
result.set_range_max(rangeElem->Get<double>("max"));
result.set_range_resolution(rangeElem->Get<double>("resolution"));
return result;
}
/////////////////////////////////////////////////
msgs::ContactSensor ContactSensorFromSDF(sdf::ElementPtr _sdf)
{
msgs::ContactSensor result;
result.set_collision_name(_sdf->Get<std::string>("collision"));
return result;
}
/////////////////////////////////////////////////
sdf::ElementPtr LightToSDF(const msgs::Light &_msg, sdf::ElementPtr _sdf)
{
sdf::ElementPtr lightSDF;
if (_sdf)
{
lightSDF = _sdf;
}
else
{
lightSDF.reset(new sdf::Element);
sdf::initFile("light.sdf", lightSDF);
}
lightSDF->GetAttribute("name")->Set(_msg.name());
if (_msg.has_type() && _msg.type() == msgs::Light::POINT)
lightSDF->GetAttribute("type")->Set("point");
else if (_msg.has_type() && _msg.type() == msgs::Light::SPOT)
lightSDF->GetAttribute("type")->Set("spot");
else if (_msg.has_type() && _msg.type() == msgs::Light::DIRECTIONAL)
lightSDF->GetAttribute("type")->Set("directional");
if (_msg.has_pose())
{
lightSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
}
if (_msg.has_diffuse())
{
lightSDF->GetElement("diffuse")->Set(msgs::Convert(_msg.diffuse()));
}
if (_msg.has_specular())
{
lightSDF->GetElement("specular")->Set(msgs::Convert(_msg.specular()));
}
if (_msg.has_direction())
{
lightSDF->GetElement("direction")->Set(ConvertIgn(_msg.direction()));
}
if (_msg.has_attenuation_constant())
{
sdf::ElementPtr elem = lightSDF->GetElement("attenuation");
elem->GetElement("constant")->Set(_msg.attenuation_constant());
}
if (_msg.has_attenuation_linear())
{
sdf::ElementPtr elem = lightSDF->GetElement("attenuation");
elem->GetElement("linear")->Set(_msg.attenuation_linear());
}
if (_msg.has_attenuation_quadratic())
{
sdf::ElementPtr elem = lightSDF->GetElement("attenuation");
elem->GetElement("quadratic")->Set(_msg.attenuation_quadratic());
}
if (_msg.has_range())
{
sdf::ElementPtr elem = lightSDF->GetElement("attenuation");
elem->GetElement("range")->Set(_msg.range());
}
if (_msg.has_cast_shadows())
lightSDF->GetElement("cast_shadows")->Set(_msg.cast_shadows());
if (_msg.has_spot_inner_angle())
{
sdf::ElementPtr elem = lightSDF->GetElement("spot");
elem->GetElement("inner_angle")->Set(_msg.spot_inner_angle());
}
if (_msg.has_spot_outer_angle())
{
sdf::ElementPtr elem = lightSDF->GetElement("spot");
elem->GetElement("outer_angle")->Set(_msg.spot_outer_angle());
}
if (_msg.has_spot_falloff())
{
sdf::ElementPtr elem = lightSDF->GetElement("spot");
elem->GetElement("falloff")->Set(_msg.spot_falloff());
}
return lightSDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr CameraSensorToSDF(const msgs::CameraSensor &_msg,
sdf::ElementPtr _sdf)
{
sdf::ElementPtr cameraSDF;
if (_sdf)
{
cameraSDF = _sdf;
}
else
{
cameraSDF.reset(new sdf::Element);
sdf::initFile("camera.sdf", cameraSDF);
}
if (_msg.has_horizontal_fov())
{
cameraSDF->GetElement("horizontal_fov")->Set(_msg.horizontal_fov());
}
if (_msg.has_image_size())
{
sdf::ElementPtr imageElem = cameraSDF->GetElement("image");
imageElem->GetElement("width")->Set(_msg.image_size().x());
imageElem->GetElement("height")->Set(_msg.image_size().y());
}
if (_msg.has_image_format())
{
sdf::ElementPtr imageElem = cameraSDF->GetElement("image");
imageElem->GetElement("format")->Set(_msg.image_format());
}
if (_msg.has_near_clip() || _msg.has_far_clip())
{
sdf::ElementPtr clipElem = cameraSDF->GetElement("clip");
if (_msg.has_near_clip())
clipElem->GetElement("near")->Set(_msg.near_clip());
if (_msg.has_far_clip())
clipElem->GetElement("far")->Set(_msg.far_clip());
}
if (_msg.has_distortion())
{
msgs::Distortion distortionMsg = _msg.distortion();
sdf::ElementPtr distortionElem =
cameraSDF->GetElement("distortion");
if (distortionMsg.has_center())
{
distortionElem->GetElement("center")->Set(
ConvertIgn(distortionMsg.center()));
}
if (distortionMsg.has_k1())
{
distortionElem->GetElement("k1")->Set(distortionMsg.k1());
}
if (distortionMsg.has_k2())
{
distortionElem->GetElement("k2")->Set(distortionMsg.k2());
}
if (distortionMsg.has_k3())
{
distortionElem->GetElement("k3")->Set(distortionMsg.k3());
}
if (distortionMsg.has_p1())
{
distortionElem->GetElement("p1")->Set(distortionMsg.p1());
}
if (distortionMsg.has_p2())
{
distortionElem->GetElement("p2")->Set(distortionMsg.p2());
}
}
return cameraSDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr CollisionToSDF(const msgs::Collision &_msg,
sdf::ElementPtr _sdf)
{
sdf::ElementPtr collisionSDF;
if (_sdf)
{
collisionSDF = _sdf;
}
else
{
collisionSDF.reset(new sdf::Element);
sdf::initFile("collision.sdf", collisionSDF);
}
if (_msg.has_name())
collisionSDF->GetAttribute("name")->Set(_msg.name());
if (_msg.has_laser_retro())
collisionSDF->GetElement("laser_retro")->Set(_msg.laser_retro());
if (_msg.has_max_contacts())
collisionSDF->GetElement("max_contacts")->Set(_msg.max_contacts());
if (_msg.has_pose())
collisionSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
if (_msg.has_geometry())
{
sdf::ElementPtr geomElem = collisionSDF->GetElement("geometry");
geomElem = GeometryToSDF(_msg.geometry(), geomElem);
}
if (_msg.has_surface())
{
sdf::ElementPtr surfaceElem = collisionSDF->GetElement("surface");
surfaceElem = SurfaceToSDF(_msg.surface(), surfaceElem);
}
return collisionSDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr LinkToSDF(const msgs::Link &_msg,
sdf::ElementPtr _sdf)
{
sdf::ElementPtr linkSDF;
if (_sdf)
{
linkSDF = _sdf;
}
else
{
linkSDF.reset(new sdf::Element);
sdf::initFile("link.sdf", linkSDF);
}
if (_msg.has_name())
linkSDF->GetAttribute("name")->Set(_msg.name());
if (_msg.has_gravity())
linkSDF->GetElement("gravity")->Set(_msg.gravity());
if (_msg.has_self_collide())
linkSDF->GetElement("self_collide")->Set(_msg.self_collide());
if (_msg.has_kinematic())
linkSDF->GetElement("kinematic")->Set(_msg.kinematic());
if (_msg.has_pose())
linkSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
if (_msg.has_inertial())
{
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
inertialElem = InertialToSDF(_msg.inertial(), inertialElem);
}
if (_msg.collision_size() > 0)
while (linkSDF->HasElement("collision"))
linkSDF->GetElement("collision")->RemoveFromParent();
for (int i = 0; i < _msg.collision_size(); ++i)
{
sdf::ElementPtr collisionElem = linkSDF->AddElement("collision");
collisionElem = CollisionToSDF(_msg.collision(i), collisionElem);
}
if (_msg.visual_size() > 0)
while (linkSDF->HasElement("visual"))
linkSDF->GetElement("visual")->RemoveFromParent();
for (int i = 0; i < _msg.visual_size(); ++i)
{
sdf::ElementPtr visualElem = linkSDF->AddElement("visual");
visualElem = VisualToSDF(_msg.visual(i), visualElem);
}
/// \todo LinkToSDF currently does not convert sensor and projector data
return linkSDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr InertialToSDF(const msgs::Inertial &_msg,
sdf::ElementPtr _sdf)
{
sdf::ElementPtr inertialSDF;
if (_sdf)
{
inertialSDF = _sdf;
}
else
{
inertialSDF.reset(new sdf::Element);
sdf::initFile("inertial.sdf", inertialSDF);
}
if (_msg.has_mass())
inertialSDF->GetElement("mass")->Set(_msg.mass());
if (_msg.has_pose())
inertialSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
sdf::ElementPtr inertiaSDF = inertialSDF->GetElement("inertia");
if (_msg.has_ixx())
inertiaSDF->GetElement("ixx")->Set(_msg.ixx());
if (_msg.has_ixy())
inertiaSDF->GetElement("ixy")->Set(_msg.ixy());
if (_msg.has_ixz())
inertiaSDF->GetElement("ixz")->Set(_msg.ixz());
if (_msg.has_iyy())
inertiaSDF->GetElement("iyy")->Set(_msg.iyy());
if (_msg.has_iyz())
inertiaSDF->GetElement("iyz")->Set(_msg.iyz());
if (_msg.has_izz())
inertiaSDF->GetElement("izz")->Set(_msg.izz());
return inertialSDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr SurfaceToSDF(const msgs::Surface &_msg,
sdf::ElementPtr _sdf)
{
sdf::ElementPtr surfaceSDF;
if (_sdf)
{
surfaceSDF = _sdf;
}
else
{
surfaceSDF.reset(new sdf::Element);
sdf::initFile("surface.sdf", surfaceSDF);
}
if (_msg.has_friction())
{
msgs::Friction friction = _msg.friction();
sdf::ElementPtr frictionElem = surfaceSDF->GetElement("friction");
sdf::ElementPtr physicsEngElem = frictionElem->GetElement("ode");
if (friction.has_mu())
physicsEngElem->GetElement("mu")->Set(friction.mu());
if (friction.has_mu2())
physicsEngElem->GetElement("mu2")->Set(friction.mu2());
if (friction.has_fdir1())
{
physicsEngElem->GetElement("fdir1")->Set(
ConvertIgn(friction.fdir1()));
}
if (friction.has_slip1())
physicsEngElem->GetElement("slip1")->Set(friction.slip1());
if (friction.has_slip2())
physicsEngElem->GetElement("slip2")->Set(friction.slip2());
if (friction.has_torsional())
{
msgs::Friction::Torsional torsional = friction.torsional();
sdf::ElementPtr torsionalElem = frictionElem->GetElement("torsional");
if (torsional.has_coefficient())
{
torsionalElem->GetElement("coefficient")->Set(
torsional.coefficient());
}
if (torsional.has_patch_radius())
{
torsionalElem->GetElement("patch_radius")->Set(
torsional.patch_radius());
}
if (torsional.has_surface_radius())
{
torsionalElem->GetElement("surface_radius")->Set(
torsional.surface_radius());
}
if (torsional.has_use_patch_radius())
{
torsionalElem->GetElement("use_patch_radius")->Set(
torsional.use_patch_radius());
}
if (torsional.has_ode())
{
msgs::Friction::Torsional::ODE ode = torsional.ode();
sdf::ElementPtr odeElem = torsionalElem->GetElement("ode");
if (ode.has_slip())
{
odeElem->GetElement("slip")->Set(ode.slip());
}
}
}
}
sdf::ElementPtr bounceElem = surfaceSDF->GetElement("bounce");
if (_msg.has_restitution_coefficient())
{
bounceElem->GetElement("restitution_coefficient")->Set(
_msg.restitution_coefficient());
}
if (_msg.has_bounce_threshold())
{
bounceElem->GetElement("threshold")->Set(
_msg.bounce_threshold());
}
sdf::ElementPtr contactElem = surfaceSDF->GetElement("contact");
if (_msg.has_collide_without_contact())
{
contactElem->GetElement("collide_without_contact")->Set(
_msg.collide_without_contact());
}
if (_msg.has_collide_without_contact_bitmask())
{
contactElem->GetElement("collide_without_contact_bitmask")->Set(
_msg.collide_without_contact_bitmask());
}
if (_msg.has_collide_bitmask())
{
contactElem->GetElement("collide_bitmask")->Set(
_msg.collide_bitmask());
}
sdf::ElementPtr odeElem = contactElem->GetElement("ode");
sdf::ElementPtr bulletElem = contactElem->GetElement("bullet");
if (_msg.has_soft_cfm())
{
odeElem->GetElement("soft_cfm")->Set(_msg.soft_cfm());
bulletElem->GetElement("soft_cfm")->Set(_msg.soft_cfm());
}
if (_msg.has_soft_erp())
{
odeElem->GetElement("soft_erp")->Set(_msg.soft_erp());
bulletElem->GetElement("soft_erp")->Set(_msg.soft_erp());
}
if (_msg.has_kp())
{
odeElem->GetElement("kp")->Set(_msg.kp());
bulletElem->GetElement("kp")->Set(_msg.kp());
}
if (_msg.has_kd())
{
odeElem->GetElement("kd")->Set(_msg.kd());
bulletElem->GetElement("kd")->Set(_msg.kd());
}
if (_msg.has_max_vel())
{
odeElem->GetElement("max_vel")->Set(_msg.max_vel());
}
if (_msg.has_min_depth())
{
odeElem->GetElement("min_depth")->Set(_msg.min_depth());
}
return surfaceSDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr GeometryToSDF(const msgs::Geometry &_msg,
sdf::ElementPtr _sdf)
{
sdf::ElementPtr geometrySDF;
if (_sdf)
{
geometrySDF = _sdf;
}
else
{
geometrySDF.reset(new sdf::Element);
sdf::initFile("geometry.sdf", geometrySDF);
}
if (!_msg.has_type())
return geometrySDF;
if (_msg.type() == msgs::Geometry::BOX &&
_msg.has_box())
{
sdf::ElementPtr geom = geometrySDF->GetElement("box");
msgs::BoxGeom boxGeom = _msg.box();
if (boxGeom.has_size())
geom->GetElement("size")->Set(ConvertIgn(boxGeom.size()));
}
else if (_msg.type() == msgs::Geometry::CYLINDER &&
_msg.has_cylinder())
{
sdf::ElementPtr geom = geometrySDF->GetElement("cylinder");
msgs::CylinderGeom cylinderGeom = _msg.cylinder();
if (cylinderGeom.has_radius())
geom->GetElement("radius")->Set(cylinderGeom.radius());
if (cylinderGeom.has_length())
geom->GetElement("length")->Set(cylinderGeom.length());
}
else if (_msg.type() == msgs::Geometry::SPHERE &&
_msg.has_sphere())
{
sdf::ElementPtr geom = geometrySDF->GetElement("sphere");
msgs::SphereGeom sphereGeom = _msg.sphere();
if (sphereGeom.has_radius())
geom->GetElement("radius")->Set(sphereGeom.radius());
}
else if (_msg.type() == msgs::Geometry::PLANE &&
_msg.has_plane())
{
sdf::ElementPtr geom = geometrySDF->GetElement("plane");
msgs::PlaneGeom planeGeom = _msg.plane();
if (planeGeom.has_normal())
{
geom->GetElement("normal")->Set(ConvertIgn(planeGeom.normal()));
}
if (planeGeom.has_size())
geom->GetElement("size")->Set(ConvertIgn(planeGeom.size()));
if (planeGeom.has_d())
gzerr << "sdformat doesn't have Plane.d variable" << std::endl;
}
else if (_msg.type() == msgs::Geometry::IMAGE &&
_msg.has_image())
{
sdf::ElementPtr geom = geometrySDF->GetElement("image");
msgs::ImageGeom imageGeom = _msg.image();
if (imageGeom.has_scale())
geom->GetElement("scale")->Set(imageGeom.scale());
if (imageGeom.has_height())
geom->GetElement("height")->Set(imageGeom.height());
if (imageGeom.has_uri())
geom->GetElement("uri")->Set(imageGeom.uri());
if (imageGeom.has_threshold())
geom->GetElement("threshold")->Set(imageGeom.threshold());
if (imageGeom.has_granularity())
geom->GetElement("granularity")->Set(imageGeom.granularity());
}
else if (_msg.type() == msgs::Geometry::HEIGHTMAP &&
_msg.has_heightmap())
{
sdf::ElementPtr geom = geometrySDF->GetElement("heightmap");
msgs::HeightmapGeom heightmapGeom = _msg.heightmap();
if (heightmapGeom.has_size())
{
geom->GetElement("size")->Set(ConvertIgn(heightmapGeom.size()));
}
if (heightmapGeom.has_origin())
{
geom->GetElement("pos")->Set(ConvertIgn(heightmapGeom.origin()));
}
if (heightmapGeom.has_sampling())
{
// check if old version of sdformat is in use
if (geom->HasElementDescription("sampling"))
{
geom->GetElement("sampling")->Set(heightmapGeom.sampling());
}
}
if (heightmapGeom.has_use_terrain_paging())
{
geom->GetElement("use_terrain_paging")->Set(
heightmapGeom.use_terrain_paging());
}
if (heightmapGeom.texture_size() > 0)
while (geom->HasElement("texture"))
geom->GetElement("texture")->RemoveFromParent();
for (int i = 0; i < heightmapGeom.texture_size(); ++i)
{
gazebo::msgs::HeightmapGeom_Texture textureMsg =
heightmapGeom.texture(i);
sdf::ElementPtr textureElem = geom->AddElement("texture");
textureElem->GetElement("diffuse")->Set(textureMsg.diffuse());
textureElem->GetElement("normal")->Set(textureMsg.normal());
textureElem->GetElement("size")->Set(textureMsg.size());
}
if (heightmapGeom.blend_size() > 0)
while (geom->HasElement("blend"))
geom->GetElement("blend")->RemoveFromParent();
for (int i = 0; i < heightmapGeom.blend_size(); ++i)
{
gazebo::msgs::HeightmapGeom_Blend blendMsg =
heightmapGeom.blend(i);
sdf::ElementPtr blendElem = geom->AddElement("blend");
blendElem->GetElement("min_height")->Set(blendMsg.min_height());
blendElem->GetElement("fade_dist")->Set(blendMsg.fade_dist());
}
if (heightmapGeom.has_filename())
geom->GetElement("uri")->Set(heightmapGeom.filename());
}
else if (_msg.type() == msgs::Geometry::MESH &&
_msg.has_mesh())
{
sdf::ElementPtr geom = geometrySDF->GetElement("mesh");
msgs::MeshGeom meshGeom = _msg.mesh();
geom = msgs::MeshToSDF(meshGeom, geom);
}
else if (_msg.type() == msgs::Geometry::POLYLINE &&
_msg.polyline_size() > 0)
{
if (_msg.polyline_size() > 0)
while (geometrySDF->HasElement("polyline"))
geometrySDF->GetElement("polyline")->RemoveFromParent();
for (int j = 0; j < _msg.polyline_size(); ++j)
{
sdf::ElementPtr polylineElem = geometrySDF->AddElement("polyline");
if (_msg.polyline(j).has_height())
polylineElem->GetElement("height")->Set(_msg.polyline(j).height());
for (int i = 0; i < _msg.polyline(j).point_size(); ++i)
{
sdf::ElementPtr pointElem = polylineElem->AddElement("point");
pointElem->Set(ConvertIgn(_msg.polyline(j).point(i)));
}
}
}
else
{
gzerr << "Unrecognized geometry type" << std::endl;
}
return geometrySDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr MeshToSDF(const msgs::MeshGeom &_msg, sdf::ElementPtr _sdf)
{
sdf::ElementPtr meshSDF;
if (_sdf)
{
meshSDF = _sdf;
}
else
{
meshSDF.reset(new sdf::Element);
sdf::initFile("mesh_shape.sdf", meshSDF);
}
if (_msg.has_filename())
meshSDF->GetElement("uri")->Set(_msg.filename());
if (_msg.has_submesh())
{
sdf::ElementPtr submeshElem = meshSDF->GetElement("submesh");
submeshElem->GetElement("name")->Set(_msg.submesh());
if (_msg.has_center_submesh())
submeshElem->GetElement("center")->Set(_msg.center_submesh());
}
if (_msg.has_scale())
{
meshSDF->GetElement("scale")->Set(ConvertIgn(_msg.scale()));
}
return meshSDF;
}
/////////////////////////////////////////////////
sdf::ElementPtr PluginToSDF(const msgs::Plugin &_msg, sdf::ElementPtr _sdf)
{
sdf::ElementPtr pluginSDF;
if (_sdf)
{
pluginSDF = _sdf;
}
else
{
pluginSDF.reset(new sdf::Element);
sdf::initFile("plugin.sdf", pluginSDF);
}
// Use the SDF parser to read all the inner xml.
std::string tmp = "<sdf version='" + std::string(SDF_VERSION) + "'>";
tmp += "<plugin name='" + _msg.name() + "' filename='" +
_msg.filename() + "'>";
tmp += _msg.innerxml();
tmp += "</plugin></sdf>";
sdf::readString(tmp, pluginSDF);
return pluginSDF;
}
////////////////////////////////////////////////////////
void AddLinkGeom(Model &_model, const Geometry &_geom)
{
_model.add_link();
int linkCount = _model.link_size();
auto link = _model.mutable_link(linkCount-1);
{
std::ostringstream linkName;
linkName << "link_" << linkCount;
link->set_name(linkName.str());
}
{
link->add_collision();
auto collision = link->mutable_collision(0);
collision->set_name("collision");
*(collision->mutable_geometry()) = _geom;
}
{
link->add_visual();
auto visual = link->mutable_visual(0);
visual->set_name("visual");
*(visual->mutable_geometry()) = _geom;
auto script = visual->mutable_material()->mutable_script();
script->add_uri();
script->set_uri(0, "file://media/materials/scripts/gazebo.material");
script->set_name("Gazebo/Grey");
}
}
////////////////////////////////////////////////////////
void AddBoxLink(Model &_model, const double _mass,
const ignition::math::Vector3d &_size)
{
Geometry geometry;
geometry.set_type(Geometry_Type_BOX);
Set(geometry.mutable_box()->mutable_size(), _size);
AddLinkGeom(_model, geometry);
int linkCount = _model.link_size();
auto link = _model.mutable_link(linkCount-1);
ignition::math::MassMatrix3d m;
if (!m.SetFromBox(_mass, _size))
{
gzerr << "Error computing inertia, not setting" << std::endl;
}
else
{
msgs::Set(link->mutable_inertial(), m);
}
}
////////////////////////////////////////////////////////
void AddCylinderLink(Model &_model,
const double _mass,
const double _radius,
const double _length)
{
Geometry geometry;
geometry.set_type(Geometry_Type_CYLINDER);
geometry.mutable_cylinder()->set_radius(_radius);
geometry.mutable_cylinder()->set_length(_length);
AddLinkGeom(_model, geometry);
int linkCount = _model.link_size();
auto link = _model.mutable_link(linkCount-1);
ignition::math::MassMatrix3d m;
if (!m.SetFromCylinderZ(_mass, _length, _radius))
{
gzerr << "Error computing inertia, not setting" << std::endl;
}
else
{
msgs::Set(link->mutable_inertial(), m);
}
}
////////////////////////////////////////////////////////
void AddSphereLink(Model &_model, const double _mass,
const double _radius)
{
Geometry geometry;
geometry.set_type(Geometry_Type_SPHERE);
geometry.mutable_sphere()->set_radius(_radius);
AddLinkGeom(_model, geometry);
int linkCount = _model.link_size();
auto link = _model.mutable_link(linkCount-1);
ignition::math::MassMatrix3d m;
if (!m.SetFromSphere(_mass, _radius))
{
gzerr << "Error computing inertia, not setting" << std::endl;
}
else
{
msgs::Set(link->mutable_inertial(), m);
}
}
////////////////////////////////////////////////////////
sdf::ElementPtr ModelToSDF(const msgs::Model &_msg, sdf::ElementPtr _sdf)
{
sdf::ElementPtr modelSDF;
if (_sdf)
{
modelSDF = _sdf;
}
else
{
modelSDF.reset(new sdf::Element);
sdf::initFile("model.sdf", modelSDF);
}
if (_msg.has_name())
modelSDF->GetAttribute("name")->Set(_msg.name());
// ignore the id field, since it's not used in sdformat
if (_msg.has_is_static())
modelSDF->GetElement("static")->Set(_msg.is_static());
if (_msg.has_pose())
modelSDF->GetElement("pose")->Set(msgs::ConvertIgn(_msg.pose()));
if (_msg.joint_size() > 0)
while (modelSDF->HasElement("joint"))
modelSDF->GetElement("joint")->RemoveFromParent();
for (int i = 0; i < _msg.joint_size(); ++i)
{
sdf::ElementPtr jointElem = modelSDF->AddElement("joint");
jointElem = JointToSDF(_msg.joint(i), jointElem);
}
if (_msg.link_size())
while (modelSDF->HasElement("link"))
modelSDF->GetElement("link")->RemoveFromParent();
for (int i = 0; i < _msg.link_size(); ++i)
{
sdf::ElementPtr linkElem = modelSDF->AddElement("link");
linkElem = LinkToSDF(_msg.link(i), linkElem);
}
// ignore the deleted field, since it's not used in sdformat
if (_msg.visual_size() > 0)
{
// model element in SDF cannot store visuals,
// so ignore them for now
gzerr << "Model visuals not yet parsed" << std::endl;
}
// ignore the scale field, since it's not used in sdformat
return modelSDF;
}
////////////////////////////////////////////////////////
sdf::ElementPtr JointToSDF(const msgs::Joint &_msg, sdf::ElementPtr _sdf)
{
sdf::ElementPtr jointSDF;
if (_sdf)
{
jointSDF = _sdf;
}
else
{
jointSDF.reset(new sdf::Element);
sdf::initFile("joint.sdf", jointSDF);
}
if (_msg.has_name())
jointSDF->GetAttribute("name")->Set(_msg.name());
if (_msg.has_type())
jointSDF->GetAttribute("type")->Set(ConvertJointType(_msg.type()));
// ignore the id field, since it's not used in sdformat
// ignore the parent_id field, since it's not used in sdformat
// ignore the child_id field, since it's not used in sdformat
// ignore the angle field, since it's not used in sdformat
if (_msg.has_parent())
jointSDF->GetElement("parent")->Set(_msg.parent());
if (_msg.has_child())
jointSDF->GetElement("child")->Set(_msg.child());
if (_msg.has_pose())
jointSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
if (_msg.has_axis1())
AxisToSDF(_msg.axis1(), jointSDF->GetElement("axis"));
if (_msg.has_axis2())
AxisToSDF(_msg.axis2(), jointSDF->GetElement("axis2"));
auto odePhysicsElem = jointSDF->GetElement("physics")->GetElement("ode");
if (_msg.has_cfm())
odePhysicsElem->GetElement("cfm")->Set(_msg.cfm());
if (_msg.has_bounce())
odePhysicsElem->GetElement("bounce")->Set(_msg.bounce());
if (_msg.has_velocity())
odePhysicsElem->GetElement("velocity")->Set(_msg.velocity());
if (_msg.has_fudge_factor())
odePhysicsElem->GetElement("fudge_factor")->Set(_msg.fudge_factor());
{
auto limitElem = odePhysicsElem->GetElement("limit");
if (_msg.has_limit_cfm())
limitElem->GetElement("cfm")->Set(_msg.limit_cfm());
if (_msg.has_limit_erp())
limitElem->GetElement("erp")->Set(_msg.limit_erp());
}
{
auto suspensionElem = odePhysicsElem->GetElement("suspension");
if (_msg.has_suspension_cfm())
suspensionElem->GetElement("cfm")->Set(_msg.suspension_cfm());
if (_msg.has_suspension_erp())
suspensionElem->GetElement("erp")->Set(_msg.suspension_erp());
}
// gearbox joint message fields
if (_msg.has_gearbox())
{
msgs::Joint::Gearbox gearboxMsg = _msg.gearbox();
if (gearboxMsg.has_gearbox_reference_body())
{
jointSDF->GetElement("gearbox_reference_body")->Set(
gearboxMsg.gearbox_reference_body());
}
if (gearboxMsg.has_gearbox_ratio())
{
jointSDF->GetElement("gearbox_ratio")->Set(
gearboxMsg.gearbox_ratio());
}
}
// screw joint message field
if (_msg.has_screw())
{
msgs::Joint::Screw screwMsg = _msg.screw();
if (screwMsg.has_thread_pitch())
jointSDF->GetElement("thread_pitch")->Set(screwMsg.thread_pitch());
}
/// \todo JointToSDF currently does not convert sensor data
return jointSDF;
}
////////////////////////////////////////////////////////
void AxisToSDF(const msgs::Axis &_msg, sdf::ElementPtr _sdf)
{
if (_msg.has_xyz())
_sdf->GetElement("xyz")->Set(ConvertIgn(_msg.xyz()));
if (_msg.has_use_parent_model_frame())
{
_sdf->GetElement("use_parent_model_frame")->Set(
_msg.use_parent_model_frame());
}
{
auto dynamicsElem = _sdf->GetElement("dynamics");
if (_msg.has_damping())
dynamicsElem->GetElement("damping")->Set(_msg.damping());
if (_msg.has_friction())
dynamicsElem->GetElement("friction")->Set(_msg.friction());
}
{
auto limitElem = _sdf->GetElement("limit");
if (_msg.has_limit_lower())
limitElem->GetElement("lower")->Set(_msg.limit_lower());
if (_msg.has_limit_upper())
limitElem->GetElement("upper")->Set(_msg.limit_upper());
if (_msg.has_limit_effort())
limitElem->GetElement("effort")->Set(_msg.limit_effort());
if (_msg.has_limit_velocity())
limitElem->GetElement("velocity")->Set(_msg.limit_velocity());
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink