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_quadrotor_propulsion.h>
00030 #include <hector_quadrotor_model/helpers.h>
00031
00032 #include <gazebo/common/Events.hh>
00033 #include <gazebo/physics/physics.hh>
00034
00035 #include <rosgraph_msgs/Clock.h>
00036 #include <geometry_msgs/WrenchStamped.h>
00037
00038 namespace gazebo {
00039
00040 using namespace common;
00041 using namespace math;
00042 using namespace hector_quadrotor_model;
00043
00044 GazeboQuadrotorPropulsion::GazeboQuadrotorPropulsion()
00045 : node_handle_(0)
00046 {
00047 }
00048
00049 GazeboQuadrotorPropulsion::~GazeboQuadrotorPropulsion()
00050 {
00051 event::Events::DisconnectWorldUpdateBegin(updateConnection);
00052 if (node_handle_) {
00053 node_handle_->shutdown();
00054 if (callback_queue_thread_.joinable())
00055 callback_queue_thread_.join();
00056 delete node_handle_;
00057 }
00058 }
00059
00061
00062 void GazeboQuadrotorPropulsion::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00063 {
00064 world = _model->GetWorld();
00065 link = _model->GetLink();
00066
00067
00068 namespace_.clear();
00069 param_namespace_ = "quadrotor_propulsion";
00070 trigger_topic_ = "quadro/trigger";
00071 command_topic_ = "command/motor";
00072 pwm_topic_ = "motor_pwm";
00073 wrench_topic_ = "propulsion/wrench";
00074 supply_topic_ = "supply";
00075 status_topic_ = "motor_status";
00076 control_tolerance_ = ros::Duration();
00077 control_delay_ = ros::Duration();
00078 frame_id_ = link->GetName();
00079
00080
00081 if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00082 if (_sdf->HasElement("paramNamespace")) param_namespace_ = _sdf->GetElement("paramNamespace")->Get<std::string>();
00083 if (_sdf->HasElement("triggerTopic")) trigger_topic_ = _sdf->GetElement("triggerTopic")->Get<std::string>();
00084 if (_sdf->HasElement("topicName")) command_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
00085 if (_sdf->HasElement("pwmTopicName")) pwm_topic_ = _sdf->GetElement("pwmTopicName")->Get<std::string>();
00086 if (_sdf->HasElement("wrenchTopic")) wrench_topic_ = _sdf->GetElement("wrenchTopic")->Get<std::string>();
00087 if (_sdf->HasElement("supplyTopic")) supply_topic_ = _sdf->GetElement("supplyTopic")->Get<std::string>();
00088 if (_sdf->HasElement("statusTopic")) status_topic_ = _sdf->GetElement("statusTopic")->Get<std::string>();
00089 if (_sdf->HasElement("frameId")) frame_id_= _sdf->GetElement("frameId")->Get<std::string>();
00090
00091 if (_sdf->HasElement("voltageTopicName")) {
00092 gzwarn << "[quadrotor_propulsion] Plugin parameter 'voltageTopicName' is deprecated! Plese change your config to use "
00093 << "'topicName' for MotorCommand messages or 'pwmTopicName' for MotorPWM messages." << std::endl;
00094 pwm_topic_ = _sdf->GetElement("voltageTopicName")->Get<std::string>();
00095 }
00096
00097
00098 controlTimer.Load(world, _sdf, "control");
00099 motorStatusTimer.Load(world, _sdf, "motorStatus");
00100 if (_sdf->HasElement("controlTolerance")) control_tolerance_.fromSec(_sdf->GetElement("controlTolerance")->Get<double>());
00101 if (_sdf->HasElement("controlDelay")) control_delay_.fromSec(_sdf->GetElement("controlDelay")->Get<double>());
00102
00103
00104 if (_sdf->HasElement("supplyVoltage")) model_.setInitialSupplyVoltage(_sdf->GetElement("supplyVoltage")->Get<double>());
00105
00106
00107 if (!ros::isInitialized())
00108 {
00109 ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00110 << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00111 return;
00112 }
00113
00114 node_handle_ = new ros::NodeHandle(namespace_);
00115
00116
00117 if (!model_.configure(ros::NodeHandle(*node_handle_, param_namespace_))) {
00118 gzwarn << "[quadrotor_propulsion] Could not properly configure the propulsion plugin. Make sure you loaded the parameter file." << std::endl;
00119 return;
00120 }
00121
00122
00123 if (!trigger_topic_.empty())
00124 {
00125 ros::AdvertiseOptions ops;
00126 ops.callback_queue = &callback_queue_;
00127 ops.init<rosgraph_msgs::Clock>(trigger_topic_, 10);
00128 trigger_publisher_ = node_handle_->advertise(ops);
00129 }
00130
00131
00132 if (!command_topic_.empty())
00133 {
00134 ros::SubscribeOptions ops;
00135 ops.callback_queue = &callback_queue_;
00136 ops.init<hector_uav_msgs::MotorCommand>(command_topic_, 1, boost::bind(&QuadrotorPropulsion::addCommandToQueue, &model_, _1));
00137 command_subscriber_ = node_handle_->subscribe(ops);
00138 }
00139
00140
00141 if (!pwm_topic_.empty())
00142 {
00143 ros::SubscribeOptions ops;
00144 ops.callback_queue = &callback_queue_;
00145 ops.init<hector_uav_msgs::MotorPWM>(pwm_topic_, 1, boost::bind(&QuadrotorPropulsion::addPWMToQueue, &model_, _1));
00146 pwm_subscriber_ = node_handle_->subscribe(ops);
00147 }
00148
00149
00150 if (!wrench_topic_.empty())
00151 {
00152 ros::AdvertiseOptions ops;
00153 ops.callback_queue = &callback_queue_;
00154 ops.init<geometry_msgs::WrenchStamped>(wrench_topic_, 10);
00155 wrench_publisher_ = node_handle_->advertise(ops);
00156 }
00157
00158
00159 if (!supply_topic_.empty())
00160 {
00161 ros::AdvertiseOptions ops;
00162 ops.callback_queue = &callback_queue_;
00163 ops.latch = true;
00164 ops.init<hector_uav_msgs::Supply>(supply_topic_, 10);
00165 supply_publisher_ = node_handle_->advertise(ops);
00166 supply_publisher_.publish(model_.getSupply());
00167 }
00168
00169
00170 if (!status_topic_.empty())
00171 {
00172 ros::AdvertiseOptions ops;
00173 ops.callback_queue = &callback_queue_;
00174 ops.init<hector_uav_msgs::MotorStatus>(status_topic_, 10);
00175 motor_status_publisher_ = node_handle_->advertise(ops);
00176 }
00177
00178
00179
00180 Reset();
00181
00182
00183
00184
00185 updateConnection = event::Events::ConnectWorldUpdateBegin(
00186 boost::bind(&GazeboQuadrotorPropulsion::Update, this));
00187 }
00188
00190
00191 void GazeboQuadrotorPropulsion::Update()
00192 {
00193
00194 Time current_time = world->GetSimTime();
00195 Time dt = current_time - last_time_;
00196 last_time_ = current_time;
00197 if (dt <= 0.0) return;
00198
00199
00200 bool trigger = controlTimer.getUpdatePeriod() > 0.0 ? controlTimer.update() : false;
00201 if (trigger && trigger_publisher_) {
00202 rosgraph_msgs::Clock clock;
00203 clock.clock = ros::Time(current_time.sec, current_time.nsec);
00204 trigger_publisher_.publish(clock);
00205
00206 ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Sent a trigger message at t = " << current_time.Double() << " (dt = " << (current_time - last_trigger_time_).Double() << ")");
00207 last_trigger_time_ = current_time;
00208 }
00209
00210
00211 callback_queue_.callAvailable();
00212
00213
00214 model_.processQueue(ros::Time(current_time.sec, current_time.nsec), control_tolerance_, control_delay_, (model_.getMotorStatus().on && trigger) ? ros::WallDuration(1.0) : ros::WallDuration(), &callback_queue_);
00215
00216
00217 geometry_msgs::Twist twist;
00218 fromVector(link->GetRelativeLinearVel(), twist.linear);
00219 fromVector(link->GetRelativeAngularVel(), twist.angular);
00220 model_.setTwist(twist);
00221
00222
00223 model_.update(dt.Double());
00224
00225
00226 Vector3 force, torque;
00227 toVector(model_.getWrench().force, force);
00228 toVector(model_.getWrench().torque, torque);
00229
00230
00231 if (wrench_publisher_) {
00232 geometry_msgs::WrenchStamped wrench_msg;
00233 wrench_msg.header.stamp = ros::Time(current_time.sec, current_time.nsec);
00234 wrench_msg.header.frame_id = frame_id_;
00235 wrench_msg.wrench = model_.getWrench();
00236 wrench_publisher_.publish(wrench_msg);
00237 }
00238
00239
00240 if (motor_status_publisher_ && motorStatusTimer.update() ) {
00241 hector_uav_msgs::MotorStatus motor_status = model_.getMotorStatus();
00242 motor_status.header.stamp = ros::Time(current_time.sec, current_time.nsec);
00243 motor_status_publisher_.publish(motor_status);
00244 last_motor_status_time_ = current_time;
00245 }
00246
00247
00248 if (supply_publisher_ && current_time >= last_supply_time_ + 1.0) {
00249 supply_publisher_.publish(model_.getSupply());
00250 last_supply_time_ = current_time;
00251 }
00252
00253
00254 link->AddRelativeForce(force);
00255 link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
00256 }
00257
00259
00260 void GazeboQuadrotorPropulsion::Reset()
00261 {
00262 model_.reset();
00263 last_time_ = Time();
00264 last_trigger_time_ = Time();
00265 last_motor_status_time_ = Time();
00266 last_supply_time_ = Time();
00267 }
00268
00270
00271 void GazeboQuadrotorPropulsion::QueueThread()
00272 {
00273 static const double timeout = 0.01;
00274
00275 while (node_handle_->ok())
00276 {
00277 callback_queue_.callAvailable(ros::WallDuration(timeout));
00278 }
00279 }
00280
00281
00282 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorPropulsion)
00283
00284 }