kurt_base: kurt_base: kurt_base.cc Source File

00001 #include <string>
00002 
00003 #include <ros/ros.h>
00004 #include <ros/console.h>
00005 
00006 #include <geometry_msgs/Twist.h>
00007 #include <nav_msgs/Odometry.h>
00008 #include <tf/transform_broadcaster.h>
00009 #include <tf/transform_listener.h>
00010 #include <sensor_msgs/JointState.h>
00011 #include <sensor_msgs/Imu.h>
00012 #include <sensor_msgs/Range.h>
00013 
00014 #include "kurt.h"
00015 #include "comm.h"
00016 
00017 class ROSComm : public Comm
00018 {
00019   public:
00020     ROSComm(
00021         const ros::NodeHandle &n,
00022         double sigma_x,
00023         double sigma_theta,
00024         double cov_x_y,
00025         double cov_x_theta,
00026         double cov_y_theta,
00027         int ticks_per_turn_of_wheel) :
00028       n_(n),
00029       sigma_x_(sigma_x),
00030       sigma_theta_(sigma_theta),
00031       cov_x_y_(cov_x_y),
00032       cov_x_theta_(cov_x_theta),
00033       cov_y_theta_(cov_y_theta),
00034       ticks_per_turn_of_wheel_(ticks_per_turn_of_wheel),
00035       publish_tf_(false),
00036       odom_pub_(n_.advertise<nav_msgs::Odometry> ("odom", 10)),
00037       range_pub_(n_.advertise<sensor_msgs::Range> ("range", 10)),
00038       imu_pub_(n_.advertise<sensor_msgs::Imu> ("imu", 10)),
00039       joint_pub_(n_.advertise<sensor_msgs::JointState> ("joint_states", 1)) { }
00040     virtual void send_odometry(double z, double x, double theta, double
00041         v_encoder, double v_encoder_angular, int wheel_a, int wheel_b, double
00042         v_encoder_left, double v_encoder_right); virtual void
00043       send_sonar_leftBack(int ir_left_back);
00044     virtual void send_sonar_front_usound_leftFront_left(int ir_right_front, int
00045         usound, int ir_left_front, int ir_left);
00046     virtual void send_sonar_back_rightBack_rightFront(int ir_back, int
00047         ir_right_back, int ir_right);
00048     virtual void send_pitch_roll(double pitch, double roll);
00049     virtual void send_gyro(double theta, double sigma);
00050     virtual void send_rotunit(double rot);
00051 
00052     void setTFPrefix(const std::string &tf_prefix);
00053 
00054   private:
00055     void populateCovariance(nav_msgs::Odometry &msg, double v_encoder, double
00056         v_encoder_angular);
00057 
00058     ros::NodeHandle n_;
00059     double sigma_x_, sigma_theta_, cov_x_y_, cov_x_theta_, cov_y_theta_;
00060     int ticks_per_turn_of_wheel_;
00061     bool publish_tf_;
00062     std::string tf_prefix_;
00063 
00064     tf::TransformBroadcaster odom_broadcaster_;
00065     ros::Publisher odom_pub_;
00066     ros::Publisher range_pub_;
00067     ros::Publisher imu_pub_;
00068     ros::Publisher joint_pub_;
00069 };
00070 
00071 void ROSComm::setTFPrefix(const std::string &tf_prefix)
00072 {
00073   tf_prefix_ = tf_prefix;
00074 }
00075 
00076 void ROSComm::populateCovariance(nav_msgs::Odometry &msg, double v_encoder, double v_encoder_angular)
00077 {
00078   double odom_multiplier = 1.0;
00079 
00080   if (fabs(v_encoder) <= 1e-8 && fabs(v_encoder_angular) <= 1e-8)
00081   {
00082     //nav_msgs::Odometry has a 6x6 covariance matrix
00083     msg.twist.covariance[0] = 1e-12;
00084     msg.twist.covariance[35] = 1e-12;
00085 
00086     msg.twist.covariance[30] = 1e-12;
00087     msg.twist.covariance[5] = 1e-12;
00088   }
00089   else
00090   {
00091     //nav_msgs::Odometry has a 6x6 covariance matrix
00092     msg.twist.covariance[0] = odom_multiplier * pow(sigma_x_, 2);
00093     msg.twist.covariance[35] = odom_multiplier * pow(sigma_theta_, 2);
00094 
00095     msg.twist.covariance[30] = odom_multiplier * cov_x_theta_;
00096     msg.twist.covariance[5] = odom_multiplier * cov_x_theta_;
00097   }
00098 
00099   msg.twist.covariance[7] = DBL_MAX;
00100   msg.twist.covariance[14] = DBL_MAX;
00101   msg.twist.covariance[21] = DBL_MAX;
00102   msg.twist.covariance[28] = DBL_MAX;
00103 
00104   msg.pose.covariance = msg.twist.covariance;
00105 
00106   if (fabs(v_encoder) <= 1e-8 && fabs(v_encoder_angular) <= 1e-8)
00107   {
00108     msg.pose.covariance[7] = 1e-12;
00109 
00110     msg.pose.covariance[1] = 1e-12;
00111     msg.pose.covariance[6] = 1e-12;
00112 
00113     msg.pose.covariance[31] = 1e-12;
00114     msg.pose.covariance[11] = 1e-12;
00115   }
00116   else
00117   {
00118     msg.pose.covariance[7] = odom_multiplier * pow(sigma_x_, 2) * pow(sigma_theta_, 2);
00119 
00120     msg.pose.covariance[1] = odom_multiplier * cov_x_y_;
00121     msg.pose.covariance[6] = odom_multiplier * cov_x_y_;
00122 
00123     msg.pose.covariance[31] = odom_multiplier * cov_y_theta_;
00124     msg.pose.covariance[11] = odom_multiplier * cov_y_theta_;
00125   }
00126 }
00127 
00128 void ROSComm::send_odometry(double z, double x, double theta, double v_encoder, double v_encoder_angular, int wheel_a, int wheel_b, double v_encoder_left, double v_encoder_right)
00129 {
00130   nav_msgs::Odometry odom;
00131   odom.header.frame_id = tf::resolve(tf_prefix_, "odom_combined");
00132   odom.child_frame_id = tf::resolve(tf_prefix_, "base_footprint");
00133 
00134   odom.header.stamp = ros::Time::now();
00135   odom.pose.pose.position.x = z;
00136   odom.pose.pose.position.y = -x;
00137   odom.pose.pose.position.z = 0.0;
00138   odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(-theta);
00139 
00140   odom.twist.twist.linear.x = v_encoder;
00141   odom.twist.twist.linear.y = 0.0;
00142   odom.twist.twist.angular.z = v_encoder_angular;
00143   populateCovariance(odom, v_encoder, v_encoder_angular);
00144 
00145   odom_pub_.publish(odom);
00146 
00147   if (publish_tf_)
00148   {
00149     geometry_msgs::TransformStamped odom_trans;
00150     odom_trans.header.frame_id = tf::resolve(tf_prefix_, "odom_combined");
00151     odom_trans.child_frame_id = tf::resolve(tf_prefix_, "base_footprint");
00152 
00153     odom_trans.header.stamp = ros::Time::now();
00154     odom_trans.transform.translation.x = z;
00155     odom_trans.transform.translation.y = -x;
00156     odom_trans.transform.translation.z = 0.0;
00157     odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(-theta);
00158 
00159     odom_broadcaster_.sendTransform(odom_trans);
00160   }
00161 
00162   sensor_msgs::JointState joint_state;
00163   joint_state.header.stamp = ros::Time::now();
00164   joint_state.name.resize(6);
00165   joint_state.position.resize(6);
00166   joint_state.name[0] = "left_front_wheel_joint";
00167   joint_state.name[1] = "left_middle_wheel_joint";
00168   joint_state.name[2] = "left_rear_wheel_joint";
00169   joint_state.name[3] = "right_front_wheel_joint";
00170   joint_state.name[4] = "right_middle_wheel_joint";
00171   joint_state.name[5] = "right_rear_wheel_joint";
00172 
00173   static double wheelpos_l = 0;
00174   wheelpos_l += 2.0 * M_PI * wheel_a / ticks_per_turn_of_wheel_;
00175   if (wheelpos_l > M_PI)
00176     wheelpos_l -= 2.0 * M_PI;
00177   if (wheelpos_l < -M_PI)
00178     wheelpos_l += 2.0 * M_PI;
00179 
00180   static double wheelpos_r = 0;
00181   wheelpos_r += 2 * M_PI * wheel_b / ticks_per_turn_of_wheel_;
00182   if (wheelpos_r > M_PI)
00183     wheelpos_r -= 2.0 * M_PI;
00184   if (wheelpos_r < -M_PI)
00185     wheelpos_r += 2.0 * M_PI;
00186 
00187   joint_state.position[0] = joint_state.position[1] = joint_state.position[2] = wheelpos_l;
00188   joint_state.position[3] = joint_state.position[4] = joint_state.position[5] = wheelpos_r;
00189 
00190   // note: we reuse joint_state here, i.e., we modify joint_state after publishing.
00191   // this is only safe as long as nothing in the same process subscribes to the
00192   // joint_states topic. same for imu above.
00193   joint_pub_.publish(joint_state);
00194 }
00195 
00196 void ROSComm::send_sonar_leftBack(int ir_left_back)
00197 {
00198   sensor_msgs::Range range;
00199   range.header.stamp = ros::Time::now();
00200 
00201   range.header.frame_id = tf::resolve(tf_prefix_, "ir_left_back");
00202   range.radiation_type = sensor_msgs::Range::INFRARED;
00203   range.field_of_view = IR_FOV;
00204   range.min_range = IR_MIN;
00205   range.max_range = IR_MAX;
00206   range.range = ir_left_back / 100.0;
00207   range_pub_.publish(range);
00208 }
00209 
00210 void ROSComm::send_sonar_front_usound_leftFront_left(int ir_right_front, int usound, int ir_left_front, int ir_left)
00211 {
00212   sensor_msgs::Range range;
00213   range.header.stamp = ros::Time::now();
00214 
00215   range.header.frame_id = tf::resolve(tf_prefix_, "ir_right_front");
00216   range.radiation_type = sensor_msgs::Range::INFRARED;
00217   range.field_of_view = IR_FOV;
00218   range.min_range = IR_MIN;
00219   range.max_range = IR_MAX;
00220   range.range = ir_right_front / 100.0;
00221   range_pub_.publish(range);
00222 
00223   range.header.frame_id = tf::resolve(tf_prefix_, "ultrasound_front");
00224   range.radiation_type = sensor_msgs::Range::ULTRASOUND;
00225   range.field_of_view = SONAR_FOV;
00226   range.min_range = SONAR_MIN;
00227   range.max_range = SONAR_MAX;
00228   range.range = usound / 100.0;
00229   range_pub_.publish(range);
00230 
00231   range.header.frame_id = tf::resolve(tf_prefix_, "ir_left_front");
00232   range.radiation_type = sensor_msgs::Range::INFRARED;
00233   range.field_of_view = IR_FOV;
00234   range.min_range = IR_MIN;
00235   range.max_range = IR_MAX;
00236   range.range = ir_left_front / 100.0;
00237   range_pub_.publish(range);
00238 
00239   range.header.frame_id = tf::resolve(tf_prefix_, "ir_left");
00240   range.radiation_type = sensor_msgs::Range::INFRARED;
00241   range.field_of_view = IR_FOV;
00242   range.min_range = IR_MIN;
00243   range.max_range = IR_MAX;
00244   range.range = ir_left / 100.0;
00245   range_pub_.publish(range);
00246 }
00247 
00248 void ROSComm::send_sonar_back_rightBack_rightFront(int ir_back, int ir_right_back, int ir_right)
00249 {
00250   sensor_msgs::Range range;
00251   range.header.stamp = ros::Time::now();
00252 
00253   range.header.frame_id = tf::resolve(tf_prefix_, "ir_back");
00254   range.radiation_type = sensor_msgs::Range::INFRARED;
00255   range.field_of_view = IR_FOV;
00256   range.min_range = IR_MIN;
00257   range.max_range = IR_MAX;
00258   range.range = ir_back / 100.0;
00259   range_pub_.publish(range);
00260 
00261   range.header.frame_id = tf::resolve(tf_prefix_, "ir_right_back");
00262   range.radiation_type = sensor_msgs::Range::INFRARED;
00263   range.field_of_view = IR_FOV;
00264   range.min_range = IR_MIN;
00265   range.max_range = IR_MAX;
00266   range.range = ir_right_back / 100.0;
00267   range_pub_.publish(range);
00268 
00269   range.header.frame_id = tf::resolve(tf_prefix_, "ir_right");
00270   range.radiation_type = sensor_msgs::Range::INFRARED;
00271   range.field_of_view = IR_FOV;
00272   range.min_range = IR_MIN;
00273   range.max_range = IR_MAX;
00274   range.range = ir_right / 100.0;
00275   range_pub_.publish(range);
00276 }
00277 
00278 void ROSComm::send_pitch_roll(double pitch, double roll)
00279 {
00280   //TODO
00281 }
00282 
00283 void ROSComm::send_gyro(double theta, double sigma)
00284 {
00285   sensor_msgs::Imu imu;
00286 
00287   // this is intentionally base_link (the location of the imu) and not base_footprint,
00288   // but because they are connected by a fixed link, it doesn't matter
00289   imu.header.frame_id = tf::resolve(tf_prefix_, "base_link");
00290   imu.header.stamp = ros::Time::now();
00291 
00292   imu.angular_velocity_covariance[0] = -1; // no data avilable, see Imu.msg
00293   imu.linear_acceleration_covariance[0] = -1;
00294 
00295   imu.orientation = tf::createQuaternionMsgFromYaw(theta);
00296   imu.orientation_covariance[0] = sigma;
00297   imu.orientation_covariance[4] = sigma;
00298   imu.orientation_covariance[8] = sigma;
00299   imu_pub_.publish(imu);
00300 }
00301 
00302 void ROSComm::send_rotunit(double rot)
00303 {
00304   sensor_msgs::JointState joint_state;
00305   joint_state.header.stamp = ros::Time::now();
00306   joint_state.name.resize(1);
00307   joint_state.position.resize(1);
00308   joint_state.name[0] = "laser_rot_joint";
00309   joint_state.position[0] = rot;
00310 
00311   joint_pub_.publish(joint_state);
00312 }
00313 
00314 class ROSCall
00315 {
00316   public:
00317     ROSCall(Kurt &kurt, double axis_length) :
00318       kurt_(kurt),
00319       axis_length_(axis_length),
00320       v_l_soll_(0.0),
00321       v_r_soll_(0.0),
00322       AntiWindup_(1.0),
00323       last_cmd_vel_time_(0.0) { }
00324     void velCallback(const geometry_msgs::Twist::ConstPtr& msg);
00325     void pidCallback(const ros::TimerEvent& event);
00326     void rotunitCallback(const geometry_msgs::Twist::ConstPtr& msg);
00327 
00328   private:
00329     Kurt &kurt_;
00330     double axis_length_;
00331     double v_l_soll_;
00332     double v_r_soll_;
00333     double AntiWindup_;
00334     ros::Time last_cmd_vel_time_;
00335 };
00336 
00337 void ROSCall::velCallback(const geometry_msgs::Twist::ConstPtr& msg)
00338 {
00339   AntiWindup_ = 1.0;
00340   last_cmd_vel_time_ = ros::Time::now();
00341   v_l_soll_ = msg->linear.x - axis_length_ * msg->angular.z /*/ wheelRadius*/;
00342   v_r_soll_ = msg->linear.x + axis_length_ * msg->angular.z/*/wheelRadius*/;
00343 
00344   if (msg->linear.x == 0 && msg->angular.z == 0)
00345   {
00346     AntiWindup_ = 0.0;
00347   }
00348 }
00349 
00350 void ROSCall::pidCallback(const ros::TimerEvent& event)
00351 {
00352   double v_l_soll = 0.0;
00353   double v_r_soll = 0.0;
00354   double AntiWindup = 1.0;
00355 
00356   if (ros::Time::now() - last_cmd_vel_time_ < ros::Duration(0.6))
00357   {
00358     v_l_soll = v_l_soll_;
00359     v_r_soll = v_r_soll_;
00360     AntiWindup = AntiWindup_;
00361   }
00362 
00363   kurt_.set_wheel_speed(v_l_soll, v_r_soll, AntiWindup);
00364 }
00365 
00366 void ROSCall::rotunitCallback(const geometry_msgs::Twist::ConstPtr& msg)
00367 {
00368     kurt_.can_rotunit_send(msg->angular.z);
00369 }
00370 
00371 int main(int argc, char** argv)
00372 {
00373   ros::init(argc, argv, "kurt_base");
00374   ros::NodeHandle n;
00375   ros::NodeHandle nh_ns("~");
00376 
00377   //Odometry parameter (defaults for kurt2 indoor)
00378   double wheel_perimeter;
00379   nh_ns.param("wheel_perimeter", wheel_perimeter, 0.379);
00380   double axis_length;
00381   nh_ns.param("axis_length", axis_length, 0.28);
00382 
00383   double turning_adaptation;
00384   nh_ns.param("turning_adaptation", turning_adaptation, 0.69);
00385   int ticks_per_turn_of_wheel;
00386   nh_ns.param("ticks_per_turn_of_wheel", ticks_per_turn_of_wheel, 21950);
00387 
00388   double sigma_x, sigma_theta, cov_x_y, cov_x_theta, cov_y_theta;
00389   nh_ns.param("x_stddev", sigma_x, 0.002);
00390   nh_ns.param("rotation_stddev", sigma_theta, 0.017);
00391   nh_ns.param("cov_xy", cov_x_y, 0.0);
00392   nh_ns.param("cov_xrotation", cov_x_theta, 0.0);
00393   nh_ns.param("cov_yrotation", cov_y_theta, 0.0);
00394 
00395   ROSComm roscomm(n, sigma_x, sigma_theta, cov_x_y, cov_x_theta, cov_y_theta, ticks_per_turn_of_wheel);
00396 
00397   Kurt kurt(roscomm, wheel_perimeter, axis_length, turning_adaptation, ticks_per_turn_of_wheel);
00398 
00399   //PID parameter (disables micro controller)
00400   std::string speedPwmLeerlaufTable;
00401   if (nh_ns.getParam("speedtable", speedPwmLeerlaufTable))
00402   {
00403     double feedforward_turn;
00404     nh_ns.param("feedforward_turn", feedforward_turn, 0.35);
00405     double ki, kp;
00406     nh_ns.param("ki", ki, 3.4);
00407     nh_ns.param("kp", kp, 0.4);
00408     if (!kurt.setPWMData(speedPwmLeerlaufTable, feedforward_turn, ki, kp))
00409       return 1;
00410   }
00411 
00412   bool use_rotunit;
00413   nh_ns.param("use_rotunit", use_rotunit, false);
00414   if (use_rotunit) {
00415     double rotunit_speed;
00416     nh_ns.param("rotunit_speed", rotunit_speed, M_PI/6.0);
00417     kurt.can_rotunit_send(rotunit_speed);
00418   }
00419 
00420   bool publish_tf;
00421   nh_ns.param("publish_tf", publish_tf, false);
00422   std::string tf_prefix;
00423   tf_prefix = tf::getPrefixParam(nh_ns);
00424   roscomm.setTFPrefix(tf_prefix);
00425 
00426   ROSCall roscall(kurt, axis_length);
00427 
00428   ros::Timer pid_timer = n.createTimer(ros::Duration(0.01), &ROSCall::pidCallback, &roscall);
00429   ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 10, &ROSCall::velCallback, &roscall);
00430   ros::Subscriber rot_vel_sub;
00431   if (use_rotunit)
00432     rot_vel_sub = n.subscribe("rot_vel", 10, &ROSCall::rotunitCallback, &roscall);
00433 
00434   while (ros::ok())
00435   {
00436     kurt.can_read_fifo();
00437     ros::spinOnce();
00438   }
00439 
00440   return 0;
00441 }