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