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 node_handle_->shutdown();
00048 delete node_handle_;
00049 }
00050
00052
00053 void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00054 {
00055 world = _model->GetWorld();
00056
00057
00058 if (!_sdf->HasElement("robotNamespace"))
00059 namespace_.clear();
00060 else
00061 namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00062
00063 if (!_sdf->HasElement("bodyName"))
00064 {
00065 link = _model->GetLink();
00066 link_name_ = link->GetName();
00067 }
00068 else {
00069 link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00070 link = _model->GetLink(link_name_);
00071 }
00072
00073 if (!link)
00074 {
00075 ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00076 return;
00077 }
00078
00079 if (!_sdf->HasElement("frameId"))
00080 frame_id_ = link->GetName();
00081 else
00082 frame_id_ = _sdf->GetElement("frameId")->GetValueString();
00083
00084 if (!_sdf->HasElement("topicName"))
00085 height_topic_ = "pressure_height";
00086 else
00087 height_topic_ = _sdf->GetElement("topicName")->GetValueString();
00088
00089 if (!_sdf->HasElement("altimeterTopicName"))
00090 altimeter_topic_ = "altimeter";
00091 else
00092 altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->GetValueString();
00093
00094 if (!_sdf->HasElement("elevation"))
00095 elevation_ = DEFAULT_ELEVATION;
00096 else
00097 elevation_ = _sdf->GetElement("elevation")->GetValueDouble();
00098
00099 if (!_sdf->HasElement("qnh"))
00100 qnh_ = DEFAULT_QNH;
00101 else
00102 qnh_ = _sdf->GetElement("qnh")->GetValueDouble();
00103
00104 sensor_model_.Load(_sdf);
00105 height_.header.frame_id = frame_id_;
00106
00107
00108 if (!ros::isInitialized())
00109 {
00110 int argc = 0;
00111 char **argv = NULL;
00112 ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00113 }
00114
00115 node_handle_ = new ros::NodeHandle(namespace_);
00116 if (!height_topic_.empty()) {
00117 #ifdef USE_MAV_MSGS
00118 height_publisher_ = node_handle_->advertise<mav_msgs::Height>(height_topic_, 10);
00119 #else
00120 height_publisher_ = node_handle_->advertise<geometry_msgs::PointStamped>(height_topic_, 10);
00121 #endif
00122 }
00123
00124 if (!altimeter_topic_.empty()) {
00125 altimeter_publisher_ = node_handle_->advertise<hector_uav_msgs::Altimeter>(altimeter_topic_, 10);
00126 }
00127
00128 Reset();
00129
00130
00131 updateTimer.Load(world, _sdf);
00132 updateConnection = updateTimer.Connect(boost::bind(&GazeboRosBaro::Update, this));
00133 }
00134
00135 void GazeboRosBaro::Reset()
00136 {
00137 updateTimer.Reset();
00138 sensor_model_.reset();
00139 }
00140
00142
00143 void GazeboRosBaro::Update()
00144 {
00145 common::Time sim_time = world->GetSimTime();
00146 double dt = updateTimer.getTimeSinceLastUpdate().Double();
00147
00148 math::Pose pose = link->GetWorldPose();
00149 double height = sensor_model_(pose.pos.z, dt);
00150
00151 if (height_publisher_) {
00152 #ifdef USE_MAV_MSGS
00153 double previous_height = height_.height;
00154 height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00155 height_.height = height;
00156 height_.height_variance = sensor_model_.gaussian_noise*sensor_model_.gaussian_noise + sensor_model_.drift*sensor_model_.drift;
00157 height_.climb = dt > 0.0 ? (height_.height - previous_height) / dt : 0.0;
00158 height_.climb_variance = sensor_model_.gaussian_noise*sensor_model_.gaussian_noise;
00159 #else
00160 height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00161 height_.point.z = height;
00162 #endif
00163
00164 height_publisher_.publish(height_);
00165 }
00166
00167 if (altimeter_publisher_) {
00168 altimeter_.header = height_.header;
00169 altimeter_.altitude = height + elevation_;
00170 altimeter_.pressure = pow((1.0 - altimeter_.altitude / 44330.0), 5.263157) * qnh_;
00171 altimeter_.qnh = qnh_;
00172 altimeter_publisher_.publish(altimeter_);
00173 }
00174 }
00175
00176
00177 GZ_REGISTER_MODEL_PLUGIN(GazeboRosBaro)
00178
00179 }