00001
00002
00003
00004
00005
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
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
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
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
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
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
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
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
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
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
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
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
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
00143 this->callback_queue_thread_ = boost::thread(boost::bind(&GazeboRsvBalance::QueueThread, this));
00144
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
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
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
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
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
00275
00276
00277
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
00282
00283
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
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
00316 this->odom_.header.stamp = current_time;
00317 this->odom_.header.frame_id = odom_frame;
00318 this->odom_.child_frame_id = base_frame;
00319
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
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
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 }