kurt_gazebo_plugins: kurt_gazebo_plugins: gazebo_ros

00001 #include <kurt_gazebo_plugins/gazebo_ros_kurt.h>
00002 #include <nav_msgs/Odometry.h>
00003 #include <geometry_msgs/Twist.h>
00004 
00005 #include <gazebo/Joint.hh>
00006 #include <gazebo/Simulator.hh>
00007 #include <gazebo/GazeboError.hh>
00008 #include <gazebo/ControllerFactory.hh>
00009 
00010 #include <ros/time.h>
00011 
00012 using namespace gazebo;
00013 
00014 GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_kurt", GazeboRosKurt)
00015 
00016 // index of left / right middle wheel joint
00017 enum
00018 {
00019   LEFT = 1, RIGHT = 4
00020 };
00021 
00022 GazeboRosKurt::GazeboRosKurt(Entity *parent) :
00023   Controller(parent)
00024 {
00025   my_parent_ = dynamic_cast<Model*> (parent);
00026 
00027   if (!my_parent_)
00028     gzthrow("Gazebo_ROS_Kurt controller requires a Model as its parent");
00029 
00030   Param::Begin(&this->parameters);
00031   robotNamespaceP_ = new ParamT<std::string> ("robotNamespace", "/", 0);   // this MUST be called 'robotNamespace' for proper remapping to work
00032   cmd_vel_topic_nameP_ = new ParamT<std::string>("cmd_vel_topic_name", "", 1);
00033   odom_topic_nameP_ = new ParamT<std::string>("odom_topic_name", "", 1);
00034   joint_states_topic_nameP_ = new ParamT<std::string>("joint_states_topic_name", "", 1);
00035   joint_nameP_.push_back(new ParamT<std::string> ("left_front_wheel_joint", "left_front_wheel_joint", 1));
00036   joint_nameP_.push_back(new ParamT<std::string> ("left_middle_wheel_joint", "left_middle_wheel_joint", 1));
00037   joint_nameP_.push_back(new ParamT<std::string> ("left_rear_wheel_joint", "left_rear_wheel_joint", 1));
00038   joint_nameP_.push_back(new ParamT<std::string> ("right_front_wheel_joint", "right_front_wheel_joint", 1));
00039   joint_nameP_.push_back(new ParamT<std::string> ("right_middle_wheel_joint", "right_middle_wheel_joint", 1));
00040   joint_nameP_.push_back(new ParamT<std::string> ("right_rear_wheel_joint", "right_rear_wheel_joint", 1));
00041   wheel_sepP_ = new ParamT<float> ("wheel_separation", 0.34, 1);
00042   wheel_diamP_ = new ParamT<float> ("wheel_diameter", 0.15, 1);
00043   turning_adaptationP_ = new ParamT<float> ("turning_adaptation", 0.69, 1);
00044   torqueP_ = new ParamT<float> ("torque", 4.0, 1);
00045   max_velocityP_ = new ParamT<float> ("max_velocity", 1.0, 1);
00046   Param::End();
00047 
00048   wheel_speed_right_ = 0.0;
00049   wheel_speed_left_ = 0.0;
00050 
00051   for (size_t i = 0; i < NUM_JOINTS; ++i)
00052   {
00053     joints_[i] = NULL;
00054   }
00055 }
00056 
00057 GazeboRosKurt::~GazeboRosKurt()
00058 {
00059   delete wheel_diamP_;
00060   delete wheel_sepP_;
00061   delete turning_adaptationP_;
00062   delete torqueP_;
00063   delete max_velocityP_;
00064   delete robotNamespaceP_;
00065   delete cmd_vel_topic_nameP_;
00066   delete odom_topic_nameP_;
00067   delete joint_states_topic_nameP_;
00068 
00069   for (size_t i = 0; i < NUM_JOINTS; ++i)
00070   {
00071     delete joint_nameP_[i];
00072   }
00073 
00074   delete rosnode_;
00075 }
00076 
00077 void GazeboRosKurt::LoadChild(XMLConfigNode *node)
00078 {
00079   robotNamespaceP_->Load(node);
00080   if (!ros::isInitialized())
00081   {
00082     int argc = 0;
00083     char** argv = NULL;
00084     ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00085   }
00086 
00087   rosnode_ = new ros::NodeHandle(**robotNamespaceP_);
00088 
00089   cmd_vel_topic_nameP_->Load(node);
00090   cmd_vel_topic_name_ = cmd_vel_topic_nameP_->GetValue();
00091   odom_topic_nameP_->Load(node);
00092   odom_topic_name_ = odom_topic_nameP_->GetValue();
00093   joint_states_topic_nameP_->Load(node);
00094   joint_states_topic_name_ = joint_states_topic_nameP_->GetValue();
00095   wheel_sepP_->Load(node);
00096   wheel_diamP_->Load(node);
00097   turning_adaptationP_->Load(node);
00098   torqueP_->Load(node);
00099   max_velocityP_->Load(node);
00100   for (size_t i = 0; i < NUM_JOINTS; ++i)
00101   {
00102     joint_nameP_[i]->Load(node);
00103     joints_[i] = my_parent_->GetJoint(**joint_nameP_[i]);
00104     if (!joints_[i])
00105       gzthrow("The controller couldn't get joint " << **joint_nameP_[i]);
00106   }
00107 
00108   cmd_vel_sub_ = rosnode_->subscribe(cmd_vel_topic_name_, 1, &GazeboRosKurt::OnCmdVel, this);
00109   odom_pub_ = rosnode_->advertise<nav_msgs::Odometry> (odom_topic_name_, 1);
00110   joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState> (joint_states_topic_name_, 1);
00111 
00112   for (size_t i = 0; i < NUM_JOINTS; ++i)
00113   {
00114     js_.name.push_back(**joint_nameP_[i]);
00115     js_.position.push_back(0);
00116     js_.velocity.push_back(0);
00117     js_.effort.push_back(0);
00118   }
00119 
00120   ROS_INFO("gazebo_ros_kurt plugin initialized");
00121 }
00122 
00123 void GazeboRosKurt::InitChild()
00124 {
00125 }
00126 
00127 void GazeboRosKurt::FiniChild()
00128 {
00129   rosnode_->shutdown();
00130 }
00131 
00132 void GazeboRosKurt::UpdateChild()
00133 {
00134   double wd, ws;
00135   double d1, d2;
00136   double dr, da;
00137   double turning_adaptation;
00138   Time step_time;
00139 
00140   wd = **(wheel_diamP_);
00141   ws = **(wheel_sepP_);
00142   turning_adaptation = **(turning_adaptationP_);
00143 
00144   d1 = d2 = 0;
00145   dr = da = 0;
00146 
00147   step_time = Simulator::Instance()->GetSimTime() - prev_update_time_;
00148   prev_update_time_ = Simulator::Instance()->GetSimTime();
00149 
00150   // Distance travelled by middle wheels
00151   d1 = step_time.Double() * (wd / 2) * joints_[LEFT]->GetVelocity(0);
00152   d2 = step_time.Double() * (wd / 2) * joints_[RIGHT]->GetVelocity(0);
00153 
00154   dr = (d1 + d2) / 2;
00155   da = (d2 - d1) / ws * turning_adaptation;
00156 
00157   // Compute odometric pose
00158   odom_pose_[0] += dr * cos(odom_pose_[2]);
00159   odom_pose_[1] += dr * sin(odom_pose_[2]);
00160   odom_pose_[2] += da;
00161 
00162   // Compute odometric instantaneous velocity
00163   odom_vel_[0] = dr / step_time.Double();
00164   odom_vel_[1] = 0.0;
00165   odom_vel_[2] = da / step_time.Double();
00166 
00167   if (Simulator::Instance()->GetSimTime() > last_cmd_vel_time_ + Time(CMD_VEL_TIMEOUT))
00168   {
00169         ROS_DEBUG("gazebo_ros_kurt: cmd_vel timeout - current: %f, last cmd_vel: %f, timeout: %f", Simulator::Instance()->GetSimTime().Double(), last_cmd_vel_time_.Double(), Time(CMD_VEL_TIMEOUT).Double());
00170         wheel_speed_left_ = wheel_speed_right_ = 0.0;
00171   }
00172 
00173   ROS_DEBUG("gazebo_ros_kurt: setting wheel speeds (left; %f, right: %f)", wheel_speed_left_ / (wd / 2.0), wheel_speed_right_ / (wd / 2.0));
00174 
00175   // turn left wheels
00176   for (unsigned short i = 0; i < NUM_JOINTS/2; i++)
00177   {
00178     joints_[i]->SetVelocity(0, wheel_speed_left_ / (wd / 2.0));
00179     joints_[i]->SetMaxForce(0, **(torqueP_));
00180   }
00181 
00182   // turn right wheels
00183   for (unsigned short i = NUM_JOINTS/2; i < NUM_JOINTS; i++)
00184   {
00185     joints_[i]->SetVelocity(0, wheel_speed_right_ / (wd / 2.0));
00186     joints_[i]->SetMaxForce(0, **(torqueP_));
00187   }
00188 
00189   nav_msgs::Odometry odom;
00190   odom.header.stamp = ros::Time::now();
00191   odom.header.frame_id = "odom_combined";
00192   odom.child_frame_id = "base_footprint";
00193   odom.pose.pose.position.x = odom_pose_[0];
00194   odom.pose.pose.position.y = odom_pose_[1];
00195   odom.pose.pose.position.z = 0;
00196 
00197   btQuaternion qt;
00198   qt.setRPY(0, 0, odom_pose_[2]);
00199 
00200   odom.pose.pose.orientation.x = qt.getX();
00201   odom.pose.pose.orientation.y = qt.getY();
00202   odom.pose.pose.orientation.z = qt.getZ();
00203   odom.pose.pose.orientation.w = qt.getW();
00204 
00205   double pose_cov[36] = { 1e-3, 0, 0, 0, 0, 0,
00206                           0, 1e-3, 0, 0, 0, 0,
00207                           0, 0, 1e6, 0, 0, 0,
00208                           0, 0, 0, 1e6, 0, 0,
00209                           0, 0, 0, 0, 1e6, 0,
00210                           0, 0, 0, 0, 0, 1e3};
00211 
00212   memcpy(&odom.pose.covariance[0], pose_cov, sizeof(double) * 36);
00213   memcpy(&odom.twist.covariance[0], pose_cov, sizeof(double) * 36);
00214 
00215   odom.twist.twist.linear.x = odom_vel_[0];
00216   odom.twist.twist.linear.y = 0;
00217   odom.twist.twist.linear.z = 0;
00218 
00219   odom.twist.twist.angular.x = 0;
00220   odom.twist.twist.angular.y = 0;
00221   odom.twist.twist.angular.z = odom_vel_[2];
00222 
00223   odom_pub_.publish(odom);
00224 
00225   js_.header.stamp = ros::Time::now();
00226 
00227   for (size_t i = 0; i < NUM_JOINTS; ++i)
00228   {
00229     js_.position[i] = joints_[i]->GetAngle(0).GetAsRadian();
00230     js_.velocity[i] = joints_[i]->GetVelocity(0);
00231   }
00232 
00233   joint_state_pub_.publish(js_);
00234 }
00235 
00236 void GazeboRosKurt::OnCmdVel(const geometry_msgs::TwistConstPtr &msg)
00237 {
00238   double vr, va;
00239   vr = msg->linear.x;
00240   va = msg->angular.z;
00241 
00242   wheel_speed_left_ = vr - va * **(wheel_sepP_) / 2;
00243   wheel_speed_right_ = vr + va * **(wheel_sepP_) / 2;
00244 
00245   // limit wheel speed
00246   if (fabs(wheel_speed_left_) > **max_velocityP_)
00247     wheel_speed_left_ = copysign(**max_velocityP_, wheel_speed_left_);
00248   if (fabs(wheel_speed_right_) > **max_velocityP_)
00249     wheel_speed_right_ = copysign(**max_velocityP_, wheel_speed_right_);
00250 
00251   last_cmd_vel_time_ = Simulator::Instance()->GetSimTime();
00252 }
gazebo_ros_kurt.cpp
