30 #include <gazebo/common/Events.hh> 31 #include <gazebo/physics/physics.hh> 60 world = _model->GetWorld();
61 link = _model->GetLink();
64 if (_sdf->HasElement(
"bodyName"))
66 link_name_ = _sdf->GetElement(
"bodyName")->Get<std::string>();
72 ROS_FATAL(
"gazebo_ros_baro plugin error: bodyName: %s does not exist\n",
link_name_.c_str());
85 if (_sdf->HasElement(
"robotNamespace"))
namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
86 if (_sdf->HasElement(
"frameId"))
frame_id_ = _sdf->GetElement(
"frameId")->Get<std::string>();
87 if (_sdf->HasElement(
"topicName"))
height_topic_ = _sdf->GetElement(
"topicName")->Get<std::string>();
88 if (_sdf->HasElement(
"altimeterTopicName"))
altimeter_topic_ = _sdf->GetElement(
"altimeterTopicName")->Get<std::string>();
89 if (_sdf->HasElement(
"elevation"))
elevation_ = _sdf->GetElement(
"elevation")->Get<
double>();
90 if (_sdf->HasElement(
"qnh"))
qnh_ = _sdf->GetElement(
"qnh")->Get<
double>();
100 ROS_FATAL_STREAM(
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 101 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
138 #if (GAZEBO_MAJOR_VERSION >= 8) 139 common::Time sim_time =
world->SimTime();
141 common::Time sim_time =
world->GetSimTime();
145 #if (GAZEBO_MAJOR_VERSION >= 8) 146 ignition::math::Pose3d pose =
link->WorldPose();
149 math::Pose pose =
link->GetWorldPose();
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_
virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf, const std::string &_prefix="update")
void publish(const boost::shared_ptr< M > &message) const
static const double DEFAULT_QNH
virtual void Load(sdf::ElementPtr _sdf, const std::string &prefix=std::string())
ROSCPP_DECL bool isInitialized()
physics::WorldPtr world
The parent World.
static const double DEFAULT_ELEVATION
virtual event::ConnectionPtr Connect(const boost::function< void()> &_subscriber, bool connectToWorldUpdateBegin=true)
geometry_msgs::PointStamped height_
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
ros::Publisher height_publisher_
hector_uav_msgs::Altimeter altimeter_
ros::NodeHandle * node_handle_
#define ROS_FATAL_STREAM(args)
virtual void Disconnect(event::ConnectionPtr const &_c=event::ConnectionPtr())
virtual void dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level)
common::Time getTimeSinceLastUpdate() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
physics::LinkPtr link
The link referred to by this plugin.
SensorModel sensor_model_
std::string height_topic_
ros::Publisher altimeter_publisher_
event::ConnectionPtr updateConnection
std::string altimeter_topic_