/*

* Copyright (C) 2018 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 <gz/msgs/actor.pb.h>

#include <gz/msgs/atmosphere.pb.h>

#include <gz/msgs/axis_aligned_box.pb.h>

#include <gz/msgs/boxgeom.pb.h>

#include <gz/msgs/capsulegeom.pb.h>

#include <gz/msgs/cylindergeom.pb.h>

#include <gz/msgs/ellipsoidgeom.pb.h>

#include <gz/msgs/entity.pb.h>

#include <gz/msgs/geometry.pb.h>

#include <gz/msgs/gps_sensor.pb.h>

#include <gz/msgs/gui.pb.h>

#include <gz/msgs/heightmapgeom.pb.h>

#include <gz/msgs/imu_sensor.pb.h>

#include <gz/msgs/lidar_sensor.pb.h>

#include <gz/msgs/light.pb.h>

#include <gz/msgs/material.pb.h>

#include <gz/msgs/planegeom.pb.h>

#include <gz/msgs/plugin.pb.h>

#include <gz/msgs/projector.pb.h>

#include <gz/msgs/spheregeom.pb.h>

#include <gz/msgs/sky.pb.h>

#include <gz/msgs/Utility.hh>

#include <gz/math/Angle.hh>

#include <gz/math/AxisAlignedBox.hh>

#include <gz/math/Helpers.hh>

#include <gz/math/Temperature.hh>

#include <gz/common/Console.hh>

#include <sdf/Actor.hh>

#include <sdf/Atmosphere.hh>

#include <sdf/AirPressure.hh>

#include <sdf/AirSpeed.hh>

#include <sdf/Altimeter.hh>

#include <sdf/Box.hh>

#include <sdf/Camera.hh>

#include <sdf/Capsule.hh>

#include <sdf/Cylinder.hh>

#include <sdf/Ellipsoid.hh>

#include <sdf/Geometry.hh>

#include <sdf/Gui.hh>

#include <sdf/Heightmap.hh>

#include <sdf/Imu.hh>

#include <sdf/Lidar.hh>

#include <sdf/Light.hh>

#include <sdf/Magnetometer.hh>

#include <sdf/Material.hh>

#include <sdf/Mesh.hh>

#include <sdf/NavSat.hh>

#include <sdf/Pbr.hh>

#include <sdf/Plane.hh>

#include <sdf/Polyline.hh>

#include <sdf/Sphere.hh>

#include <algorithm>

#include <string>

#include "gz/sim/Conversions.hh"

#include "gz/sim/Export.hh"

#include "gz/sim/Util.hh"

using namespace gz;

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Entity_Type gz::sim::convert(const std::string &_in)

{

msgs::Entity_Type out = msgs::Entity_Type_NONE;

if (_in == "light") {

return msgs::Entity_Type_LIGHT;

}

else if (_in == "model")

{

return msgs::Entity_Type_MODEL;

}

else if (_in == "link")

{

return msgs::Entity_Type_LINK;

}

else if (_in == "visual")

{

return msgs::Entity_Type_VISUAL;

}

else if (_in == "collision")

{

return msgs::Entity_Type_COLLISION;

}

else if (_in == "sensor")

{

return msgs::Entity_Type_SENSOR;

}

else if (_in == "joint")

{

return msgs::Entity_Type_JOINT;

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

math::Pose3d gz::sim::convert(const msgs::Pose &_in)

{

math::Pose3d out(_in.position().x(),

_in.position().y(),

_in.position().z(),

_in.orientation().w(),

_in.orientation().x(),

_in.orientation().y(),

_in.orientation().z());

out.Correct();

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Collision gz::sim::convert(const sdf::Collision &_in)

{

msgs::Collision out;

out.set_name(_in.Name());

msgs::Set(out.mutable_pose(), _in.RawPose());

out.mutable_geometry()->CopyFrom(convert<msgs::Geometry>(*_in.Geom()));

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Collision gz::sim::convert(const msgs::Collision &_in)

{

sdf::Collision out;

out.SetName(_in.name());

out.SetRawPose(msgs::Convert(_in.pose()));

out.SetGeom(convert<sdf::Geometry>(_in.geometry()));

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Geometry gz::sim::convert(const sdf::Geometry &_in)

{

msgs::Geometry out;

if (_in.Type() == sdf::GeometryType::BOX && _in.BoxShape())

{

out.set_type(msgs::Geometry::BOX);

msgs::Set(out.mutable_box()->mutable_size(), _in.BoxShape()->Size());

}

else if (_in.Type() == sdf::GeometryType::CAPSULE && _in.CapsuleShape())

{

out.set_type(msgs::Geometry::CAPSULE);

out.mutable_capsule()->set_radius(_in.CapsuleShape()->Radius());

out.mutable_capsule()->set_length(_in.CapsuleShape()->Length());

}

else if (_in.Type() == sdf::GeometryType::CYLINDER && _in.CylinderShape())

{

out.set_type(msgs::Geometry::CYLINDER);

out.mutable_cylinder()->set_radius(_in.CylinderShape()->Radius());

out.mutable_cylinder()->set_length(_in.CylinderShape()->Length());

}

else if (_in.Type() == sdf::GeometryType::ELLIPSOID && _in.EllipsoidShape())

{

out.set_type(msgs::Geometry::ELLIPSOID);

msgs::Set(out.mutable_ellipsoid()->mutable_radii(),

_in.EllipsoidShape()->Radii());

}

else if (_in.Type() == sdf::GeometryType::PLANE && _in.PlaneShape())

{

out.set_type(msgs::Geometry::PLANE);

msgs::Set(out.mutable_plane()->mutable_normal(),

_in.PlaneShape()->Normal());

msgs::Set(out.mutable_plane()->mutable_size(),

_in.PlaneShape()->Size());

}

else if (_in.Type() == sdf::GeometryType::SPHERE && _in.SphereShape())

{

out.set_type(msgs::Geometry::SPHERE);

out.mutable_sphere()->set_radius(_in.SphereShape()->Radius());

}

else if (_in.Type() == sdf::GeometryType::MESH && _in.MeshShape())

{

auto meshSdf = _in.MeshShape();

out.set_type(msgs::Geometry::MESH);

auto meshMsg = out.mutable_mesh();

msgs::Set(meshMsg->mutable_scale(), meshSdf->Scale());

meshMsg->set_filename(asFullPath(meshSdf->Uri(), meshSdf->FilePath()));

meshMsg->set_submesh(meshSdf->Submesh());

meshMsg->set_center_submesh(meshSdf->CenterSubmesh());

}

else if (_in.Type() == sdf::GeometryType::HEIGHTMAP && _in.HeightmapShape())

{

auto heightmapSdf = _in.HeightmapShape();

out.set_type(msgs::Geometry::HEIGHTMAP);

auto heightmapMsg = out.mutable_heightmap();

heightmapMsg->set_filename(asFullPath(heightmapSdf->Uri(),

heightmapSdf->FilePath()));

msgs::Set(heightmapMsg->mutable_size(), heightmapSdf->Size());

msgs::Set(heightmapMsg->mutable_origin(), heightmapSdf->Position());

heightmapMsg->set_use_terrain_paging(heightmapSdf->UseTerrainPaging());

heightmapMsg->set_sampling(heightmapSdf->Sampling());

for (auto i = 0u; i < heightmapSdf->TextureCount(); ++i)

{

auto textureSdf = heightmapSdf->TextureByIndex(i);

auto textureMsg = heightmapMsg->add_texture();

textureMsg->set_size(textureSdf->Size());

textureMsg->set_diffuse(asFullPath(textureSdf->Diffuse(),

heightmapSdf->FilePath()));

textureMsg->set_normal(asFullPath(textureSdf->Normal(),

heightmapSdf->FilePath()));

}

for (auto i = 0u; i < heightmapSdf->BlendCount(); ++i)

{

auto blendSdf = heightmapSdf->BlendByIndex(i);

auto blendMsg = heightmapMsg->add_blend();

blendMsg->set_min_height(blendSdf->MinHeight());

blendMsg->set_fade_dist(blendSdf->FadeDistance());

}

}

else if (_in.Type() == sdf::GeometryType::POLYLINE &&

!_in.PolylineShape().empty())

{

out.set_type(msgs::Geometry::POLYLINE);

for (const auto &polyline : _in.PolylineShape())

{

auto polylineMsg = out.add_polyline();

polylineMsg->set_height(polyline.Height());

for (const auto &point : polyline.Points())

{

msgs::Set(polylineMsg->add_point(), point);

}

}

}

else

{

gzerr << "Geometry type [" << static_cast<int>(_in.Type())

<< "] not supported" << std::endl;

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Geometry gz::sim::convert(const msgs::Geometry &_in)

{

sdf::Geometry out;

if (_in.type() == msgs::Geometry::BOX && _in.has_box())

{

out.SetType(sdf::GeometryType::BOX);

sdf::Box boxShape;

boxShape.SetSize(msgs::Convert(_in.box().size()));

out.SetBoxShape(boxShape);

}

else if (_in.type() == msgs::Geometry::CAPSULE && _in.has_capsule())

{

out.SetType(sdf::GeometryType::CAPSULE);

sdf::Capsule capsuleShape;

capsuleShape.SetRadius(_in.capsule().radius());

capsuleShape.SetLength(_in.capsule().length());

out.SetCapsuleShape(capsuleShape);

}

else if (_in.type() == msgs::Geometry::CYLINDER && _in.has_cylinder())

{

out.SetType(sdf::GeometryType::CYLINDER);

sdf::Cylinder cylinderShape;

cylinderShape.SetRadius(_in.cylinder().radius());

cylinderShape.SetLength(_in.cylinder().length());

out.SetCylinderShape(cylinderShape);

}

else if (_in.type() == msgs::Geometry::ELLIPSOID && _in.has_ellipsoid())

{

out.SetType(sdf::GeometryType::ELLIPSOID);

sdf::Ellipsoid ellipsoidShape;

ellipsoidShape.SetRadii(msgs::Convert(_in.ellipsoid().radii()));

out.SetEllipsoidShape(ellipsoidShape);

}

else if (_in.type() == msgs::Geometry::PLANE && _in.has_plane())

{

out.SetType(sdf::GeometryType::PLANE);

sdf::Plane planeShape;

planeShape.SetNormal(msgs::Convert(_in.plane().normal()));

planeShape.SetSize(msgs::Convert(_in.plane().size()));

out.SetPlaneShape(planeShape);

}

else if (_in.type() == msgs::Geometry::SPHERE && _in.has_sphere())

{

out.SetType(sdf::GeometryType::SPHERE);

sdf::Sphere sphereShape;

sphereShape.SetRadius(_in.sphere().radius());

out.SetSphereShape(sphereShape);

}

else if (_in.type() == msgs::Geometry::MESH && _in.has_mesh())

{

out.SetType(sdf::GeometryType::MESH);

sdf::Mesh meshShape;

meshShape.SetScale(msgs::Convert(_in.mesh().scale()));

meshShape.SetUri(_in.mesh().filename());

meshShape.SetSubmesh(_in.mesh().submesh());

meshShape.SetCenterSubmesh(_in.mesh().center_submesh());

out.SetMeshShape(meshShape);

}

else if (_in.type() == msgs::Geometry::HEIGHTMAP && _in.has_heightmap())

{

out.SetType(sdf::GeometryType::HEIGHTMAP);

sdf::Heightmap heightmapShape;

heightmapShape.SetUri(_in.heightmap().filename());

heightmapShape.SetSize(msgs::Convert(_in.heightmap().size()));

heightmapShape.SetPosition(msgs::Convert(_in.heightmap().origin()));

heightmapShape.SetUseTerrainPaging(_in.heightmap().use_terrain_paging());

heightmapShape.SetSampling(_in.heightmap().sampling());

for (int i = 0; i < _in.heightmap().texture_size(); ++i)

{

auto textureMsg = _in.heightmap().texture(i);

sdf::HeightmapTexture textureSdf;

textureSdf.SetSize(textureMsg.size());

textureSdf.SetDiffuse(textureMsg.diffuse());

textureSdf.SetNormal(textureMsg.normal());

heightmapShape.AddTexture(textureSdf);

}

for (int i = 0; i < _in.heightmap().blend_size(); ++i)

{

auto blendMsg = _in.heightmap().blend(i);

sdf::HeightmapBlend blendSdf;

blendSdf.SetMinHeight(blendMsg.min_height());

blendSdf.SetFadeDistance(blendMsg.fade_dist());

heightmapShape.AddBlend(blendSdf);

}

out.SetHeightmapShape(heightmapShape);

}

else if (_in.type() == msgs::Geometry::POLYLINE && _in.polyline_size() > 0)

{

out.SetType(sdf::GeometryType::POLYLINE);

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

for (auto i = 0; i < _in.polyline().size(); ++i)

{

auto polylineMsg = _in.polyline(i);

sdf::Polyline polylineShape;

polylineShape.SetHeight(polylineMsg.height());

for (auto j = 0; j < polylineMsg.point().size(); ++j)

{

polylineShape.AddPoint(msgs::Convert(polylineMsg.point(j)));

}

polylines.push_back(polylineShape);

}

out.SetPolylineShape(polylines);

}

else

{

gzerr << "Geometry type [" << static_cast<int>(_in.type())

<< "] not supported" << std::endl;

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Material gz::sim::convert(const sdf::Material &_in)

{

msgs::Material out;

msgs::Set(out.mutable_ambient(), _in.Ambient());

msgs::Set(out.mutable_diffuse(), _in.Diffuse());

msgs::Set(out.mutable_specular(), _in.Specular());

out.set_shininess(_in.Shininess());

msgs::Set(out.mutable_emissive(), _in.Emissive());

out.set_render_order(_in.RenderOrder());

out.set_lighting(_in.Lighting());

out.set_double_sided(_in.DoubleSided());

auto pbr = _in.PbrMaterial();

if (pbr)

{

auto pbrMsg = out.mutable_pbr();

auto workflow = pbr->Workflow(sdf::PbrWorkflowType::METAL);

if (workflow)

pbrMsg->set_type(msgs::Material_PBR_WorkflowType_METAL);

else

{

workflow = pbr->Workflow(sdf::PbrWorkflowType::SPECULAR);

if (workflow)

pbrMsg->set_type(msgs::Material_PBR_WorkflowType_SPECULAR);

}

if (workflow)

{

pbrMsg->set_metalness(workflow->Metalness());

pbrMsg->set_metalness_map(workflow->MetalnessMap().empty() ? "" :

asFullPath(workflow->MetalnessMap(), _in.FilePath()));

pbrMsg->set_roughness(workflow->Roughness());

pbrMsg->set_roughness_map(workflow->RoughnessMap().empty() ? "" :

asFullPath(workflow->RoughnessMap(), _in.FilePath()));

pbrMsg->set_glossiness(workflow->Glossiness());

pbrMsg->set_glossiness_map(workflow->GlossinessMap().empty() ? "" :

asFullPath(workflow->GlossinessMap(), _in.FilePath()));

pbrMsg->set_specular_map(workflow->SpecularMap().empty() ? "" :

asFullPath(workflow->SpecularMap(), _in.FilePath()));

pbrMsg->set_albedo_map(workflow->AlbedoMap().empty() ? "" :

asFullPath(workflow->AlbedoMap(), _in.FilePath()));

pbrMsg->set_normal_map(workflow->NormalMap().empty() ? "" :

asFullPath(workflow->NormalMap(), _in.FilePath()));

pbrMsg->set_ambient_occlusion_map(

workflow->AmbientOcclusionMap().empty() ? "" :

asFullPath(workflow->AmbientOcclusionMap(), _in.FilePath()));

pbrMsg->set_environment_map(workflow->EnvironmentMap().empty() ? "" :

asFullPath(workflow->EnvironmentMap(), _in.FilePath()));

pbrMsg->set_emissive_map(workflow->EmissiveMap().empty() ? "" :

asFullPath(workflow->EmissiveMap(), _in.FilePath()));

pbrMsg->set_light_map(workflow->LightMap().empty() ? "" :

asFullPath(workflow->LightMap(), _in.FilePath()));

pbrMsg->set_light_map_texcoord_set(workflow->LightMapTexCoordSet());

}

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Material gz::sim::convert(const msgs::Material &_in)

{

sdf::Material out;

out.SetAmbient(msgs::Convert(_in.ambient()));

out.SetDiffuse(msgs::Convert(_in.diffuse()));

out.SetSpecular(msgs::Convert(_in.specular()));

out.SetShininess(_in.shininess());

out.SetEmissive(msgs::Convert(_in.emissive()));

out.SetRenderOrder(_in.render_order());

out.SetLighting(_in.lighting());

out.SetDoubleSided(_in.double_sided());

if (_in.has_pbr())

{

const auto &pbrMsg = _in.pbr();

sdf::Pbr pbr;

sdf::PbrWorkflow workflow;

if (pbrMsg.type() == msgs::Material_PBR_WorkflowType_METAL)

workflow.SetType(sdf::PbrWorkflowType::METAL);

else if (pbrMsg.type() == msgs::Material_PBR_WorkflowType_SPECULAR)

workflow.SetType(sdf::PbrWorkflowType::SPECULAR);

workflow.SetAlbedoMap(pbrMsg.albedo_map());

workflow.SetNormalMap(pbrMsg.normal_map());

workflow.SetMetalness(pbrMsg.metalness());

workflow.SetMetalnessMap(pbrMsg.metalness_map());

workflow.SetRoughness(pbrMsg.roughness());

workflow.SetRoughnessMap(pbrMsg.roughness_map());

workflow.SetGlossiness(pbrMsg.glossiness());

workflow.SetGlossinessMap(pbrMsg.glossiness_map());

workflow.SetSpecularMap(pbrMsg.specular_map());

workflow.SetEnvironmentMap(pbrMsg.environment_map());

workflow.SetAmbientOcclusionMap(pbrMsg.ambient_occlusion_map());

workflow.SetEmissiveMap(pbrMsg.emissive_map());

workflow.SetLightMap(pbrMsg.light_map(), pbrMsg.light_map_texcoord_set());

pbr.SetWorkflow(workflow.Type(), workflow);

out.SetPbrMaterial(pbr);

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Actor gz::sim::convert(const sdf::Actor &_in)

{

msgs::Actor out;

out.mutable_entity()->set_name(_in.Name());

msgs::Set(out.mutable_pose(), _in.RawPose());

out.set_skin_filename(asFullPath(_in.SkinFilename(), _in.FilePath()));

out.set_skin_scale(_in.SkinScale());

for (unsigned int i = 0; i < _in.AnimationCount(); ++i)

{

auto newAnim = out.add_animations();

auto anim = _in.AnimationByIndex(i);

newAnim->set_name(anim->Name());

newAnim->set_filename(asFullPath(anim->Filename(), anim->FilePath()));

newAnim->set_scale(anim->Scale());

newAnim->set_interpolate_x(anim->InterpolateX());

}

out.set_script_loop(_in.ScriptLoop());

out.set_script_delay_start(_in.ScriptDelayStart());

out.set_script_auto_start(_in.ScriptAutoStart());

for (unsigned int i = 0; i < _in.TrajectoryCount(); ++i)

{

auto newTraj = out.add_trajectories();

auto traj = _in.TrajectoryByIndex(i);

newTraj->set_id(traj->Id());

newTraj->set_type(traj->Type());

newTraj->set_tension(traj->Tension());

for (unsigned int j = 0; j < traj->WaypointCount(); ++j)

{

auto newPoint = newTraj->add_waypoints();

auto point = traj->WaypointByIndex(j);

newPoint->set_time(point->Time());

msgs::Set(newPoint->mutable_pose(), point->Pose());

}

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Actor gz::sim::convert(const msgs::Actor &_in)

{

sdf::Actor out;

out.SetName(_in.entity().name());

out.SetRawPose(msgs::Convert(_in.pose()));

out.SetSkinFilename(_in.skin_filename());

out.SetSkinScale(_in.skin_scale());

for (int i = 0; i < _in.animations_size(); ++i)

{

const auto &anim = _in.animations(i);

sdf::Animation newAnim;

newAnim.SetName(anim.name());

newAnim.SetFilename(anim.filename());

newAnim.SetScale(anim.scale());

newAnim.SetInterpolateX(anim.interpolate_x());

out.AddAnimation(newAnim);

}

out.SetScriptLoop(_in.script_loop());

out.SetScriptDelayStart(_in.script_delay_start());

out.SetScriptAutoStart(_in.script_auto_start());

for (int i = 0; i < _in.trajectories_size(); ++i)

{

const auto &traj = _in.trajectories(i);

sdf::Trajectory newTraj;

newTraj.SetId(traj.id());

newTraj.SetType(traj.type());

newTraj.SetTension(traj.tension());

for (int j = 0; j < traj.waypoints_size(); ++j)

{

const auto &point = traj.waypoints(j);

sdf::Waypoint newPoint;

newPoint.SetTime(point.time());

newPoint.SetPose(msgs::Convert(point.pose()));

newTraj.AddWaypoint(newPoint);

}

out.AddTrajectory(newTraj);

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Light gz::sim::convert(const sdf::Light &_in)

{

msgs::Light out;

out.set_name(_in.Name());

msgs::Set(out.mutable_pose(), _in.RawPose());

msgs::Set(out.mutable_diffuse(), _in.Diffuse());

msgs::Set(out.mutable_specular(), _in.Specular());

out.set_attenuation_constant(_in.ConstantAttenuationFactor());

out.set_attenuation_linear(_in.LinearAttenuationFactor());

out.set_attenuation_quadratic(_in.QuadraticAttenuationFactor());

out.set_range(_in.AttenuationRange());

out.set_intensity(_in.Intensity());

msgs::Set(out.mutable_direction(), _in.Direction());

out.set_cast_shadows(_in.CastShadows());

out.set_spot_inner_angle(_in.SpotInnerAngle().Radian());

out.set_spot_outer_angle(_in.SpotOuterAngle().Radian());

out.set_spot_falloff(_in.SpotFalloff());

out.set_is_light_off(!_in.LightOn());

out.set_visualize_visual(_in.Visualize());

if (_in.Type() == sdf::LightType::POINT)

out.set_type(msgs::Light_LightType_POINT);

else if (_in.Type() == sdf::LightType::SPOT)

out.set_type(msgs::Light_LightType_SPOT);

else if (_in.Type() == sdf::LightType::DIRECTIONAL)

out.set_type(msgs::Light_LightType_DIRECTIONAL);

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Light gz::sim::convert(const msgs::Light &_in)

{

sdf::Light out;

out.SetName(_in.name());

out.SetRawPose(msgs::Convert(_in.pose()));

out.SetDiffuse(msgs::Convert(_in.diffuse()));

out.SetSpecular(msgs::Convert(_in.specular()));

out.SetConstantAttenuationFactor(_in.attenuation_constant());

out.SetLinearAttenuationFactor(_in.attenuation_linear());

out.SetQuadraticAttenuationFactor(_in.attenuation_quadratic());

out.SetAttenuationRange(_in.range());

out.SetDirection(msgs::Convert(_in.direction()));

out.SetIntensity(_in.intensity());

out.SetCastShadows(_in.cast_shadows());

out.SetSpotInnerAngle(math::Angle(_in.spot_inner_angle()));

out.SetSpotOuterAngle(math::Angle(_in.spot_outer_angle()));

out.SetSpotFalloff(_in.spot_falloff());

out.SetLightOn(!_in.is_light_off());

out.SetVisualize(_in.visualize_visual());

if (_in.type() == msgs::Light_LightType_POINT)

out.SetType(sdf::LightType::POINT);

else if (_in.type() == msgs::Light_LightType_SPOT)

out.SetType(sdf::LightType::SPOT);

else if (_in.type() == msgs::Light_LightType_DIRECTIONAL)

out.SetType(sdf::LightType::DIRECTIONAL);

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::GUI gz::sim::convert(const sdf::Gui &_in)

{

msgs::GUI out;

out.set_fullscreen(_in.Fullscreen());

// Set gui plugins

for (uint64_t i = 0; i < _in.PluginCount(); ++i)

{

auto pluginMsg = out.add_plugin();

pluginMsg->CopyFrom(convert<msgs::Plugin>(*_in.PluginByIndex(i)));

}

if (_in.Element()->HasElement("camera"))

{

gzwarn << "<gui><camera> can't be converted yet" << std::endl;

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Time gz::sim::convert(

const std::chrono::steady_clock::duration &_in)

{

msgs::Time out;

auto secNsec = math::durationToSecNsec(_in);

out.set_sec(secNsec.first);

out.set_nsec(secNsec.second);

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

std::chrono::steady_clock::duration gz::sim::convert(

const msgs::Time &_in)

{

return std::chrono::seconds(_in.sec()) + std::chrono::nanoseconds(_in.nsec());

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Inertial gz::sim::convert(const math::Inertiald &_in)

{

return msgs::Convert(_in);

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

math::Inertiald gz::sim::convert(const msgs::Inertial &_in)

{

return msgs::Convert(_in);

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Axis gz::sim::convert(const sdf::JointAxis &_in)

{

msgs::Axis out;

msgs::Set(out.mutable_xyz(), _in.Xyz());

out.set_xyz_expressed_in(_in.XyzExpressedIn());

out.set_damping(_in.Damping());

out.set_friction(_in.Friction());

out.set_limit_lower(_in.Lower());

out.set_limit_upper(_in.Upper());

out.set_limit_effort(_in.Effort());

out.set_limit_velocity(_in.MaxVelocity());

// TODO(anyone) Only on SDF:

// * initial position

// * spring reference

// * spring stiffness

// * stiffness

// * dissipation

// Only on msg:

// * position

// * velocity

// * force

// * acceleration

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::JointAxis gz::sim::convert(const msgs::Axis &_in)

{

sdf::JointAxis out;

sdf::Errors errors = out.SetXyz(msgs::Convert(_in.xyz()));

for (const auto &err : errors) {

gzerr << err.Message() << std::endl;

}

out.SetXyzExpressedIn(_in.xyz_expressed_in());

out.SetDamping(_in.damping());

out.SetFriction(_in.friction());

out.SetLower(_in.limit_lower());

out.SetUpper(_in.limit_upper());

out.SetEffort(_in.limit_effort());

out.SetMaxVelocity(_in.limit_velocity());

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Scene gz::sim::convert(const sdf::Scene &_in)

{

msgs::Scene out;

// todo(anyone) add Name to sdf::Scene?

// out.set_name(_in.Name());

msgs::Set(out.mutable_ambient(), _in.Ambient());

msgs::Set(out.mutable_background(), _in.Background());

out.set_shadows(_in.Shadows());

out.set_grid(_in.Grid());

out.set_origin_visual(_in.OriginVisual());

if (_in.Sky())

{

msgs::Sky *skyMsg = out.mutable_sky();

skyMsg->set_time(_in.Sky()->Time());

skyMsg->set_sunrise(_in.Sky()->Sunrise());

skyMsg->set_sunset(_in.Sky()->Sunset());

skyMsg->set_wind_speed(_in.Sky()->CloudSpeed());

skyMsg->set_wind_direction(_in.Sky()->CloudDirection().Radian());

skyMsg->set_humidity(_in.Sky()->CloudHumidity());

skyMsg->set_mean_cloud_size(_in.Sky()->CloudMeanSize());

skyMsg->set_cubemap_uri(_in.Sky()->CubemapUri());

msgs::Set(skyMsg->mutable_cloud_ambient(),

_in.Sky()->CloudAmbient());

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Scene gz::sim::convert(const msgs::Scene &_in)

{

sdf::Scene out;

// todo(anyone) add SetName to sdf::Scene?

// out.SetName(_in.name());

out.SetAmbient(msgs::Convert(_in.ambient()));

out.SetBackground(msgs::Convert(_in.background()));

out.SetShadows(_in.shadows());

out.SetGrid(_in.grid());

out.SetOriginVisual(_in.origin_visual());

if (_in.has_sky())

{

sdf::Sky sky;

sky.SetTime(_in.sky().time());

sky.SetSunrise(_in.sky().sunrise());

sky.SetSunset(_in.sky().sunset());

sky.SetCloudSpeed(_in.sky().wind_speed());

sky.SetCloudDirection(math::Angle(_in.sky().wind_direction()));

sky.SetCloudHumidity(_in.sky().humidity());

sky.SetCloudMeanSize(_in.sky().mean_cloud_size());

sky.SetCloudAmbient(msgs::Convert(_in.sky().cloud_ambient()));

sky.SetCubemapUri(_in.sky().cubemap_uri());

out.SetSky(sky);

}

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Atmosphere gz::sim::convert(const sdf::Atmosphere &_in)

{

msgs::Atmosphere out;

out.set_temperature(_in.Temperature().Kelvin());

out.set_pressure(_in.Pressure());

if (_in.Type() == sdf::AtmosphereType::ADIABATIC)

{

out.set_type(msgs::Atmosphere::ADIABATIC);

}

// todo(anyone) add mass density to sdf::Atmosphere?

// out.set_mass_density(_in.MassDensity());k

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Atmosphere gz::sim::convert(const msgs::Atmosphere &_in)

{

sdf::Atmosphere out;

out.SetTemperature(math::Temperature(_in.temperature()));

out.SetPressure(_in.pressure());

// todo(anyone) add temperature gradient to msgs::Atmosphere?

if (_in.type() == msgs::Atmosphere::ADIABATIC)

{

out.SetType(sdf::AtmosphereType::ADIABATIC);

}

return out;

}

//////////////////////////////////////////////////

void gz::sim::set(msgs::Time *_msg,

const std::chrono::steady_clock::duration &_in)

{

auto secNsec = math::durationToSecNsec(_in);

_msg->set_sec(secNsec.first);

_msg->set_nsec(secNsec.second);

}

//////////////////////////////////////////////////

void gz::sim::set(msgs::WorldStatistics *_msg,

const sim::UpdateInfo &_in)

{

set(_msg->mutable_sim_time(), _in.simTime);

set(_msg->mutable_real_time(), _in.realTime);

set(_msg->mutable_step_size(), _in.dt);

_msg->set_iterations(_in.iterations);

_msg->set_paused(_in.paused);

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

msgs::Physics gz::sim::convert(const sdf::Physics &_in)

{

msgs::Physics out;

out.set_max_step_size(_in.MaxStepSize());

out.set_real_time_factor(_in.RealTimeFactor());

return out;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Physics gz::sim::convert(const msgs::Physics &_in)

{

sdf::Physics out;

out.SetRealTimeFactor(_in.real_time_factor());

out.SetMaxStepSize(_in.max_step_size());

return out;

}

//////////////////////////////////////////////////

void gz::sim::set(msgs::SensorNoise *_msg, const sdf::Noise &_sdf)

{

switch (_sdf.Type())

{

case sdf::NoiseType::GAUSSIAN:

_msg->set_type(msgs::SensorNoise::GAUSSIAN);

break;

case sdf::NoiseType::GAUSSIAN_QUANTIZED:

_msg->set_type(msgs::SensorNoise::GAUSSIAN_QUANTIZED);

break;

case sdf::NoiseType::NONE:

default:

_msg->set_type(msgs::SensorNoise::NONE);

break;

}

_msg->set_mean(_sdf.Mean());

_msg->set_stddev(_sdf.StdDev());

_msg->set_bias_mean(_sdf.BiasMean());

_msg->set_bias_stddev(_sdf.BiasStdDev());

_msg->set_precision(_sdf.Precision());

_msg->set_dynamic_bias_stddev(_sdf.DynamicBiasStdDev());

_msg->set_dynamic_bias_correlation_time(_sdf.DynamicBiasCorrelationTime());

}

//////////////////////////////////////////////////

std::string gz::sim::convert(const sdf::LightType &_in)

{

if (_in == sdf::LightType::POINT)

{

return std::string("point");

}

else if (_in == sdf::LightType::DIRECTIONAL)

{

return std::string("directional");

}

else if (_in == sdf::LightType::SPOT)

{

return std::string("spot");

}

return std::string("");

}

//////////////////////////////////////////////////

sdf::LightType gz::sim::convert(const std::string &_in)

{

std::string inLowerCase = _in;

std::transform(_in.begin(), _in.end(), inLowerCase.begin(), ::tolower);

if (inLowerCase == "point")

{

return sdf::LightType::POINT;

}

else if (inLowerCase == "directional")

{

return sdf::LightType::DIRECTIONAL;

}

else if (inLowerCase == "spot")

{

return sdf::LightType::SPOT;

}

return sdf::LightType::INVALID;

}

//////////////////////////////////////////////////

template<>

GZ_SIM_VISIBLE

sdf::Noise gz::sim::convert(const msgs::SensorNoise &_in)

{

sdf::Noise out;

switch (_in.type())

{

case msgs::SensorNoise::GAUSSIAN:

out.SetType(sdf::NoiseType::GAUSSIAN);

break;

case msgs::SensorNoise::GAUSSIAN_QUANTIZED:

out.SetType(sdf::NoiseType::GAUSSIAN_QUANTIZED);

break;

case msgs::SensorNoise::NONE:

default:

out.SetType(sdf::NoiseType::NONE);

break;