turtlebot_gazebo_plugins: turtlebot_gazebo_plugins: gazebo_ros

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