Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032
00033 static const double DEFAULT_ELEVATION = 0.0;
00034 static const double DEFAULT_QNH = 1013.25;
00035
00036 namespace gazebo {
00037
00038 GazeboRosBaro::GazeboRosBaro()
00039 {
00040 }
00041
00043
00044 GazeboRosBaro::~GazeboRosBaro()
00045 {
00046 updateTimer.Disconnect(updateConnection);
00047
00048 dynamic_reconfigure_server_.reset();
00049
00050 node_handle_->shutdown();
00051 delete node_handle_;
00052 }
00053
00055
00056 void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00057 {
00058 world = _model->GetWorld();
00059 link = _model->GetLink();
00060 link_name_ = link->GetName();
00061
00062 if (_sdf->HasElement("bodyName"))
00063 {
00064 link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
00065 link = _model->GetLink(link_name_);
00066 }
00067
00068 if (!link)
00069 {
00070 ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00071 return;
00072 }
00073
00074
00075 namespace_.clear();
00076 frame_id_ = link->GetName();
00077 height_topic_ = "pressure_height";
00078 altimeter_topic_ = "altimeter";
00079 elevation_ = DEFAULT_ELEVATION;
00080 qnh_ = DEFAULT_QNH;
00081
00082
00083 if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00084 if (_sdf->HasElement("frameId")) frame_id_ = _sdf->GetElement("frameId")->Get<std::string>();
00085 if (_sdf->HasElement("topicName")) height_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
00086 if (_sdf->HasElement("altimeterTopicName")) altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->Get<std::string>();
00087 if (_sdf->HasElement("elevation")) elevation_ = _sdf->GetElement("elevation")->Get<double>();
00088 if (_sdf->HasElement("qnh")) qnh_ = _sdf->GetElement("qnh")->Get<double>();
00089
00090
00091 sensor_model_.Load(_sdf);
00092
00093 height_.header.frame_id = frame_id_;
00094
00095
00096 if (!ros::isInitialized())
00097 {
00098 ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00099 << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00100 return;
00101 }
00102
00103 node_handle_ = new ros::NodeHandle(namespace_);
00104
00105
00106 if (!height_topic_.empty()) {
00107 height_publisher_ = node_handle_->advertise<geometry_msgs::PointStamped>(height_topic_, 10);
00108 }
00109
00110
00111 if (!altimeter_topic_.empty()) {
00112 altimeter_publisher_ = node_handle_->advertise<hector_uav_msgs::Altimeter>(altimeter_topic_, 10);
00113 }
00114
00115
00116 dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, altimeter_topic_)));
00117 dynamic_reconfigure_server_->setCallback(boost::bind(&SensorModel::dynamicReconfigureCallback, &sensor_model_, _1, _2));
00118
00119 Reset();
00120
00121
00122 updateTimer.Load(world, _sdf);
00123 updateConnection = updateTimer.Connect(boost::bind(&GazeboRosBaro::Update, this));
00124 }
00125
00126 void GazeboRosBaro::Reset()
00127 {
00128 updateTimer.Reset();
00129 sensor_model_.reset();
00130 }
00131
00133
00134 void GazeboRosBaro::Update()
00135 {
00136 common::Time sim_time = world->GetSimTime();
00137 double dt = updateTimer.getTimeSinceLastUpdate().Double();
00138
00139 math::Pose pose = link->GetWorldPose();
00140 double height = sensor_model_(pose.pos.z, dt);
00141
00142 if (height_publisher_) {
00143 height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00144 height_.point.z = height;
00145 height_publisher_.publish(height_);
00146 }
00147
00148 if (altimeter_publisher_) {
00149 altimeter_.header = height_.header;
00150 altimeter_.altitude = height + elevation_;
00151 altimeter_.pressure = pow((1.0 - altimeter_.altitude / 44330.0), 5.263157) * qnh_;
00152 altimeter_.qnh = qnh_;
00153 altimeter_publisher_.publish(altimeter_);
00154 }
00155 }
00156
00157
00158 GZ_REGISTER_MODEL_PLUGIN(GazeboRosBaro)
00159
00160 }