gazebo_ros_create.cpp
Go to the documentation of this file.
00001 //#include <boost/bind.hpp>
00002 #include <nav_msgs/Odometry.h>
00003 #include <sensor_msgs/JointState.h>
00004 #include <geometry_msgs/Twist.h>
00005 #include <create_node/TurtlebotSensorState.h>
00006 
00007 #include "sensors/SensorManager.hh"
00008 #include "sensors/RaySensor.hh"
00009 
00010 #include <turtlebot_plugins/gazebo_ros_create.h>
00011 
00012 #include <ros/time.h>
00013 
00014 using namespace gazebo;
00015 
00016 enum {LEFT= 0, RIGHT=1, FRONT=2, REAR=3};
00017 
00018 GazeboRosCreate::GazeboRosCreate()
00019 {
00020   this->spinner_thread_ = new boost::thread( boost::bind( &GazeboRosCreate::spin, this) );
00021 
00022   wheel_speed_ = new float[2];
00023   wheel_speed_[LEFT] = 0.0;
00024   wheel_speed_[RIGHT] = 0.0;
00025 
00026   set_joints_[0] = false;
00027   set_joints_[1] = false;
00028   set_joints_[2] = false;
00029   set_joints_[3] = false;
00030   joints_[0].reset();
00031   joints_[1].reset();
00032   joints_[2].reset();
00033   joints_[3].reset();
00034 }
00035 
00036 GazeboRosCreate::~GazeboRosCreate()
00037 {
00038   rosnode_->shutdown();
00039   this->spinner_thread_->join();
00040   delete this->spinner_thread_;
00041   delete [] wheel_speed_;
00042   delete rosnode_;
00043 }
00044     
00045 void GazeboRosCreate::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00046 {
00047   this->my_world_ = _parent->GetWorld();
00048 
00049   this->my_parent_ = _parent;
00050   if (!this->my_parent_)
00051   {
00052     ROS_FATAL("Gazebo_ROS_Create controller requires a Model as its parent");
00053     return;
00054   }
00055 
00056 
00057   this->node_namespace_ = "";
00058   if (_sdf->HasElement("node_namespace"))
00059     this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";
00060 
00061 
00062   left_wheel_joint_name_ = "left_wheel_joint";
00063   if (_sdf->HasElement("left_wheel_joint"))
00064     left_wheel_joint_name_ = _sdf->GetElement("left_wheel_joint")->GetValueString();
00065 
00066   right_wheel_joint_name_ = "right_wheel_joint";
00067   if (_sdf->HasElement("right_wheel_joint"))
00068     right_wheel_joint_name_ = _sdf->GetElement("right_wheel_joint")->GetValueString();
00069 
00070   front_castor_joint_name_ = "front_castor_joint";
00071   if (_sdf->HasElement("front_castor_joint"))
00072     front_castor_joint_name_ = _sdf->GetElement("front_castor_joint")->GetValueString();
00073 
00074   rear_castor_joint_name_ = "rear_castor_joint";
00075   if (_sdf->HasElement("rear_castor_joint"))
00076     rear_castor_joint_name_ = _sdf->GetElement("rear_castor_joint")->GetValueString();
00077 
00078   wheel_sep_ = 0.34;
00079   if (_sdf->HasElement("wheel_separation"))
00080     wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
00081 
00082   wheel_sep_ = 0.34;
00083   if (_sdf->HasElement("wheel_separation"))
00084     wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
00085 
00086   wheel_diam_ = 0.15;
00087   if (_sdf->HasElement("wheel_diameter"))
00088     wheel_diam_ = _sdf->GetElement("wheel_diameter")->GetValueDouble();
00089 
00090   torque_ = 10.0;
00091   if (_sdf->HasElement("torque"))
00092     torque_ = _sdf->GetElement("torque")->GetValueDouble();
00093 
00094 
00095   //base_geom_name_ = "base_geom";
00096   base_geom_name_ = "base_footprint_geom_base_link";
00097   if (_sdf->HasElement("base_geom"))
00098     base_geom_name_ = _sdf->GetElement("base_geom")->GetValueString();
00099   base_geom_ = my_parent_->GetChildCollision(base_geom_name_);
00100   if (!base_geom_)
00101   {
00102     // This is a hack for ROS Diamond back. E-turtle and future releases
00103     // will not need this, because it will contain the fixed-joint reduction
00104     // in urdf2gazebo
00105     base_geom_ = my_parent_->GetChildCollision("base_footprint_geom");
00106     if (!base_geom_)
00107     {
00108       ROS_ERROR("Unable to find geom[%s]",base_geom_name_.c_str());
00109       return;
00110     }
00111   }
00112 
00113   base_geom_->SetContactsEnabled(true);
00114   contact_event_ = base_geom_->ConnectContact(boost::bind(&GazeboRosCreate::OnContact, this, _1, _2));
00115 
00116   // Get then name of the parent model
00117   std::string modelName = _sdf->GetParent()->GetValueString("name");
00118 
00119   // Listen to the update event. This event is broadcast every
00120   // simulation iteration.
00121   this->updateConnection = event::Events::ConnectWorldUpdateStart(
00122       boost::bind(&GazeboRosCreate::UpdateChild, this));
00123   gzdbg << "plugin model name: " << modelName << "\n";
00124 
00125   wall_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
00126     sensors::SensorManager::Instance()->GetSensor("wall_sensor"));
00127   if (!wall_sensor_)
00128   {
00129     ROS_ERROR("Unable to find sensor[wall_sensor]");
00130     return;
00131   }
00132 
00133   left_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
00134     sensors::SensorManager::Instance()->GetSensor("left_cliff_sensor"));
00135   right_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
00136     sensors::SensorManager::Instance()->GetSensor("right_cliff_sensor"));
00137   leftfront_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
00138     sensors::SensorManager::Instance()->GetSensor("leftfront_cliff_sensor"));
00139   rightfront_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
00140     sensors::SensorManager::Instance()->GetSensor("rightfront_cliff_sensor"));
00141 
00142   wall_sensor_->SetActive(true);
00143   left_cliff_sensor_->SetActive(true);
00144   right_cliff_sensor_->SetActive(true);
00145   rightfront_cliff_sensor_->SetActive(true);
00146   leftfront_cliff_sensor_->SetActive(true);
00147 
00148   if (!ros::isInitialized())
00149   {
00150     int argc = 0;
00151     char** argv = NULL;
00152     ros::init(argc, argv, "gazebo_turtlebot", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00153   }
00154 
00155   rosnode_ = new ros::NodeHandle( node_namespace_ );
00156 
00157   cmd_vel_sub_ = rosnode_->subscribe("/cmd_vel", 1, &GazeboRosCreate::OnCmdVel, this );
00158 
00159   sensor_state_pub_ = rosnode_->advertise<create_node::TurtlebotSensorState>("sensor_state", 1);
00160   odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);
00161 
00162   joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states", 1);
00163 
00164   js_.name.push_back( left_wheel_joint_name_ );
00165   js_.position.push_back(0);
00166   js_.velocity.push_back(0);
00167   js_.effort.push_back(0);
00168 
00169   js_.name.push_back( right_wheel_joint_name_ );
00170   js_.position.push_back(0);
00171   js_.velocity.push_back(0);
00172   js_.effort.push_back(0);
00173 
00174   js_.name.push_back( front_castor_joint_name_ );
00175   js_.position.push_back(0);
00176   js_.velocity.push_back(0);
00177   js_.effort.push_back(0);
00178 
00179   js_.name.push_back( rear_castor_joint_name_ );
00180   js_.position.push_back(0);
00181   js_.velocity.push_back(0);
00182   js_.effort.push_back(0);
00183 
00184   prev_update_time_ = 0;
00185   last_cmd_vel_time_ = 0;
00186 
00187 
00188   sensor_state_.bumps_wheeldrops = 0x0;
00189 
00190   //TODO: fix this
00191 
00192   joints_[LEFT] = my_parent_->GetJoint(left_wheel_joint_name_);
00193   joints_[RIGHT] = my_parent_->GetJoint(right_wheel_joint_name_);
00194   joints_[FRONT] = my_parent_->GetJoint(front_castor_joint_name_);
00195   joints_[REAR] = my_parent_->GetJoint(rear_castor_joint_name_);
00196 
00197   if (joints_[LEFT]) set_joints_[LEFT] = true;
00198   if (joints_[RIGHT]) set_joints_[RIGHT] = true;
00199   if (joints_[FRONT]) set_joints_[FRONT] = true;
00200   if (joints_[REAR]) set_joints_[REAR] = true;
00201 
00202   //initialize time and odometry position
00203   prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
00204   odom_pose_[0] = 0.0;
00205   odom_pose_[1] = 0.0;
00206   odom_pose_[2] = 0.0;
00207 }
00208 
00209 
00210 void GazeboRosCreate::OnContact(const std::string &name, const physics::Contact &contact)
00211 {
00212   float y_overlap = 0.16495 * sin( 10 * (M_PI/180.0) );
00213 
00214   for (unsigned int j=0; j < (unsigned int)contact.count; j++)
00215   {
00216     // Make sure the contact is on the front bumper
00217     if (contact.positions[j].x > 0.012 && contact.positions[j].z < 0.06 && 
00218         contact.positions[j].z > 0.01)
00219     {
00220       // Right bump sensor
00221       if (contact.positions[j].y <= y_overlap)
00222         sensor_state_.bumps_wheeldrops |= 0x1; 
00223       // Left bump sensor
00224       if (contact.positions[j].y >= -y_overlap)
00225         sensor_state_.bumps_wheeldrops |= 0x2; 
00226     }
00227   }
00228 
00229 }
00230 
00231 void GazeboRosCreate::UpdateChild()
00232 {
00233   common::Time time_now = this->my_world_->GetSimTime();
00234   common::Time step_time = time_now - prev_update_time_;
00235   prev_update_time_ = time_now;
00236 
00237   double wd, ws;
00238   double d1, d2;
00239   double dr, da;
00240 
00241   wd = wheel_diam_;
00242   ws = wheel_sep_;
00243 
00244   d1 = d2 = 0;
00245   dr = da = 0;
00246 
00247   // Distance travelled by front wheels
00248   if (set_joints_[LEFT])
00249     d1 = step_time.Double() * (wd / 2) * joints_[LEFT]->GetVelocity(0);
00250   if (set_joints_[RIGHT])
00251     d2 = step_time.Double() * (wd / 2) * joints_[RIGHT]->GetVelocity(0);
00252 
00253   // Can see NaN values here, just zero them out if needed
00254   if (isnan(d1)) {
00255     ROS_WARN_THROTTLE(0.1, "Gazebo ROS Create plugin. NaN in d1. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[LEFT]->GetVelocity(0));
00256     d1 = 0;
00257   }
00258 
00259   if (isnan(d2)) {
00260     ROS_WARN_THROTTLE(0.1, "Gazebo ROS Create plugin. NaN in d2. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[RIGHT]->GetVelocity(0));
00261     d2 = 0;
00262   }
00263 
00264   dr = (d1 + d2) / 2;
00265   da = (d2 - d1) / ws;
00266 
00267   // Compute odometric pose
00268   odom_pose_[0] += dr * cos( odom_pose_[2] );
00269   odom_pose_[1] += dr * sin( odom_pose_[2] );
00270   odom_pose_[2] += da;
00271 
00272   // Compute odometric instantaneous velocity
00273   odom_vel_[0] = dr / step_time.Double();
00274   odom_vel_[1] = 0.0;
00275   odom_vel_[2] = da / step_time.Double();
00276 
00277 
00278   if (set_joints_[LEFT])
00279   {
00280     joints_[LEFT]->SetVelocity( 0, wheel_speed_[LEFT] / (wd/2.0) );
00281     joints_[LEFT]->SetMaxForce( 0, torque_ );
00282   }
00283   if (set_joints_[RIGHT])
00284   {
00285     joints_[RIGHT]->SetVelocity( 0, wheel_speed_[RIGHT] / (wd / 2.0) );
00286     joints_[RIGHT]->SetMaxForce( 0, torque_ );
00287   }
00288 
00289   nav_msgs::Odometry odom;
00290   odom.header.stamp.sec = time_now.sec;
00291   odom.header.stamp.nsec = time_now.nsec;
00292   odom.header.frame_id = "odom";
00293   odom.child_frame_id = "base_footprint";
00294   odom.pose.pose.position.x = odom_pose_[0];
00295   odom.pose.pose.position.y = odom_pose_[1];
00296   odom.pose.pose.position.z = 0;
00297 
00298   tf::Quaternion qt;
00299   qt.setEuler(0,0,odom_pose_[2]);
00300 
00301   odom.pose.pose.orientation.x = qt.getX();
00302   odom.pose.pose.orientation.y = qt.getY();
00303   odom.pose.pose.orientation.z = qt.getZ();
00304   odom.pose.pose.orientation.w = qt.getW();
00305 
00306   double pose_cov[36] = { 1e-3, 0, 0, 0, 0, 0,
00307                           0, 1e-3, 0, 0, 0, 0,
00308                           0, 0, 1e6, 0, 0, 0,
00309                           0, 0, 0, 1e6, 0, 0,
00310                           0, 0, 0, 0, 1e6, 0,
00311                           0, 0, 0, 0, 0, 1e3};
00312 
00313   memcpy( &odom.pose.covariance[0], pose_cov, sizeof(double)*36 );
00314   memcpy( &odom.twist.covariance[0], pose_cov, sizeof(double)*36 );
00315 
00316   odom.twist.twist.linear.x = 0;
00317   odom.twist.twist.linear.y = 0;
00318   odom.twist.twist.linear.z = 0;
00319 
00320   odom.twist.twist.angular.x = 0;
00321   odom.twist.twist.angular.y = 0;
00322   odom.twist.twist.angular.z = 0;
00323 
00324   odom_pub_.publish( odom ); 
00325 
00326   js_.header.stamp.sec = time_now.sec;
00327   js_.header.stamp.nsec = time_now.nsec;
00328   if (this->set_joints_[LEFT])
00329   {
00330     js_.position[0] = joints_[LEFT]->GetAngle(0).GetAsRadian();
00331     js_.velocity[0] = joints_[LEFT]->GetVelocity(0);
00332   }
00333 
00334   if (this->set_joints_[RIGHT])
00335   {
00336     js_.position[1] = joints_[RIGHT]->GetAngle(0).GetAsRadian();
00337     js_.velocity[1] = joints_[RIGHT]->GetVelocity(0);
00338   }
00339 
00340   if (this->set_joints_[FRONT])
00341   {
00342     js_.position[2] = joints_[FRONT]->GetAngle(0).GetAsRadian();
00343     js_.velocity[2] = joints_[FRONT]->GetVelocity(0);
00344   }
00345 
00346   if (this->set_joints_[REAR])
00347   {
00348     js_.position[3] = joints_[REAR]->GetAngle(0).GetAsRadian();
00349     js_.velocity[3] = joints_[REAR]->GetVelocity(0);
00350   }
00351 
00352   joint_state_pub_.publish( js_ );
00353 
00354   this->UpdateSensors();
00355 
00356   //timeout if didn't receive cmd in a while
00357   common::Time time_since_last_cmd = time_now - last_cmd_vel_time_;
00358   if (time_since_last_cmd.Double() > 0.6)
00359   {
00360     wheel_speed_[LEFT] = 0;
00361     wheel_speed_[RIGHT] = 0;
00362   }
00363 
00364 }
00365 
00366 void GazeboRosCreate::UpdateSensors()
00367 {
00368   if (wall_sensor_->GetRange(0) < 0.04)
00369     sensor_state_.wall = true;
00370   else
00371     sensor_state_.wall = false;
00372 
00373   if (left_cliff_sensor_->GetRange(0) > 0.02)
00374     sensor_state_.cliff_left = true;
00375   else
00376     sensor_state_.cliff_left = false;
00377 
00378   if (right_cliff_sensor_->GetRange(0) > 0.02)
00379     sensor_state_.cliff_right = true;
00380   else
00381     sensor_state_.cliff_right = false;
00382 
00383   if (rightfront_cliff_sensor_->GetRange(0) > 0.02)
00384     sensor_state_.cliff_front_right = true;
00385   else
00386     sensor_state_.cliff_front_right = false;
00387 
00388   if (leftfront_cliff_sensor_->GetRange(0) > 0.02)
00389     sensor_state_.cliff_front_left = true;
00390   else
00391     sensor_state_.cliff_front_left = false;
00392 
00393   sensor_state_pub_.publish(sensor_state_);
00394 
00395   // Reset the bump sensors
00396   sensor_state_.bumps_wheeldrops = 0x0;
00397 }
00398 
00399 void GazeboRosCreate::OnCmdVel( const geometry_msgs::TwistConstPtr &msg)
00400 {
00401   last_cmd_vel_time_ = this->my_world_->GetSimTime();
00402   double vr, va;
00403   vr = msg->linear.x;
00404   va = msg->angular.z;
00405 
00406   wheel_speed_[LEFT] = vr - va * (wheel_sep_) / 2;
00407   wheel_speed_[RIGHT] = vr + va * (wheel_sep_) / 2;
00408 }
00409 
00410 void GazeboRosCreate::spin()
00411 {
00412   while(ros::ok()) ros::spinOnce();
00413 }
00414 
00415 GZ_REGISTER_MODEL_PLUGIN(GazeboRosCreate);
00416 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends


turtlebot_gazebo_plugins
Author(s): Nate Koenig
autogenerated on Mon Dec 24 2012 23:58:56