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

705 lines
21 KiB
C++
Raw Normal View History

first
2019-03-28 10:57:49 +08:00
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/common/Events.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/DynamicLines.hh"
#include "gazebo/rendering/Visual.hh"
#include "gazebo/rendering/Light.hh"
#include "gazebo/rendering/LightPrivate.hh"
using namespace gazebo;
using namespace rendering;
unsigned int LightPrivate::lightCounter = 0;
//////////////////////////////////////////////////
Light::Light(ScenePtr _scene)
: dataPtr(new LightPrivate)
{
this->dataPtr->line = NULL;
this->dataPtr->scene = _scene;
this->dataPtr->lightCounter++;
this->dataPtr->sdf.reset(new sdf::Element);
sdf::initFile("light.sdf", this->dataPtr->sdf);
}
//////////////////////////////////////////////////
Light::~Light()
{
if (this->dataPtr->light)
{
this->dataPtr->scene->OgreSceneManager()->destroyLight(this->Name());
}
this->dataPtr->scene->OgreSceneManager()->destroyEntity(
this->Name() + "_selection_sphere");
if (this->dataPtr->visual)
{
this->dataPtr->visual->DeleteDynamicLine(this->dataPtr->line);
this->dataPtr->scene->RemoveVisual(this->dataPtr->visual);
this->dataPtr->visual.reset();
}
this->dataPtr->sdf->Reset();
this->dataPtr->sdf.reset();
this->dataPtr->scene.reset();
}
//////////////////////////////////////////////////
void Light::Load(sdf::ElementPtr _sdf)
{
this->dataPtr->sdf->Copy(_sdf);
this->Load();
this->dataPtr->scene->AddLight(shared_from_this());
}
//////////////////////////////////////////////////
void Light::Load()
{
math::Vector3 vec;
try
{
this->dataPtr->light =
this->dataPtr->scene->OgreSceneManager()->createLight(this->Name());
}
catch(Ogre::Exception &e)
{
gzthrow("Ogre Error:" << e.getFullDescription() << "\n" << \
"Unable to create a light");
}
this->Update();
this->dataPtr->visual.reset(new Visual(this->Name(),
this->dataPtr->scene->WorldVisual(), false));
this->dataPtr->visual->AttachObject(this->dataPtr->light);
this->dataPtr->scene->AddVisual(this->dataPtr->visual);
this->CreateVisual();
}
//////////////////////////////////////////////////
void Light::Update()
{
this->SetCastShadows(this->dataPtr->sdf->Get<bool>("cast_shadows"));
this->SetLightType(this->dataPtr->sdf->Get<std::string>("type"));
this->SetDiffuseColor(
this->dataPtr->sdf->GetElement("diffuse")->Get<common::Color>());
this->SetSpecularColor(
this->dataPtr->sdf->GetElement("specular")->Get<common::Color>());
this->SetDirection(
this->dataPtr->sdf->Get<ignition::math::Vector3d>("direction"));
if (this->dataPtr->sdf->HasElement("attenuation"))
{
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("attenuation");
this->SetAttenuation(elem->Get<double>("constant"),
elem->Get<double>("linear"),
elem->Get<double>("quadratic"));
this->SetRange(elem->Get<double>("range"));
}
if (this->dataPtr->sdf->HasElement("spot"))
{
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("spot");
this->SetSpotInnerAngle(elem->Get<double>("inner_angle"));
this->SetSpotOuterAngle(elem->Get<double>("outer_angle"));
this->SetSpotFalloff(elem->Get<double>("falloff"));
}
}
//////////////////////////////////////////////////
void Light::UpdateSDFFromMsg(const msgs::Light &_msg)
{
msgs::LightToSDF(_msg, this->dataPtr->sdf);
}
//////////////////////////////////////////////////
void Light::UpdateFromMsg(ConstLightPtr &_msg)
{
this->UpdateSDFFromMsg(*_msg);
this->Update();
if (_msg->has_pose())
{
this->SetPosition(msgs::ConvertIgn(_msg->pose().position()));
this->SetRotation(msgs::ConvertIgn(_msg->pose().orientation()));
}
}
//////////////////////////////////////////////////
void Light::LoadFromMsg(const msgs::Light &_msg)
{
this->UpdateSDFFromMsg(_msg);
this->Load();
if (_msg.has_pose())
{
this->SetPosition(msgs::ConvertIgn(_msg.pose().position()));
this->SetRotation(msgs::ConvertIgn(_msg.pose().orientation()));
}
}
//////////////////////////////////////////////////
void Light::LoadFromMsg(ConstLightPtr &_msg)
{
this->LoadFromMsg(*_msg);
}
//////////////////////////////////////////////////
void Light::SetName(const std::string &_name)
{
this->dataPtr->sdf->GetAttribute("name")->Set(_name);
}
//////////////////////////////////////////////////
std::string Light::GetName() const
{
return this->Name();
}
//////////////////////////////////////////////////
std::string Light::Name() const
{
return this->dataPtr->sdf->Get<std::string>("name");
}
//////////////////////////////////////////////////
std::string Light::GetType() const
{
return this->Type();
}
//////////////////////////////////////////////////
std::string Light::Type() const
{
return this->dataPtr->sdf->Get<std::string>("type");
}
//////////////////////////////////////////////////
// The lines draw a visualization of the camera
void Light::CreateVisual()
{
if (!this->dataPtr->visual)
return;
if (this->dataPtr->line)
this->dataPtr->line->Clear();
else
{
this->dataPtr->line =
this->dataPtr->visual->CreateDynamicLine(RENDERING_LINE_LIST);
this->dataPtr->line->setMaterial("Gazebo/LightOn");
this->dataPtr->line->setVisibilityFlags(GZ_VISIBILITY_GUI);
this->dataPtr->visual->SetVisible(true);
// Create a visual to hold the light selection object.
VisualPtr lightSelectionVis(new Visual(this->Name() + "_selection",
this->dataPtr->visual, false));
lightSelectionVis->Load();
lightSelectionVis->SetType(Visual::VT_GUI);
// Make sure the unit_sphere has been inserted.
lightSelectionVis->InsertMesh("unit_sphere");
lightSelectionVis->AttachMesh("unit_sphere");
lightSelectionVis->SetMaterial("Gazebo/White");
// Make sure the selection object is rendered only in the selection
// buffer.
lightSelectionVis->SetVisibilityFlags(GZ_VISIBILITY_SELECTABLE);
lightSelectionVis->SetCastShadows(false);
// Scale the selection object to roughly match the light visual size.
lightSelectionVis->SetScale(ignition::math::Vector3d(0.25, 0.25, 0.25));
}
std::string lightType = this->dataPtr->sdf->Get<std::string>("type");
if (lightType == "directional")
{
float s =.5;
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, -s));
}
if (lightType == "point")
{
float s = 0.1;
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, s));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, s));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, s));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, s));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, -s));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(-s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, -s));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, -s));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(s, -s, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, -s));
}
else if (lightType == "spot")
{
double innerAngle =
this->dataPtr->light->getSpotlightInnerAngle().valueRadians();
double outerAngle =
this->dataPtr->light->getSpotlightOuterAngle().valueRadians();
double angles[2];
double range = 0.2;
angles[0] = range * tan(outerAngle);
angles[1] = range * tan(innerAngle);
unsigned int i = 0;
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, 0));
this->dataPtr->line->AddPoint(
ignition::math::Vector3d(angles[i], angles[i], -range));
for (i = 0; i < 2; i++)
{
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
angles[i], angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
-angles[i], -angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
angles[i], -angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(0, 0, 0));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
-angles[i], angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
angles[i], angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
-angles[i], angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
-angles[i], angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
-angles[i], -angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
-angles[i], -angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
angles[i], -angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
angles[i], -angles[i], -range));
this->dataPtr->line->AddPoint(ignition::math::Vector3d(
angles[i], angles[i], -range));
}
}
}
//////////////////////////////////////////////////
void Light::SetPosition(const math::Vector3 &_p)
{
this->SetPosition(_p.Ign());
}
//////////////////////////////////////////////////
void Light::SetPosition(const ignition::math::Vector3d &_p)
{
this->dataPtr->visual->SetPosition(_p);
}
//////////////////////////////////////////////////
math::Vector3 Light::GetPosition() const
{
return this->Position();
}
//////////////////////////////////////////////////
ignition::math::Vector3d Light::Position() const
{
return this->dataPtr->visual->GetPosition().Ign();
}
//////////////////////////////////////////////////
void Light::SetRotation(const math::Quaternion &_q)
{
this->SetRotation(_q.Ign());
}
//////////////////////////////////////////////////
void Light::SetRotation(const ignition::math::Quaterniond &_q)
{
this->dataPtr->visual->SetRotation(_q);
}
//////////////////////////////////////////////////
math::Quaternion Light::GetRotation() const
{
return this->Rotation();
}
//////////////////////////////////////////////////
ignition::math::Quaterniond Light::Rotation() const
{
return this->dataPtr->visual->GetRotation().Ign();
}
//////////////////////////////////////////////////
bool Light::SetSelected(const bool _s)
{
if (this->dataPtr->light->getType() != Ogre::Light::LT_DIRECTIONAL)
{
if (_s)
this->dataPtr->line->setMaterial("Gazebo/PurpleGlow");
else
this->dataPtr->line->setMaterial("Gazebo/LightOn");
}
return true;
}
//////////////////////////////////////////////////
void Light::ToggleShowVisual()
{
this->dataPtr->visual->ToggleVisible();
}
//////////////////////////////////////////////////
void Light::ShowVisual(const bool _s)
{
this->dataPtr->visual->SetVisible(_s);
}
//////////////////////////////////////////////////
bool Light::GetVisible() const
{
return this->Visible();
}
//////////////////////////////////////////////////
bool Light::Visible() const
{
return this->dataPtr->visual->GetVisible();
}
//////////////////////////////////////////////////
void Light::SetLightType(const std::string &_type)
{
// Set the light _type
if (_type == "point")
this->dataPtr->light->setType(Ogre::Light::LT_POINT);
else if (_type == "directional")
this->dataPtr->light->setType(Ogre::Light::LT_DIRECTIONAL);
else if (_type == "spot")
this->dataPtr->light->setType(Ogre::Light::LT_SPOTLIGHT);
else
{
gzerr << "Unknown light type[" << _type << "]\n";
}
if (this->dataPtr->sdf->Get<std::string>("type") != _type)
this->dataPtr->sdf->GetAttribute("type")->Set(_type);
this->CreateVisual();
}
//////////////////////////////////////////////////
void Light::SetDiffuseColor(const common::Color &_color)
{
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("diffuse");
if (_color != elem->Get<common::Color>())
elem->Set(_color);
this->dataPtr->light->setDiffuseColour(_color.r, _color.g, _color.b);
}
//////////////////////////////////////////////////
common::Color Light::GetDiffuseColor() const
{
return this->DiffuseColor();
}
//////////////////////////////////////////////////
common::Color Light::DiffuseColor() const
{
return this->dataPtr->sdf->GetElement("diffuse")->Get<common::Color>();
}
//////////////////////////////////////////////////
common::Color Light::GetSpecularColor() const
{
return this->SpecularColor();
}
//////////////////////////////////////////////////
common::Color Light::SpecularColor() const
{
return this->dataPtr->sdf->GetElement("specular")->Get<common::Color>();
}
//////////////////////////////////////////////////
void Light::SetSpecularColor(const common::Color &_color)
{
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("specular");
if (elem->Get<common::Color>() != _color)
elem->Set(_color);
this->dataPtr->light->setSpecularColour(_color.r, _color.g, _color.b);
}
//////////////////////////////////////////////////
void Light::SetDirection(const math::Vector3 &_dir)
{
this->SetDirection(_dir.Ign());
}
//////////////////////////////////////////////////
void Light::SetDirection(const ignition::math::Vector3d &_dir)
{
// Set the direction which the light points
math::Vector3 vec = _dir;
vec.Normalize();
if (vec != this->dataPtr->sdf->Get<math::Vector3>("direction"))
this->dataPtr->sdf->GetElement("direction")->Set(vec);
this->dataPtr->light->setDirection(vec.x, vec.y, vec.z);
}
//////////////////////////////////////////////////
math::Vector3 Light::GetDirection() const
{
return this->Direction();
}
//////////////////////////////////////////////////
ignition::math::Vector3d Light::Direction() const
{
return this->dataPtr->sdf->Get<ignition::math::Vector3d>("direction");
}
//////////////////////////////////////////////////
void Light::SetAttenuation(double constant, double linear, double quadratic)
{
// Constant factor. 1.0 means never attenuate, 0.0 is complete attenuation
if (constant < 0)
constant = 0;
else if (constant > 1.0)
constant = 1.0;
// Linear factor. 1 means attenuate evenly over the distance
if (linear < 0)
linear = 0;
else if (linear > 1.0)
linear = 1.0;
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("attenuation");
elem->GetElement("constant")->Set(constant);
elem->GetElement("linear")->Set(linear);
elem->GetElement("quadratic")->Set(quadratic);
// Set attenuation
this->dataPtr->light->setAttenuation(elem->Get<double>("range"),
constant, linear, quadratic);
}
//////////////////////////////////////////////////
void Light::SetRange(const double _range)
{
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("attenuation");
elem->GetElement("range")->Set(_range);
this->dataPtr->light->setAttenuation(elem->Get<double>("range"),
elem->Get<double>("constant"),
elem->Get<double>("linear"),
elem->Get<double>("quadratic"));
}
//////////////////////////////////////////////////
void Light::SetCastShadows(const bool /*_cast*/)
{
this->dataPtr->light->setCastShadows(true);
/*if (this->dataPtr->light->getType() == Ogre::Light::LT_SPOTLIGHT ||
this->dataPtr->light->getType() == Ogre::Light::LT_DIRECTIONAL)
{
this->dataPtr->light->setCastShadows(_cast);
}
else
{
this->dataPtr->light->setCastShadows(false);
}*/
}
//////////////////////////////////////////////////
void Light::SetSpotInnerAngle(const double _angle)
{
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("spot");
elem->GetElement("inner_angle")->Set(_angle);
if (this->dataPtr->light->getType() == Ogre::Light::LT_SPOTLIGHT)
{
this->dataPtr->light->setSpotlightRange(
Ogre::Radian(elem->Get<double>("inner_angle")),
Ogre::Radian(elem->Get<double>("outer_angle")),
elem->Get<double>("falloff"));
}
}
//////////////////////////////////////////////////
void Light::SetSpotOuterAngle(const double _angle)
{
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("spot");
elem->GetElement("outer_angle")->Set(_angle);
if (this->dataPtr->light->getType() == Ogre::Light::LT_SPOTLIGHT)
{
this->dataPtr->light->setSpotlightRange(
Ogre::Radian(elem->Get<double>("inner_angle")),
Ogre::Radian(elem->Get<double>("outer_angle")),
elem->Get<double>("falloff"));
}
}
//////////////////////////////////////////////////
void Light::SetSpotFalloff(const double _angle)
{
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("spot");
elem->GetElement("falloff")->Set(_angle);
if (this->dataPtr->light->getType() == Ogre::Light::LT_SPOTLIGHT)
{
this->dataPtr->light->setSpotlightRange(
Ogre::Radian(elem->Get<double>("inner_angle")),
Ogre::Radian(elem->Get<double>("outer_angle")),
elem->Get<double>("falloff"));
}
}
//////////////////////////////////////////////////
void Light::FillMsg(msgs::Light &_msg) const
{
std::string lightType = this->dataPtr->sdf->Get<std::string>("type");
_msg.set_name(this->Name());
if (lightType == "point")
_msg.set_type(msgs::Light::POINT);
else if (lightType == "spot")
_msg.set_type(msgs::Light::SPOT);
else if (lightType == "directional")
_msg.set_type(msgs::Light::DIRECTIONAL);
msgs::Set(_msg.mutable_pose()->mutable_position(), this->Position());
msgs::Set(_msg.mutable_pose()->mutable_orientation(),
this->Rotation());
msgs::Set(_msg.mutable_diffuse(), this->DiffuseColor());
msgs::Set(_msg.mutable_specular(), this->SpecularColor());
msgs::Set(_msg.mutable_direction(), this->Direction());
_msg.set_cast_shadows(this->dataPtr->light->getCastShadows());
sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("attenuation");
_msg.set_attenuation_constant(elem->Get<double>("constant"));
_msg.set_attenuation_linear(elem->Get<double>("linear"));
_msg.set_attenuation_quadratic(elem->Get<double>("quadratic"));
_msg.set_range(elem->Get<double>("range"));
if (lightType == "spot")
{
elem = this->dataPtr->sdf->GetElement("spot");
_msg.set_spot_inner_angle(elem->Get<double>("inner_angle"));
_msg.set_spot_outer_angle(elem->Get<double>("outer_angle"));
_msg.set_spot_falloff(elem->Get<double>("falloff"));
}
}
//////////////////////////////////////////////////
LightPtr Light::Clone(const std::string &_name, ScenePtr _scene)
{
LightPtr result(new Light(_scene));
sdf::ElementPtr sdfCopy(new sdf::Element);
sdfCopy->Copy(this->dataPtr->sdf);
sdfCopy->GetAttribute("name")->Set(_name);
result->Load(sdfCopy);
result->SetPosition(this->Position());
result->SetRotation(this->Rotation());
return result;
}