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