gazebo_rsv_balance.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *  Copyright (c) 2015 Robosavvy Ltd.
00003  *  Author: Vitor Matos
00004  *
00005  *  Based on diff_drive_plugin
00006  *********************************************************************/
00007 
00008 #include "gazebo_rsv_balance/gazebo_rsv_balance.h"
00009 
00010 #include <string>
00011 #include <map>
00012 
00013 #include <ros/ros.h>
00014 #include <sdf/sdf.hh>
00015 
00016 namespace gazebo
00017 {
00018 
00019 GazeboRsvBalance::GazeboRsvBalance() {}
00020 
00021 GazeboRsvBalance::~GazeboRsvBalance() {}
00022 
00023 void GazeboRsvBalance::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00024 {
00025   this->parent_ = _parent;
00026   this->sdf_ = _sdf;
00027   this->gazebo_ros_ = GazeboRosPtr(new GazeboRos(_parent, _sdf, "RsvBalancePlugin"));
00028 
00029   this->gazebo_ros_->isInitialized();
00030 
00031   // Obtain parameters from rosparam
00032   this->gazebo_ros_->getParameter<bool>(this->publish_odom_tf_, "publishOdomTF", true);
00033   this->gazebo_ros_->getParameter<bool>(this->publish_wheel_joint_, "publishWheelJointState", true);
00034 
00035   this->gazebo_ros_->getParameter<std::string>(this->command_topic_, "commandTopic", "cmd_vel");
00036   this->gazebo_ros_->getParameter<std::string>(this->odom_topic_, "odomTopic", "odom");
00037   this->gazebo_ros_->getParameter<std::string>(this->base_frame_id_, "baseFrameId", "base_link");
00038   this->gazebo_ros_->getParameter<std::string>(this->odom_frame_id_, "odomFrameId", "odom");
00039 
00040   this->gazebo_ros_->getParameter<bool>(this->publish_state_, "publishState", true);
00041   this->gazebo_ros_->getParameter<double>(this->update_rate_, "updateRate", 50.0);
00042   this->gazebo_ros_->getParameter<double>(this->publish_state_rate_, "publishStateRate", 50);
00043   this->gazebo_ros_->getParameter<double>(this->publish_diagnostics_rate_, "publishDiagnosticsRate", 1);
00044 
00045   std::map<std::string, OdomSource> odom_options;
00046   odom_options["encoder"] = ENCODER;
00047   odom_options["world"] = WORLD;
00048   this->gazebo_ros_->getParameter<OdomSource>(this->odom_source_, "odomSource", odom_options, WORLD);
00049 
00050   this->mode_map_["park"] = PARK;
00051   this->mode_map_["tractor"] = TRACTOR;
00052   this->mode_map_["balance"] = BALANCE;
00053   this->gazebo_ros_->getParameter<Mode>(this->current_mode_, "startMode", this->mode_map_, BALANCE);
00054 
00055   if (!this->sdf_->HasElement("wheelSeparation") || !this->sdf_->HasElement("wheelRadius") )
00056   {
00057     ROS_ERROR("RsvBalancePlugin - Missing <wheelSeparation> or <wheelDiameter>, Aborting");
00058     return;
00059   }
00060   else
00061   {
00062     this->gazebo_ros_->getParameter<double>(this->wheel_separation_, "wheelSeparation");
00063     this->gazebo_ros_->getParameter<double>(this->wheel_radius_, "wheelRadius");
00064   }
00065 
00066   this->joints_.resize(2);
00067   this->joints_[LEFT] = this->gazebo_ros_->getJoint(this->parent_, "leftJoint", "left_joint");
00068   this->joints_[RIGHT] = this->gazebo_ros_->getJoint(this->parent_, "rightJoint", "right_joint");
00069 
00070   // Control loop timing
00071   if (this->update_rate_ > 0.0)
00072   {
00073     this->update_period_ = 1.0 / this->update_rate_;
00074   }
00075   else
00076   {
00077     ROS_WARN("RsvBalancePlugin - Update rate < 0. Update period set to: 0.1. ");
00078     this->update_period_ = 0.1;
00079   }
00080   this->last_update_time_ = this->parent_->GetWorld()->GetSimTime();
00081 
00082 
00083   // Command velocity subscriber
00084   ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Twist>(this->command_topic_, 5,
00085               boost::bind(&GazeboRsvBalance::cmdVelCallback, this, _1),
00086               ros::VoidPtr(), &this->queue_);
00087   this->cmd_vel_subscriber_ = this->gazebo_ros_->node()->subscribe(so);
00088   ROS_INFO("%s: Subscribed to %s!", this->gazebo_ros_->info(), this->command_topic_.c_str());
00089   // Tilt equilibrium subscriber
00090   so = ros::SubscribeOptions::create<std_msgs::Float64>("tilt_equilibrium", 5,
00091               boost::bind(&GazeboRsvBalance::cmdTiltCallback, this, _1),
00092               ros::VoidPtr(), &this->queue_);
00093   this->cmd_tilt_subscriber_ = this->gazebo_ros_->node()->subscribe(so);
00094   ROS_INFO("%s: Subscribed to %s!", this->gazebo_ros_->info(), "tilt_equilibrium");
00095 
00096   // Odometry publisher
00097   this->odometry_publisher_ = this->gazebo_ros_->node()->advertise<nav_msgs::Odometry>(this->odom_topic_, 10);
00098   ROS_INFO("%s: Advertise odom on %s !", this->gazebo_ros_->info(), this->odom_topic_.c_str());
00099   // State publisher
00100   this->state_publisher_ = this->gazebo_ros_->node()->advertise<rsv_balance_msgs::State>("state", 10);
00101   ROS_INFO("%s: Advertise system state on %s !", this->gazebo_ros_->info(), "state");
00102   // Joint state publisher
00103   this->joint_state_publisher_ = this->gazebo_ros_->node()->advertise<sensor_msgs::JointState>("joint_states", 10);
00104   ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info());
00105 
00106   // Service for changing operating mode
00107   ros::AdvertiseServiceOptions ao = ros::AdvertiseServiceOptions::create<rsv_balance_msgs::SetMode>("set_mode",
00108         boost::bind(&GazeboRsvBalance::setMode, this, _1),
00109         ros::VoidPtr(),
00110         &this->queue_);
00111   this->set_mode_server_ = this->gazebo_ros_->node()->advertiseService(ao);
00112   // Service for chaning input source
00113   ao = ros::AdvertiseServiceOptions::create<rsv_balance_msgs::SetInput>("set_input",
00114         boost::bind(&GazeboRsvBalance::setInput, this, _1),
00115         ros::VoidPtr(),
00116         &this->queue_);
00117   this->set_input_server_ = this->gazebo_ros_->node()->advertiseService(ao);
00118   // Reset override service
00119   ao = ros::AdvertiseServiceOptions::create<std_srvs::Empty>("reset_override",
00120         boost::bind(&GazeboRsvBalance::resetOverride, this, _1),
00121         ros::VoidPtr(),
00122         &this->queue_);
00123   this->reset_override_server_ = this->gazebo_ros_->node()->advertiseService(ao);
00124   // Reset odom service
00125   ao = ros::AdvertiseServiceOptions::create<std_srvs::Empty>("reset_odom",
00126         boost::bind(&GazeboRsvBalance::resetOdom, this, _1),
00127         ros::VoidPtr(),
00128         &this->queue_);
00129   this->reset_odom_server_ = this->gazebo_ros_->node()->advertiseService(ao);
00130 
00131   // Broadcaster for TF
00132   this->transform_broadcaster_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
00133 
00134   this->resetVariables();
00135   this->state_control_.resetControl();
00136   this->x_desired_ = 0;
00137   this->rot_desired_ = 0;
00138   this->tilt_desired_ = 0;
00139   this->u_control_ = this->state_control_.getControl();
00140 
00141   this->alive_ = true;
00142   // start custom queue
00143   this->callback_queue_thread_ = boost::thread(boost::bind(&GazeboRsvBalance::QueueThread, this));
00144   // listen to the update event (broadcast every simulation iteration)
00145   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRsvBalance::UpdateChild, this));
00146 }
00147 
00153 void GazeboRsvBalance::resetVariables()
00154 {
00155   this->x_desired_ = 0;
00156   this->rot_desired_ = 0;
00157   this->odom_offset_pos_ = math::Vector3(0, 0, 0);
00158   this->odom_offset_rot_ = math::Vector3(0, 0, 0);
00159 }
00160 
00164 bool GazeboRsvBalance::setMode(rsv_balance_msgs::SetMode::Request  &req)
00165 {
00166   // Ugly implementation means bad concept
00167   switch (req.mode)
00168   {
00169     case rsv_balance_msgs::SetModeRequest::PARK:
00170       ROS_INFO("%s: Mode: park", this->gazebo_ros_->info());
00171       this->current_mode_ = PARK;
00172       break;
00173     case rsv_balance_msgs::SetModeRequest::TRACTOR:
00174       ROS_INFO("%s: Mode: tractor", this->gazebo_ros_->info());
00175       this->current_mode_ = TRACTOR;
00176       break;
00177     case rsv_balance_msgs::SetModeRequest::BALANCE:
00178       ROS_INFO("%s: Mode: balance", this->gazebo_ros_->info());
00179       this->current_mode_ = BALANCE;
00180       break;
00181     default:
00182       return false;
00183   };
00184   return true;
00185 }
00186 
00190 bool GazeboRsvBalance::setInput(rsv_balance_msgs::SetInput::Request  &req)
00191 {
00192   // In simulation input should always be serial communication.
00193   ROS_INFO("%s: Input: %d", this->gazebo_ros_->info(), req.input);
00194   return true;
00195 }
00196 
00200 bool GazeboRsvBalance::resetOverride(std_srvs::Empty::Request  &req)
00201 {
00202   // In simulation we don't have RC override, nothing to reset
00203   ROS_INFO("%s: Reset Override", this->gazebo_ros_->info());
00204   return true;
00205 }
00206 
00210 bool GazeboRsvBalance::resetOdom(std_srvs::Empty::Request  &req)
00211 {
00212   ROS_INFO("%s: Reset Odom", this->gazebo_ros_->info());
00213   this->resetOdometry();
00214   return true;
00215 }
00216 
00220 void GazeboRsvBalance::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
00221 {
00222   this->x_desired_ = cmd_msg->linear.x;
00223   this->rot_desired_ = cmd_msg->angular.z;
00224 }
00225 
00229 void GazeboRsvBalance::cmdTiltCallback(const std_msgs::Float64::ConstPtr& cmd_tilt)
00230 {
00231   this->tilt_desired_ = cmd_tilt->data;
00232 }
00233 
00237 void GazeboRsvBalance::updateIMU()
00238 {
00239   // Store pitch and dpitch
00240   math::Pose pose = this->parent_->GetWorldPose();
00241   math::Vector3 veul = this->parent_->GetRelativeAngularVel();
00242   this->imu_pitch_ = pose.rot.GetPitch();
00243   this->imu_dpitch_ = veul.y;
00244 }
00245 
00250 void GazeboRsvBalance::resetOdometry()
00251 {
00252   math::Pose pose = this->parent_->GetWorldPose();
00253   this->odom_offset_pos_ = pose.pos;
00254   this->odom_offset_rot_.z = pose.rot.GetYaw();
00255 }
00256 
00261 void GazeboRsvBalance::updateOdometry()
00262 {
00263   double ang_velocity_left = -this->joints_[LEFT]->GetVelocity(0);
00264   double ang_velocity_right = this->joints_[RIGHT]->GetVelocity(0);
00265 
00266   this->feedback_v_ = this->wheel_radius_/2.0 * (ang_velocity_right + ang_velocity_left);
00267   this->feedback_w_ = this->wheel_radius_/this->wheel_separation_ * (ang_velocity_right - ang_velocity_left);
00268 
00269   if (odom_source_ == WORLD)
00270   {
00271     math::Pose pose = this->parent_->GetWorldPose();
00272     math::Vector3 velocity_linear = this->parent_->GetRelativeLinearVel();
00273     math::Vector3 velocity_angular = this->parent_->GetRelativeAngularVel();
00274     // TODO(vmatos): reset odometry by setting an offset in world
00275     // this->odom_.pose.pose.position.x = pose.pos[0] - this->odom_offset_pos_[0];
00276     // this->odom_.pose.pose.position.y = pose.pos[1] - this->odom_offset_pos_[1];
00277     // this->odom_.pose.pose.position.z = pose.pos[2] - this->odom_offset_pos_[2];
00278     this->odom_.pose.pose.position.x = pose.pos[0];
00279     this->odom_.pose.pose.position.y = pose.pos[1];
00280     this->odom_.pose.pose.position.z = pose.pos[2];
00281     // tf::Quaternion qt;
00282     // qt.setRPY(pose.rot.GetRoll(), pose.rot.GetPitch(), pose.rot.GetYaw() - this->odom_offset_rot_[2]);
00283     // qt.setRPY(pose.rot.GetRoll(), pose.rot.GetPitch(), pose.rot.GetYaw());
00284     this->odom_.pose.pose.orientation.x = pose.rot.x;
00285     this->odom_.pose.pose.orientation.y = pose.rot.y;
00286     this->odom_.pose.pose.orientation.z = pose.rot.z;
00287     this->odom_.pose.pose.orientation.w = pose.rot.w;
00288     this->odom_.twist.twist.linear.x = velocity_linear[0];
00289     this->odom_.twist.twist.linear.y = velocity_linear[1];
00290     this->odom_.twist.twist.angular.z = velocity_angular.z;
00291   }
00292   else
00293   {
00294     ROS_WARN("%s - Odometry from other sources not yet supported.", this->gazebo_ros_->info());
00295   }
00296 }
00297 
00302 void GazeboRsvBalance::publishOdometry()
00303 {
00304   ros::Time current_time = ros::Time::now();
00305   std::string odom_frame = this->gazebo_ros_->resolveTF(this->odom_frame_id_);
00306   std::string base_frame = this->gazebo_ros_->resolveTF(this->base_frame_id_);
00307 
00308   // set odometry covariance
00309   this->odom_.pose.covariance[0] = 0.00001;
00310   this->odom_.pose.covariance[7] = 0.00001;
00311   this->odom_.pose.covariance[14] = 1000000000000.0;
00312   this->odom_.pose.covariance[21] = 1000000000000.0;
00313   this->odom_.pose.covariance[28] = 1000000000000.0;
00314   this->odom_.pose.covariance[35] = 0.001;
00315   // set header
00316   this->odom_.header.stamp = current_time;
00317   this->odom_.header.frame_id = odom_frame;
00318   this->odom_.child_frame_id = base_frame;
00319   //  Publish odometry
00320   this->odometry_publisher_.publish(this->odom_);
00321 
00322   if (this->publish_odom_tf_)
00323   {
00324     tf::Vector3 vt;
00325     tf::Quaternion qt;
00326     vt = tf::Vector3(this->odom_.pose.pose.position.x,
00327                      this->odom_.pose.pose.position.y,
00328                      this->odom_.pose.pose.position.z);
00329     qt = tf::Quaternion(this->odom_.pose.pose.orientation.x, this->odom_.pose.pose.orientation.y,
00330                         this->odom_.pose.pose.orientation.z, this->odom_.pose.pose.orientation.w);
00331     tf::Transform base_to_odom(qt, vt);
00332     this->transform_broadcaster_->sendTransform(
00333             tf::StampedTransform(base_to_odom, current_time, odom_frame, base_frame));
00334   }
00335 }
00336 
00340 void GazeboRsvBalance::publishWheelJointState()
00341 {
00342   ros::Time current_time = ros::Time::now();
00343 
00344   sensor_msgs::JointState joint_state;
00345   joint_state.header.stamp = current_time;
00346   joint_state.name.resize(joints_.size());
00347   joint_state.position.resize(joints_.size());
00348 
00349   for (int i = 0; i < 2; i++)
00350   {
00351     physics::JointPtr joint = this->joints_[i];
00352     math::Angle angle = joint->GetAngle(0);
00353     joint_state.name[i] = joint->GetName();
00354     joint_state.position[i] = angle.Radian();
00355   }
00356   this->joint_state_publisher_.publish(joint_state);
00357 }
00358 
00362 void GazeboRsvBalance::Reset()
00363 {
00364   this->resetVariables();
00365   this->state_control_.resetControl();
00366   // Reset control
00367   this->last_update_time_ = this->parent_->GetWorld()->GetSimTime();
00368   this->current_mode_ = BALANCE;
00369   this->imu_pitch_ = 0;
00370   this->imu_dpitch_ = 0;
00371   this->feedback_v_ = 0;
00372   this->feedback_w_ = 0;
00373 }
00374 
00378 void GazeboRsvBalance::UpdateChild()
00379 {
00380   common::Time current_time = this->parent_->GetWorld()->GetSimTime();
00381   double seconds_since_last_update = (current_time - this->last_update_time_).Double();
00382 
00383   // Only execute control loop on specified rate
00384   if (seconds_since_last_update >= this->update_period_)
00385   {
00386     if (fabs(this->imu_pitch_) > .9 && this->current_mode_ == BALANCE) {
00387       this->current_mode_ = TRACTOR;
00388     }
00389 
00390     this->updateIMU();
00391     this->updateOdometry();
00392     this->publishOdometry();
00393     this->publishWheelJointState();
00394 
00395     double x_desired[4];
00396     x_desired[balance_control::theta] = tilt_desired_;
00397     x_desired[balance_control::dx] = x_desired_;
00398     x_desired[balance_control::dphi] = rot_desired_;
00399     x_desired[balance_control::dtheta] = 0;
00400 
00401     double y_fbk[4];
00402     y_fbk[balance_control::theta] = this->imu_pitch_;
00403     y_fbk[balance_control::dx] = this->feedback_v_;
00404     y_fbk[balance_control::dphi] = this->feedback_w_;
00405     y_fbk[balance_control::dtheta] = this->imu_dpitch_;
00406 
00407     this->state_control_.stepControl(seconds_since_last_update, x_desired, y_fbk);
00408 
00409     this->last_update_time_ += common::Time(this->update_period_);
00410   }
00411 
00412   switch (this->current_mode_)
00413   {
00414     case BALANCE:
00415       this->joints_[LEFT]->SetForce(0, -this->u_control_[balance_control::tauL]);
00416       this->joints_[RIGHT]->SetForce(0, this->u_control_[balance_control::tauR]);
00417       break;
00418     case TRACTOR:
00419       this->joints_[LEFT]->SetVelocity(0, -( (2.0*this->x_desired_ - this->rot_desired_*this->wheel_separation_)
00420                                                                                         / (this->wheel_radius_*2.0)));
00421       this->joints_[RIGHT]->SetVelocity(0, ( (2.0*this->x_desired_ + this->rot_desired_*this->wheel_separation_)
00422                                                                                         / (this->wheel_radius_*2.0)));
00423       break;
00424     case PARK:
00425       this->joints_[LEFT]->SetVelocity(0, 0);
00426       this->joints_[RIGHT]->SetVelocity(0, 0);
00427       this->joints_[LEFT]->SetForce(0, 0);
00428       this->joints_[RIGHT]->SetForce(0, 0);
00429       break;
00430   };
00431 }
00432 
00436 void GazeboRsvBalance::FiniChild()
00437 {
00438   this->alive_ = false;
00439   this->queue_.clear();
00440   this->queue_.disable();
00441   this->gazebo_ros_->node()->shutdown();
00442   this->callback_queue_thread_.join();
00443 }
00444 
00445 void GazeboRsvBalance::QueueThread()
00446 {
00447   static const double timeout = 0.01;
00448   while (this->alive_ && this->gazebo_ros_->node()->ok())
00449   {
00450     this->queue_.callAvailable(ros::WallDuration(timeout));
00451   }
00452 }
00453 
00454 GZ_REGISTER_MODEL_PLUGIN(GazeboRsvBalance)
00455 }  // namespace gazebo


rsv_balance_gazebo
Author(s): Vitor Matos
autogenerated on Fri Feb 12 2016 00:23:36