gazebo_ros_diffdrive_uos.cpp
Go to the documentation of this file.
00001 #include <diffdrive_gazebo_plugin/gazebo_ros_diffdrive_uos.h>
00002 #include <nav_msgs/Odometry.h>
00003 #include <geometry_msgs/Twist.h>
00004 
00005 #include <ros/time.h>
00006 
00007 #include <tf/transform_broadcaster.h>
00008 
00009 using namespace gazebo;
00010 
00011 // index of left / right middle wheel joint
00012 enum
00013 {
00014   LEFT = 1, RIGHT = 4
00015 };
00016 
00017 GazeboRosDiffdrive::GazeboRosDiffdrive() :
00018   wheel_speed_right_(0.0),
00019   wheel_speed_left_(0.0)
00020 {
00021   this->spinner_thread_ = new boost::thread( boost::bind( &GazeboRosDiffdrive::spin, this) );
00022 
00023   for (size_t i = 0; i < NUM_JOINTS; ++i)
00024   {
00025     joints_[i].reset();
00026   }
00027 }
00028 
00029 GazeboRosDiffdrive::~GazeboRosDiffdrive()
00030 {
00031   rosnode_->shutdown();
00032   this->spinner_thread_->join();
00033   delete this->spinner_thread_;
00034   delete rosnode_;
00035 }
00036 
00037 void GazeboRosDiffdrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00038 {
00039   this->my_world_ = _parent->GetWorld();
00040 
00041   this->my_parent_ = _parent;
00042   if (!this->my_parent_)
00043   {
00044     ROS_FATAL("Gazebo_ROS_Create controller requires a Model as its parent");
00045     return;
00046   }
00047 
00048 
00049   // this MUST be called 'robotNamespace' for proper remapping to work
00050   // TODO: could be 'node_namespace' now
00051   this->node_namespace_ = "/";
00052   if (_sdf->HasElement("robotNamespace"))
00053     this->node_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00054 
00055 
00056   cmd_vel_topic_name_ = "/cmd_vel";
00057   if (_sdf->HasElement("cmd_vel_topic_name"))
00058     cmd_vel_topic_name_ = _sdf->GetElement("cmd_vel_topic_name")->GetValueString();
00059 
00060   odom_topic_name_ = "/odom";
00061   if (_sdf->HasElement("odom_topic_name"))
00062     odom_topic_name_ = _sdf->GetElement("odom_topic_name")->GetValueString();
00063 
00064   joint_states_topic_name_ = "/joint_states";
00065   if (_sdf->HasElement("joint_states_topic_name"))
00066     joint_states_topic_name_ = _sdf->GetElement("joint_states_topic_name")->GetValueString();
00067 
00068   js_.name.resize(NUM_JOINTS);
00069   js_.position.resize(NUM_JOINTS);
00070   js_.velocity.resize(NUM_JOINTS);
00071   js_.effort.resize(NUM_JOINTS);
00072 
00073   for (size_t i = 0; i < NUM_JOINTS; ++i)
00074   {
00075     js_.position[i] = 0;
00076     js_.velocity[i] = 0;
00077     js_.effort[i] = 0;
00078   }
00079 
00080   js_.name[0] = "left_front_wheel_joint";
00081   if (_sdf->HasElement("left_front_wheel_joint"))
00082     js_.name[0] = _sdf->GetElement("left_front_wheel_joint")->GetValueString();
00083 
00084   js_.name[1] = "left_middle_wheel_joint";
00085   if (_sdf->HasElement("left_middle_wheel_joint"))
00086     js_.name[1] = _sdf->GetElement("left_middle_wheel_joint")->GetValueString();
00087 
00088   js_.name[2] = "left_rear_wheel_joint";
00089   if (_sdf->HasElement("left_rear_wheel_joint"))
00090     js_.name[2] = _sdf->GetElement("left_rear_wheel_joint")->GetValueString();
00091 
00092   js_.name[3] = "right_front_wheel_joint";
00093   if (_sdf->HasElement("right_front_wheel_joint"))
00094     js_.name[3] = _sdf->GetElement("right_front_wheel_joint")->GetValueString();
00095 
00096   js_.name[4] = "right_middle_wheel_joint";
00097   if (_sdf->HasElement("right_middle_wheel_joint"))
00098     js_.name[5] = _sdf->GetElement("right_middle_wheel_joint")->GetValueString();
00099 
00100   js_.name[5] = "right_rear_wheel_joint";
00101   if (_sdf->HasElement("right_rear_wheel_joint"))
00102     js_.name[5] = _sdf->GetElement("right_rear_wheel_joint")->GetValueString();
00103 
00104   wheel_sep_ = 0.34;
00105   if (_sdf->HasElement("wheel_separation"))
00106     wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
00107 
00108   turning_adaptation_ = 0.15;
00109   if (_sdf->HasElement("turning_adaptation"))
00110     turning_adaptation_ = _sdf->GetElement("turning_adaptation")->GetValueDouble();
00111 
00112   wheel_diam_ = 0.15;
00113   if (_sdf->HasElement("wheel_diameter"))
00114     wheel_diam_ = _sdf->GetElement("wheel_diameter")->GetValueDouble();
00115 
00116   torque_ = 4.0;
00117   if (_sdf->HasElement("torque"))
00118     torque_ = _sdf->GetElement("torque")->GetValueDouble();
00119 
00120   max_velocity_ = 4.0;
00121   if (_sdf->HasElement("max_velocity"))
00122     max_velocity_ = _sdf->GetElement("max_velocity")->GetValueDouble();
00123 
00124 
00125   if (!ros::isInitialized())
00126   {
00127     int argc = 0;
00128     char** argv = NULL;
00129     ros::init(argc, argv, "gazebo_ros_diffdrive_uos", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00130   }
00131 
00132   rosnode_ = new ros::NodeHandle(node_namespace_);
00133 
00134   cmd_vel_sub_ = rosnode_->subscribe(cmd_vel_topic_name_, 1, &GazeboRosDiffdrive::OnCmdVel, this);
00135   odom_pub_ = rosnode_->advertise<nav_msgs::Odometry> (odom_topic_name_, 1);
00136   joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState> (joint_states_topic_name_, 1);
00137 
00138   for (size_t i = 0; i < NUM_JOINTS; ++i)
00139   {
00140     joints_[i] = my_parent_->GetJoint(js_.name[i]);
00141     if (!joints_[i])
00142       gzthrow("The controller couldn't get joint " << js_.name[i]);
00143   }
00144 
00145   //initialize time and odometry position
00146   prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
00147   odom_pose_[0] = 0.0;
00148   odom_pose_[1] = 0.0;
00149   odom_pose_[2] = 0.0;
00150 
00151   // Get then name of the parent model
00152   std::string modelName = _sdf->GetParent()->GetValueString("name");
00153 
00154   // Listen to the update event. This event is broadcast every
00155   // simulation iteration.
00156   this->updateConnection = event::Events::ConnectWorldUpdateStart(
00157       boost::bind(&GazeboRosDiffdrive::UpdateChild, this));
00158   gzdbg << "plugin model name: " << modelName << "\n";
00159 
00160   ROS_INFO("gazebo_ros_diffdrive_uos plugin initialized");
00161 }
00162 
00163 void GazeboRosDiffdrive::UpdateChild()
00164 {
00165   common::Time time_now = this->my_world_->GetSimTime();
00166   common::Time step_time = time_now - prev_update_time_;
00167   prev_update_time_ = time_now;
00168 
00169   double wd, ws;
00170   double d1, d2;
00171   double dr, da;
00172   double turning_adaptation;
00173 
00174   wd = wheel_diam_;
00175   ws = wheel_sep_;
00176   turning_adaptation = turning_adaptation_;
00177 
00178   d1 = d2 = 0;
00179   dr = da = 0;
00180 
00181   // Distance travelled by middle wheels
00182   d1 = step_time.Double() * (wd / 2) * joints_[LEFT]->GetVelocity(0);
00183   d2 = step_time.Double() * (wd / 2) * joints_[RIGHT]->GetVelocity(0);
00184 
00185   // Can see NaN values here, just zero them out if needed
00186   if (isnan(d1)) {
00187     ROS_WARN_THROTTLE(0.1, "gazebo_ros_diffdrive_uos: NaN in d1. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[LEFT]->GetVelocity(0));
00188     d1 = 0;
00189   }
00190 
00191   if (isnan(d2)) {
00192     ROS_WARN_THROTTLE(0.1, "gazebo_ros_diffdrive_uos: NaN in d2. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[RIGHT]->GetVelocity(0));
00193     d2 = 0;
00194   }
00195 
00196   dr = (d1 + d2) / 2;
00197   da = (d2 - d1) / ws * turning_adaptation;
00198 
00199   // Compute odometric pose
00200   odom_pose_[0] += dr * cos(odom_pose_[2]);
00201   odom_pose_[1] += dr * sin(odom_pose_[2]);
00202   odom_pose_[2] += da;
00203 
00204   // Compute odometric instantaneous velocity
00205   odom_vel_[0] = dr / step_time.Double();
00206   odom_vel_[1] = 0.0;
00207   odom_vel_[2] = da / step_time.Double();
00208 
00209   if (this->my_world_->GetSimTime() > last_cmd_vel_time_ + common::Time(CMD_VEL_TIMEOUT))
00210   {
00211         ROS_DEBUG("gazebo_ros_diffdrive_uos: cmd_vel timeout - current: %f, last cmd_vel: %f, timeout: %f", this->my_world_->GetSimTime().Double(), last_cmd_vel_time_.Double(), common::Time(CMD_VEL_TIMEOUT).Double());
00212         wheel_speed_left_ = wheel_speed_right_ = 0.0;
00213   }
00214 
00215   ROS_DEBUG("gazebo_ros_diffdrive_uos: setting wheel speeds (left; %f, right: %f)", wheel_speed_left_ / (wd / 2.0), wheel_speed_right_ / (wd / 2.0));
00216 
00217   // turn left wheels
00218   for (unsigned short i = 0; i < NUM_JOINTS/2; i++)
00219   {
00220     joints_[i]->SetVelocity(0, wheel_speed_left_ / (wd / 2.0));
00221     joints_[i]->SetMaxForce(0, torque_);
00222   }
00223 
00224   // turn right wheels
00225   for (unsigned short i = NUM_JOINTS/2; i < NUM_JOINTS; i++)
00226   {
00227     joints_[i]->SetVelocity(0, wheel_speed_right_ / (wd / 2.0));
00228     joints_[i]->SetMaxForce(0, torque_);
00229   }
00230 
00231   nav_msgs::Odometry odom;
00232   odom.header.stamp.sec = time_now.sec;
00233   odom.header.stamp.nsec = time_now.nsec;
00234   odom.header.frame_id = "odom_combined";
00235   odom.child_frame_id = "base_footprint";
00236   odom.pose.pose.position.x = odom_pose_[0];
00237   odom.pose.pose.position.y = odom_pose_[1];
00238   odom.pose.pose.position.z = 0;
00239 
00240   tf::Quaternion qt;
00241   qt.setEuler(0, 0, odom_pose_[2]);
00242 
00243   odom.pose.pose.orientation.x = qt.getX();
00244   odom.pose.pose.orientation.y = qt.getY();
00245   odom.pose.pose.orientation.z = qt.getZ();
00246   odom.pose.pose.orientation.w = qt.getW();
00247 
00248   double pose_cov[36] = { 1e-3, 0, 0, 0, 0, 0,
00249                           0, 1e-3, 0, 0, 0, 0,
00250                           0, 0, 1e6, 0, 0, 0,
00251                           0, 0, 0, 1e6, 0, 0,
00252                           0, 0, 0, 0, 1e6, 0,
00253                           0, 0, 0, 0, 0, 1e3};
00254 
00255   memcpy(&odom.pose.covariance[0], pose_cov, sizeof(double) * 36);
00256   memcpy(&odom.twist.covariance[0], pose_cov, sizeof(double) * 36);
00257 
00258   odom.twist.twist.linear.x = odom_vel_[0];
00259   odom.twist.twist.linear.y = 0;
00260   odom.twist.twist.linear.z = 0;
00261 
00262   odom.twist.twist.angular.x = 0;
00263   odom.twist.twist.angular.y = 0;
00264   odom.twist.twist.angular.z = odom_vel_[2];
00265 
00266   odom_pub_.publish(odom);
00267 
00268   js_.header.stamp.sec = time_now.sec;
00269   js_.header.stamp.nsec = time_now.nsec;
00270 
00271   for (size_t i = 0; i < NUM_JOINTS; ++i)
00272   {
00273     js_.position[i] = joints_[i]->GetAngle(0).Radian();
00274     js_.velocity[i] = joints_[i]->GetVelocity(0);
00275   }
00276 
00277   joint_state_pub_.publish(js_);
00278 }
00279 
00280 void GazeboRosDiffdrive::OnCmdVel(const geometry_msgs::TwistConstPtr &msg)
00281 {
00282   last_cmd_vel_time_ = this->my_world_->GetSimTime();
00283   double vr, va;
00284   vr = msg->linear.x;
00285   va = msg->angular.z;
00286 
00287   wheel_speed_left_ = vr - va * wheel_sep_ / 2;
00288   wheel_speed_right_ = vr + va * wheel_sep_ / 2;
00289 
00290   // limit wheel speed
00291   if (fabs(wheel_speed_left_) > max_velocity_)
00292     wheel_speed_left_ = copysign(max_velocity_, wheel_speed_left_);
00293   if (fabs(wheel_speed_right_) > max_velocity_)
00294     wheel_speed_right_ = copysign(max_velocity_, wheel_speed_right_);
00295 }
00296 
00297 void GazeboRosDiffdrive::spin()
00298 {
00299   while(ros::ok()) ros::spinOnce();
00300 }
00301 
00302 GZ_REGISTER_MODEL_PLUGIN(GazeboRosDiffdrive);


diffdrive_gazebo_plugin
Author(s): Martin Günther
autogenerated on Sun Oct 5 2014 23:28:33