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
00037 namespace gazebo {
00038
00039 using namespace common;
00040 using namespace math;
00041 using namespace hector_quadrotor_model;
00042
00043 GazeboQuadrotorPropulsion::GazeboQuadrotorPropulsion()
00044 {
00045 }
00046
00047 GazeboQuadrotorPropulsion::~GazeboQuadrotorPropulsion()
00048 {
00049 event::Events::DisconnectWorldUpdateBegin(updateConnection);
00050 node_handle_->shutdown();
00051 callback_queue_thread_.join();
00052 delete node_handle_;
00053 }
00054
00056
00057 void GazeboQuadrotorPropulsion::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00058 {
00059 world = _model->GetWorld();
00060 link = _model->GetLink();
00061
00062
00063 namespace_.clear();
00064 param_namespace_ = "quadrotor_propulsion";
00065 trigger_topic_ = "quadro/trigger";
00066 voltage_topic_ = "motor_pwm";
00067 wrench_topic_ = "wrench_out";
00068 supply_topic_ = "supply";
00069 status_topic_ = "motor_status";
00070 control_tolerance_ = ros::Duration();
00071 control_delay_ = ros::Duration();
00072
00073
00074 if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->GetValueString();
00075 if (_sdf->HasElement("paramNamespace")) param_namespace_ = _sdf->GetElement("paramNamespace")->GetValueString();
00076 if (_sdf->HasElement("triggerTopic")) trigger_topic_ = _sdf->GetElement("triggerTopic")->GetValueString();
00077 if (_sdf->HasElement("voltageTopicName")) voltage_topic_ = _sdf->GetElement("voltageTopicName")->GetValueString();
00078 if (_sdf->HasElement("wrenchTopic")) wrench_topic_ = _sdf->GetElement("wrenchTopic")->GetValueString();
00079 if (_sdf->HasElement("supplyTopic")) supply_topic_ = _sdf->GetElement("supplyTopic")->GetValueString();
00080 if (_sdf->HasElement("statusTopic")) status_topic_ = _sdf->GetElement("statusTopic")->GetValueString();
00081
00082
00083 model_.configure(param_namespace_);
00084
00085
00086 controlTimer.Load(world, _sdf, "control");
00087 if (_sdf->HasElement("controlTolerance")) control_tolerance_.fromSec(_sdf->GetElement("controlTolerance")->GetValueDouble());
00088 if (_sdf->HasElement("controlDelay")) control_delay_.fromSec(_sdf->GetElement("controlDelay")->GetValueDouble());
00089
00090
00091 if (_sdf->HasElement("supplyVoltage")) model_.setInitialSupplyVoltage(_sdf->GetElement("supplyVoltage")->GetValueDouble());
00092
00093
00094 if (!ros::isInitialized())
00095 {
00096 int argc = 0;
00097 char **argv = NULL;
00098 ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00099 }
00100 node_handle_ = new ros::NodeHandle(namespace_);
00101
00102
00103 if (!trigger_topic_.empty())
00104 {
00105 ros::AdvertiseOptions ops;
00106 ops.callback_queue = &callback_queue_;
00107 ops.init<rosgraph_msgs::Clock>(trigger_topic_, 10);
00108 trigger_publisher_ = node_handle_->advertise(ops);
00109 }
00110
00111
00112 if (!voltage_topic_.empty())
00113 {
00114 ros::SubscribeOptions ops;
00115 ops.callback_queue = &callback_queue_;
00116 ops.init<hector_uav_msgs::MotorPWM>(voltage_topic_, 1, boost::bind(&QuadrotorPropulsion::addVoltageToQueue, &model_, _1));
00117 voltage_subscriber_ = node_handle_->subscribe(ops);
00118 }
00119
00120
00121 if (!wrench_topic_.empty())
00122 {
00123 ros::AdvertiseOptions ops;
00124 ops.callback_queue = &callback_queue_;
00125 ops.init<geometry_msgs::Wrench>(wrench_topic_, 10);
00126 wrench_publisher_ = node_handle_->advertise(ops);
00127 }
00128
00129
00130 if (!supply_topic_.empty())
00131 {
00132 ros::AdvertiseOptions ops;
00133 ops.callback_queue = &callback_queue_;
00134 ops.latch = true;
00135 ops.init<hector_uav_msgs::Supply>(supply_topic_, 10);
00136 supply_publisher_ = node_handle_->advertise(ops);
00137 supply_publisher_.publish(model_.getSupply());
00138 }
00139
00140
00141 if (!status_topic_.empty())
00142 {
00143 ros::AdvertiseOptions ops;
00144 ops.callback_queue = &callback_queue_;
00145 ops.init<hector_uav_msgs::MotorStatus>(status_topic_, 10);
00146 motor_status_publisher_ = node_handle_->advertise(ops);
00147 }
00148
00149
00150
00151 Reset();
00152
00153
00154
00155
00156 updateConnection = event::Events::ConnectWorldUpdateBegin(
00157 boost::bind(&GazeboQuadrotorPropulsion::Update, this));
00158 }
00159
00161
00162 void GazeboQuadrotorPropulsion::Update()
00163 {
00164
00165 Time current_time = world->GetSimTime();
00166 Time dt = current_time - last_time_;
00167 last_time_ = current_time;
00168 if (dt <= 0.0) return;
00169
00170
00171 bool trigger = controlTimer.update();
00172 if (trigger && trigger_publisher_) {
00173 rosgraph_msgs::Clock clock;
00174 clock.clock = ros::Time(current_time.sec, current_time.nsec);
00175 trigger_publisher_.publish(clock);
00176
00177 ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Sent a trigger message at t = " << current_time.Double() << " (dt = " << (current_time - last_trigger_time_).Double() << ")");
00178 last_trigger_time_ = current_time;
00179 }
00180
00181
00182 callback_queue_.callAvailable();
00183
00184
00185 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_);
00186
00187
00188 geometry_msgs::Twist twist;
00189 fromVector(link->GetRelativeLinearVel(), twist.linear);
00190 fromVector(link->GetRelativeAngularVel(), twist.angular);
00191 model_.setTwist(twist);
00192
00193
00194 model_.update(dt.Double());
00195
00196
00197 Vector3 force, torque;
00198 toVector(model_.getWrench().force, force);
00199 toVector(model_.getWrench().torque, torque);
00200
00201
00202 if (wrench_publisher_) {
00203 wrench_publisher_.publish(model_.getWrench());
00204 }
00205
00206
00207 if (motor_status_publisher_ && trigger ) {
00208 hector_uav_msgs::MotorStatus motor_status = model_.getMotorStatus();
00209 motor_status.header.stamp = ros::Time(current_time.sec, current_time.nsec);
00210 motor_status_publisher_.publish(motor_status);
00211 last_motor_status_time_ = current_time;
00212 }
00213
00214
00215 if (supply_publisher_ && current_time >= last_supply_time_ + 1.0) {
00216 supply_publisher_.publish(model_.getSupply());
00217 last_supply_time_ = current_time;
00218 }
00219
00220
00221 link->AddRelativeForce(force);
00222 link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
00223 }
00224
00226
00227 void GazeboQuadrotorPropulsion::Reset()
00228 {
00229 model_.reset();
00230 last_time_ = Time();
00231 last_trigger_time_ = Time();
00232 last_motor_status_time_ = Time();
00233 last_supply_time_ = Time();
00234 }
00235
00237
00238 void GazeboQuadrotorPropulsion::QueueThread()
00239 {
00240 static const double timeout = 0.01;
00241
00242 while (node_handle_->ok())
00243 {
00244 callback_queue_.callAvailable(ros::WallDuration(timeout));
00245 }
00246 }
00247
00248
00249 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorPropulsion)
00250
00251 }