p9fj35fl6/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/sensors/WirelessTransceiver.cc

134 lines
3.4 KiB
C++

/*
* 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.
*
*/
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
#include <boost/algorithm/string.hpp>
#include <sstream>
#include "gazebo/msgs/msgs.hh"
#include "gazebo/sensors/SensorFactory.hh"
#include "gazebo/sensors/SensorManager.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
#include "gazebo/sensors/WirelessTransceiver.hh"
using namespace gazebo;
using namespace sensors;
/////////////////////////////////////////////////
WirelessTransceiver::WirelessTransceiver()
: Sensor(sensors::OTHER)
{
this->active = false;
}
/////////////////////////////////////////////////
WirelessTransceiver::~WirelessTransceiver()
{
}
//////////////////////////////////////////////////
std::string WirelessTransceiver::Topic() const
{
std::string topicName = "~/";
topicName += this->ParentName() + "/" + this->Name() + "/transceiver";
boost::replace_all(topicName, "::", "/");
return topicName;
}
//////////////////////////////////////////////////
void WirelessTransceiver::Load(const std::string &_worldName)
{
Sensor::Load(_worldName);
this->parentEntity = boost::dynamic_pointer_cast<physics::Link>(
this->world->GetEntity(this->ParentName()));
GZ_ASSERT(this->parentEntity.lock() != NULL, "parentEntity is NULL");
this->referencePose = this->pose +
this->parentEntity.lock()->GetWorldPose().Ign();
if (!this->sdf->HasElement("transceiver"))
{
gzthrow("Transceiver sensor is missing <transceiver> SDF element");
}
sdf::ElementPtr transceiverElem =
this->sdf->GetElement("transceiver");
this->gain = transceiverElem->Get<double>("gain");
this->power = transceiverElem->Get<double>("power");
if (this->gain < 0)
{
gzerr << "Attempting to set a negative gain of [" <<
this->gain << "]. Using a value of 1.\n";
this->gain = 1;
}
if (this->power < 0)
{
gzerr << "Attempting to set a negative transceiver power of[" <<
this->power << "]. Using a value of 1.\n";
this->power = 1;
}
}
//////////////////////////////////////////////////
void WirelessTransceiver::Init()
{
Sensor::Init();
}
/////////////////////////////////////////////////
void WirelessTransceiver::Fini()
{
this->pub.reset();
this->parentEntity.lock().reset();
Sensor::Fini();
}
/////////////////////////////////////////////////
double WirelessTransceiver::GetPower() const
{
return this->Power();
}
/////////////////////////////////////////////////
double WirelessTransceiver::Power() const
{
return this->power;
}
/////////////////////////////////////////////////
double WirelessTransceiver::GetGain() const
{
return this->Gain();
}
/////////////////////////////////////////////////
double WirelessTransceiver::Gain() const
{
return this->gain;
}