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_simple_controller.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032
00033 #include <cmath>
00034
00035 #include <geometry_msgs/Wrench.h>
00036
00037 namespace gazebo {
00038
00039 GazeboQuadrotorSimpleController::GazeboQuadrotorSimpleController()
00040 {
00041 }
00042
00044
00045 GazeboQuadrotorSimpleController::~GazeboQuadrotorSimpleController()
00046 {
00047 event::Events::DisconnectWorldUpdateBegin(updateConnection);
00048
00049 node_handle_->shutdown();
00050 delete node_handle_;
00051 }
00052
00054
00055 void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00056 {
00057 gzwarn << "The GazeboQuadrotorSimpleController plugin is DEPRECATED in ROS hydro. Please use the twist_controller in package hector_quadrotor_controller instead." << std::endl;
00058
00059 world = _model->GetWorld();
00060 link = _model->GetLink();
00061 link_name_ = link->GetName();
00062
00063
00064 namespace_.clear();
00065 velocity_topic_ = "cmd_vel";
00066 imu_topic_.clear();
00067 state_topic_.clear();
00068 wrench_topic_ = "wrench_out";
00069 max_force_ = -1;
00070 auto_engage_ = true;
00071
00072
00073 if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00074 if (_sdf->HasElement("topicName")) velocity_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
00075 if (_sdf->HasElement("imuTopic")) imu_topic_ = _sdf->GetElement("imuTopic")->Get<std::string>();
00076 if (_sdf->HasElement("stateTopic")) state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();
00077 if (_sdf->HasElement("wrenchTopic")) wrench_topic_ = _sdf->GetElement("wrenchTopic")->Get<std::string>();
00078 if (_sdf->HasElement("maxForce")) max_force_ = _sdf->GetElement("maxForce")->Get<double>();
00079 if (_sdf->HasElement("autoEngage")) auto_engage_ = _sdf->GetElement("autoEngage")->Get<bool>();
00080
00081 if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue()) {
00082 link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
00083 link = _model->GetLink(link_name_);
00084 }
00085
00086 if (!link)
00087 {
00088 ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00089 return;
00090 }
00091
00092
00093 controllers_.roll.Load(_sdf, "rollpitch");
00094 controllers_.pitch.Load(_sdf, "rollpitch");
00095 controllers_.yaw.Load(_sdf, "yaw");
00096 controllers_.velocity_x.Load(_sdf, "velocityXY");
00097 controllers_.velocity_y.Load(_sdf, "velocityXY");
00098 controllers_.velocity_z.Load(_sdf, "velocityZ");
00099
00100
00101 inertia = link->GetInertial()->GetPrincipalMoments();
00102 mass = link->GetInertial()->GetMass();
00103
00104
00105 if (!ros::isInitialized())
00106 {
00107 ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00108 << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00109 return;
00110 }
00111
00112 node_handle_ = new ros::NodeHandle(namespace_);
00113 ros::NodeHandle param_handle(*node_handle_, "controller");
00114
00115
00116 param_handle.getParam("velocity_topic", velocity_topic_);
00117 if (!velocity_topic_.empty())
00118 {
00119 ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
00120 velocity_topic_, 1,
00121 boost::bind(&GazeboQuadrotorSimpleController::VelocityCallback, this, _1),
00122 ros::VoidPtr(), &callback_queue_);
00123 velocity_subscriber_ = node_handle_->subscribe(ops);
00124 }
00125
00126
00127 param_handle.getParam("imu_topic", imu_topic_);
00128 if (!imu_topic_.empty())
00129 {
00130 ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
00131 imu_topic_, 1,
00132 boost::bind(&GazeboQuadrotorSimpleController::ImuCallback, this, _1),
00133 ros::VoidPtr(), &callback_queue_);
00134 imu_subscriber_ = node_handle_->subscribe(ops);
00135
00136 ROS_INFO_NAMED("quadrotor_simple_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
00137 }
00138
00139
00140 param_handle.getParam("state_topic", state_topic_);
00141 if (!state_topic_.empty())
00142 {
00143 ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
00144 state_topic_, 1,
00145 boost::bind(&GazeboQuadrotorSimpleController::StateCallback, this, _1),
00146 ros::VoidPtr(), &callback_queue_);
00147 state_subscriber_ = node_handle_->subscribe(ops);
00148
00149 ROS_INFO_NAMED("quadrotor_simple_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
00150 }
00151
00152
00153 param_handle.getParam("wrench_topic", wrench_topic_);
00154 if (!wrench_topic_.empty())
00155 {
00156 ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::Wrench>(
00157 wrench_topic_, 10,
00158 ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_
00159 );
00160 wrench_publisher_ = node_handle_->advertise(ops);
00161 }
00162
00163
00164 {
00165 ros::AdvertiseServiceOptions ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00166 "engage", boost::bind(&GazeboQuadrotorSimpleController::EngageCallback, this, _1, _2),
00167 ros::VoidConstPtr(), &callback_queue_
00168 );
00169 engage_service_server_ = node_handle_->advertiseService(ops);
00170
00171 ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00172 "shutdown", boost::bind(&GazeboQuadrotorSimpleController::ShutdownCallback, this, _1, _2),
00173 ros::VoidConstPtr(), &callback_queue_
00174 );
00175 shutdown_service_server_ = node_handle_->advertiseService(ops);
00176 }
00177
00178
00179
00180
00181 Reset();
00182
00183
00184
00185
00186 controlTimer.Load(world, _sdf);
00187 updateConnection = event::Events::ConnectWorldUpdateBegin(
00188 boost::bind(&GazeboQuadrotorSimpleController::Update, this));
00189 }
00190
00192
00193 void GazeboQuadrotorSimpleController::VelocityCallback(const geometry_msgs::TwistConstPtr& velocity)
00194 {
00195 velocity_command_ = *velocity;
00196 }
00197
00198 void GazeboQuadrotorSimpleController::ImuCallback(const sensor_msgs::ImuConstPtr& imu)
00199 {
00200 pose.rot.Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
00201 euler = pose.rot.GetAsEuler();
00202 angular_velocity = pose.rot.RotateVector(math::Vector3(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z));
00203 }
00204
00205 void GazeboQuadrotorSimpleController::StateCallback(const nav_msgs::OdometryConstPtr& state)
00206 {
00207 math::Vector3 velocity1(velocity);
00208
00209 if (imu_topic_.empty()) {
00210 pose.pos.Set(state->pose.pose.position.x, state->pose.pose.position.y, state->pose.pose.position.z);
00211 pose.rot.Set(state->pose.pose.orientation.w, state->pose.pose.orientation.x, state->pose.pose.orientation.y, state->pose.pose.orientation.z);
00212 euler = pose.rot.GetAsEuler();
00213 angular_velocity.Set(state->twist.twist.angular.x, state->twist.twist.angular.y, state->twist.twist.angular.z);
00214 }
00215
00216 velocity.Set(state->twist.twist.linear.x, state->twist.twist.linear.y, state->twist.twist.linear.z);
00217
00218
00219 double dt = !state_stamp.isZero() ? (state->header.stamp - state_stamp).toSec() : 0.0;
00220 state_stamp = state->header.stamp;
00221 if (dt > 0.0) {
00222 acceleration = (velocity - velocity1) / dt;
00223 } else {
00224 acceleration.Set();
00225 }
00226 }
00227
00228 bool GazeboQuadrotorSimpleController::EngageCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
00229 {
00230 ROS_INFO_NAMED("quadrotor_simple_controller", "Engaging motors!");
00231 running_ = true;
00232 return true;
00233 }
00234
00235 bool GazeboQuadrotorSimpleController::ShutdownCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
00236 {
00237 ROS_INFO_NAMED("quadrotor_simple_controller", "Shutting down motors!");
00238 running_ = false;
00239 return true;
00240 }
00241
00243
00244 void GazeboQuadrotorSimpleController::Update()
00245 {
00246
00247 callback_queue_.callAvailable();
00248
00249 double dt;
00250 if (controlTimer.update(dt) && dt > 0.0) {
00251
00252 if (imu_topic_.empty()) {
00253 pose = link->GetWorldPose();
00254 angular_velocity = link->GetWorldAngularVel();
00255 euler = pose.rot.GetAsEuler();
00256 }
00257 if (state_topic_.empty()) {
00258 acceleration = (link->GetWorldLinearVel() - velocity) / dt;
00259 velocity = link->GetWorldLinearVel();
00260 }
00261
00262
00263 if (auto_engage_) {
00264 if (!running_ && velocity_command_.linear.z > 0.1) {
00265 running_ = true;
00266 ROS_INFO_NAMED("quadrotor_simple_controller", "Engaging motors!");
00267 } else if (running_ && controllers_.velocity_z.i < -1.0 && velocity_command_.linear.z < -0.1 && (velocity.z > -0.1 && velocity.z < 0.1)) {
00268 running_ = false;
00269 ROS_INFO_NAMED("quadrotor_simple_controller", "Shutting down motors!");
00270 }
00271 }
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282 math::Vector3 gravity_body = pose.rot.RotateVector(world->GetPhysicsEngine()->GetGravity());
00283 double gravity = gravity_body.GetLength();
00284 double load_factor = gravity * gravity / world->GetPhysicsEngine()->GetGravity().Dot(gravity_body);
00285
00286
00287 math::Quaternion heading_quaternion(cos(euler.z/2),0,0,sin(euler.z/2));
00288 math::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity);
00289 math::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration);
00290 math::Vector3 angular_velocity_body = pose.rot.RotateVectorReverse(angular_velocity);
00291
00292
00293 force.Set(0.0, 0.0, 0.0);
00294 torque.Set(0.0, 0.0, 0.0);
00295 if (running_) {
00296 double pitch_command = controllers_.velocity_x.update(velocity_command_.linear.x, velocity_xy.x, acceleration_xy.x, dt) / gravity;
00297 double roll_command = -controllers_.velocity_y.update(velocity_command_.linear.y, velocity_xy.y, acceleration_xy.y, dt) / gravity;
00298 torque.x = inertia.x * controllers_.roll.update(roll_command, euler.x, angular_velocity_body.x, dt);
00299 torque.y = inertia.y * controllers_.pitch.update(pitch_command, euler.y, angular_velocity_body.y, dt);
00300
00301
00302 torque.z = inertia.z * controllers_.yaw.update(velocity_command_.angular.z, angular_velocity.z, 0, dt);
00303 force.z = mass * (controllers_.velocity_z.update(velocity_command_.linear.z, velocity.z, acceleration.z, dt) + load_factor * gravity);
00304 if (max_force_ > 0.0 && force.z > max_force_) force.z = max_force_;
00305 if (force.z < 0.0) force.z = 0.0;
00306
00307 } else {
00308 controllers_.roll.reset();
00309 controllers_.pitch.reset();
00310 controllers_.yaw.reset();
00311 controllers_.velocity_x.reset();
00312 controllers_.velocity_y.reset();
00313 controllers_.velocity_z.reset();
00314 }
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326 if (wrench_publisher_) {
00327 geometry_msgs::Wrench wrench;
00328 wrench.force.x = force.x;
00329 wrench.force.y = force.y;
00330 wrench.force.z = force.z;
00331 wrench.torque.x = torque.x;
00332 wrench.torque.y = torque.y;
00333 wrench.torque.z = torque.z;
00334 wrench_publisher_.publish(wrench);
00335 }
00336 }
00337
00338
00339 link->AddRelativeForce(force);
00340 link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
00341 }
00342
00344
00345 void GazeboQuadrotorSimpleController::Reset()
00346 {
00347 controllers_.roll.reset();
00348 controllers_.pitch.reset();
00349 controllers_.yaw.reset();
00350 controllers_.velocity_x.reset();
00351 controllers_.velocity_y.reset();
00352 controllers_.velocity_z.reset();
00353
00354 force.Set();
00355 torque.Set();
00356
00357
00358 pose.Reset();
00359 velocity.Set();
00360 angular_velocity.Set();
00361 acceleration.Set();
00362 euler.Set();
00363 state_stamp = ros::Time();
00364
00365 running_ = false;
00366 }
00367
00369
00370 GazeboQuadrotorSimpleController::PIDController::PIDController()
00371 {
00372 }
00373
00374 GazeboQuadrotorSimpleController::PIDController::~PIDController()
00375 {
00376 }
00377
00378 void GazeboQuadrotorSimpleController::PIDController::Load(sdf::ElementPtr _sdf, const std::string& prefix)
00379 {
00380 gain_p = 0.0;
00381 gain_d = 0.0;
00382 gain_i = 0.0;
00383 time_constant = 0.0;
00384 limit = -1.0;
00385
00386 if (!_sdf) return;
00387
00388 if (_sdf->HasElement(prefix + "ProportionalGain")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->Get<double>();
00389 if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->Get<double>();
00390 if (_sdf->HasElement(prefix + "IntegralGain")) gain_i = _sdf->GetElement(prefix + "IntegralGain")->Get<double>();
00391 if (_sdf->HasElement(prefix + "TimeConstant")) time_constant = _sdf->GetElement(prefix + "TimeConstant")->Get<double>();
00392 if (_sdf->HasElement(prefix + "Limit")) limit = _sdf->GetElement(prefix + "Limit")->Get<double>();
00393 }
00394
00395 double GazeboQuadrotorSimpleController::PIDController::update(double new_input, double x, double dx, double dt)
00396 {
00397
00398 if (limit > 0.0 && fabs(new_input) > limit) new_input = (new_input < 0 ? -1.0 : 1.0) * limit;
00399
00400
00401 if (dt + time_constant > 0.0) {
00402 dinput = (new_input - input) / (dt + time_constant);
00403 input = (dt * new_input + time_constant * input) / (dt + time_constant);
00404 }
00405
00406
00407 p = input - x;
00408 d = dinput - dx;
00409 i = i + dt * p;
00410
00411
00412 output = gain_p * p + gain_d * d + gain_i * i;
00413 return output;
00414 }
00415
00416 void GazeboQuadrotorSimpleController::PIDController::reset()
00417 {
00418 input = dinput = 0;
00419 p = i = d = output = 0;
00420 }
00421
00422
00423 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorSimpleController)
00424
00425 }