state_tf.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <tf/transform_broadcaster.h>
00032 #include "math.h"
00033 #include "corobot_msgs/PosMsg.h"
00034 #include "nav_msgs/Odometry.h"
00035 #include <dynamic_reconfigure/server.h>
00036 #include <corobot_state_tf/corobot_state_tfConfig.h>
00037 #include "utility.h"
00038 
00039 // Utility object for geometry functions
00040 Utility u;
00041 
00042 // To calculate - distance in meter for one count of the encoders.The Phidget C API gives the number of encoder ticks *4.
00043 double DistancePerCount; 
00044 
00045 // Distance in meters between the left and right wheels, measured from the middle of the wheels
00046 double lengthBetweenTwoWheels; 
00047 
00048 // X, y, and orientation of the robot
00049 double x, y, th; 
00050 
00051 // True if we are waiting for first encoder measurement
00052 bool firstTime = true; 
00053 
00054 // True if 4-W robot
00055 bool is4WheelDrive;
00056 
00057 // If true, publish the transform from odom to base_footprint
00058 bool publish_odom_tf; 
00059 
00060 // True if we are publishing odometry
00061 bool odometry_activated = true;
00062 
00063 
00064 // Variables used to calculate the velocity. 
00065 // The velocity is calculated at time of publishing, 
00066 // every 20ms and position and timestamp need to be saved
00067 double previous_x = 0;
00068 double previous_y = 0;
00069 double previous_th = 0;
00070 
00071 double previous_vx = 0;
00072 double previous_vy = 0;
00073 double previous_vth = 0;
00074 
00075 
00076 // Want to only update x,y,th after a certain amount of PosMsgs are received
00077 corobot_msgs::PosMsg previous;
00078 corobot_msgs::PosMsg current;
00079 
00080 std::vector<float> dists;
00081 
00082 
00083 bool x_startError = false;
00084 bool y_startError = false;
00085 bool th_startError = false;
00086 
00087 
00088 
00089 void errorAdjustment(const double dx, const double dth) {
00090   //std::cout<<"\n\ndx: "<<dx<<" dth: "<<dth;
00091 
00092   // Adjust theta
00093   float th_error = -0.042555431f*dth;
00094   th = u.displaceAngle(th, th_error);
00095   
00096   float tempx = dx*cos(th);
00097   float tempy = dx*sin(th);
00098 
00099   // Calculate x error
00100   float x_error = -0.0181546483f*tempx;// + 0.0008176502;
00101   
00102   // Calculate y error
00103   float y_error = -0.0181546483f*tempy;// + 0.0008176502;
00104 
00105 
00106   // Set the new x,y values
00107   x += (tempx + x_error);
00108   y += (tempy + y_error);
00109 
00110 
00111   // Check if acceleration is higher than some threshold
00112   double v_x = (x - previous_x) / (current.header.stamp.toSec() - previous.header.stamp.toSec());
00113   double v_y = (y - previous_y) / (current.header.stamp.toSec() - previous.header.stamp.toSec());
00114   double v_th = (th - previous_th) / (current.header.stamp.toSec() - previous.header.stamp.toSec());
00115 
00116   double a_x = (v_x - previous_vx) / (current.header.stamp.toSec() - previous.header.stamp.toSec());
00117   double a_y = (v_y - previous_vy) / (current.header.stamp.toSec() - previous.header.stamp.toSec());
00118   double a_th = (v_th - previous_vth) / (current.header.stamp.toSec() - previous.header.stamp.toSec());
00119   //std::cout<<"\n\nv_th: "<<v_th<<" previous_vth: "<<previous_vth<<" a_th: "<<a_th;
00120 
00121   if( a_x > 0.6 && !x_startError && current.header.stamp != previous.header.stamp) {
00122     x += 0.009810897f/2.f;
00123     x_startError = true;
00124   }
00125 
00126   if(a_y > 0.6 && !y_startError && current.header.stamp != previous.header.stamp) {
00127     y += 0.009810897f/2.f;
00128     y_startError = true;
00129   }
00130 
00131   if( (a_th > 0.1 || a_th < -0.1) && !th_startError && current.header.stamp != previous.header.stamp) {
00132     th -= (0.005077337f / 2.f);
00133     th_startError = true;
00134   }
00135   else if(v_th > 0.001) {
00136     th_startError = false;
00137   }
00138 }
00139 
00140 
00142 void setValues() {
00143   
00144   // Set previous time encoder data was measured
00145   ros::Time last_time_encoder = previous.header.stamp;
00146   
00147   // Time corresponding to the encoder measurement
00148   ros::Time current_time_encoder = current.header.stamp; 
00149 
00150   // Get time difference
00151   double delta_time = (current_time_encoder - last_time_encoder).toSec();
00152   
00153   // Check for if initialization is needed
00154   if(firstTime == false) { 
00155 
00156     // Distance made by the left and right wheels
00157     double distance_left = ((double)(current.px - previous.px) * DistancePerCount);                 
00158     double distance_right = ((double)(current.py - previous.py) * DistancePerCount); 
00159 
00160     // Get the overall distance traveled
00161     double dx = (distance_left + distance_right)/2.0;
00162       
00163     // Length of the arc formed by wheel turning
00164     double arc_length = (distance_right - distance_left);
00165 
00166     // Central angle formed by arc is proportionate to arc length
00167     // length / circumference = theta / 2*PI, which is 2PI*l/2PI*r = l/r
00168     double dth = (arc_length)/(lengthBetweenTwoWheels); 
00169 
00170     // Displace th by dth
00171     th = u.displaceAngle(th, dth);
00172 
00173     // Adjust for error
00174     errorAdjustment(dx, dth);
00175   } // end if not first time
00176   // Else, set firstTime=false
00177   else {
00178     firstTime = false;
00179   } // end else
00180 } // End setValues
00181 
00182 
00184 void WheelCallback(const corobot_msgs::PosMsg& pos) {
00185 
00186   current = pos;
00187 
00188 } // End WheelCallback
00189 
00190 
00191 
00192 void dynamic_reconfigureCallback(corobot_state_tf::corobot_state_tfConfig &config, uint32_t level) {
00193                 odometry_activated = config.camera_state_tf;
00194 }
00195 
00196 
00197 
00199 void publish_odometry(ros::Publisher& odom_pub, tf::TransformBroadcaster& odom_broadcaster, tf::TransformBroadcaster& broadcaster) {
00200 
00201   // Set values needed to calculate odometry
00202   setValues();
00203 
00204         // Publish the transform between the base_link and the laser range finder
00205         broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.15, 0, 0)),ros::Time::now(),"base_link", "laser"));
00206 
00207         // All odometry is 6DOF we'll need a quaternion created from yaw
00208         geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
00209         geometry_msgs::Quaternion odom_quat2 = tf::createQuaternionMsgFromYaw(0);
00210 
00211         // First, publish the transform over tf
00212         geometry_msgs::TransformStamped odom_trans;
00213         odom_trans.header.stamp = ros::Time::now();
00214         odom_trans.header.frame_id = "odom";
00215         odom_trans.child_frame_id = "base_footprint";
00216 
00217         odom_trans.transform.translation.x = x;
00218         odom_trans.transform.translation.y = y;
00219         odom_trans.transform.translation.z = 0.0;
00220         odom_trans.transform.rotation = odom_quat;
00221 
00222         geometry_msgs::TransformStamped odom_trans2;
00223         odom_trans2.header.stamp = current.header.stamp;
00224         odom_trans2.header.frame_id = "base_footprint";
00225         odom_trans2.child_frame_id = "base_link";
00226 
00227         odom_trans2.transform.translation.x = 0;
00228         odom_trans2.transform.translation.y = 0;
00229         odom_trans2.transform.translation.z = 0.0;
00230         odom_trans2.transform.rotation = odom_quat2;
00231 
00232 
00233         // Send the transform
00234         if(publish_odom_tf)
00235                 odom_broadcaster.sendTransform(odom_trans);
00236         odom_broadcaster.sendTransform(odom_trans2);
00237 
00238         // Next, publish the odometry message 
00239         nav_msgs::Odometry odom;
00240         odom.header.stamp = current.header.stamp;
00241         odom.header.frame_id = "odom";
00242 
00243         // Set the position
00244         odom.pose.pose.position.x = x;
00245         odom.pose.pose.position.y = y;
00246         odom.pose.pose.position.z = 0.0;
00247         odom.pose.pose.orientation = odom_quat;
00248         
00249   // These covariance values are "random". Some better values could be found after experiments and calculations
00250         odom.pose.covariance[0] = 0.01;
00251         odom.pose.covariance[7] = 0.01;
00252         odom.pose.covariance[14] = 10000;
00253         odom.pose.covariance[21] = 10000;
00254         odom.pose.covariance[28] = 10000;
00255         odom.pose.covariance[35] = 0.1;
00256 
00257 
00260   odom.child_frame_id = "base_link";
00261 
00262   // If robot is moving
00263   if (firstTime == false && current.header.stamp != previous.header.stamp) {
00264   
00265   
00266     // Velocity values are current - previous / time_difference
00267     // X
00268     odom.twist.twist.linear.x = (x - previous_x)/((current.header.stamp - previous.header.stamp).toSec());
00269 
00270     // Y
00271     odom.twist.twist.linear.y = (y - previous_y)/((current.header.stamp - previous.header.stamp).toSec());
00272 
00273     // Theta
00274     odom.twist.twist.angular.z = (u.findDistanceBetweenAngles(th, previous_th)) / ((current.header.stamp - previous.header.stamp).toSec());
00275 
00276     // Set the previous values
00277     previous_x = x;
00278     previous_y = y;
00279     previous_th = th;
00280     previous.header.stamp = current.header.stamp;
00281 
00282     previous_vx = odom.twist.twist.linear.x;
00283     previous_vy = odom.twist.twist.linear.y;
00284     previous_vth = odom.twist.twist.angular.z;
00285   }
00286 
00287   // Else, set velocities to zero
00288   else {
00289     odom.twist.twist.linear.x = 0;
00290     odom.twist.twist.linear.y = 0;
00291     odom.twist.twist.angular.z = 0;
00292     previous.header.stamp = ros::Time::now();
00293 
00294     previous_vx = 0;
00295     previous_vy = 0;
00296     previous_vth = 0;
00297     x_startError = false;
00298     y_startError = false;
00299     th_startError = false;
00300   }
00301         
00302   // These covariance values are "random". Some better values could be found after experiments and calculations
00303   odom.twist.covariance[0] = 0.01;
00304   odom.twist.covariance[7] = 0.01;
00305   odom.twist.covariance[14] = 10000;
00306   odom.twist.covariance[21] = 10000;
00307         odom.twist.covariance[28] = 10000;
00308         odom.twist.covariance[35] = 0.1;
00309 
00310         // Publish the message
00311         odom_pub.publish(odom);
00312 
00313   // Set previous
00314   previous = current;
00315 } //End publish_odometry
00316 
00317 
00318 int main(int argc, char** argv) {
00319   int ticks_meter; 
00320 
00321   ros::init(argc, argv, "corobot_state_tf");
00322   ros::NodeHandle n;
00323   ros::NodeHandle nh("~");
00324  
00325 
00326   // True if we have four wheel robot
00327   nh.param("FourWheelDrive", is4WheelDrive, false);
00328   
00329   // The length between the left and right wheel
00330   nh.param("base_width", lengthBetweenTwoWheels, 0.254); 
00331 
00332   // Number of encoder ticks per metet
00333   nh.param("ticks_meter", ticks_meter, 9400); 
00334 
00335   // True if we are publishing odometry and tf
00336   nh.param("publish_odom_tf", publish_odom_tf, true);
00337 
00338   dynamic_reconfigure::Server<corobot_state_tf::corobot_state_tfConfig> server;
00339   dynamic_reconfigure::Server<corobot_state_tf::corobot_state_tfConfig>::CallbackType f;
00340 
00341   f = boost::bind(&dynamic_reconfigureCallback, _1, _2);
00342   server.setCallback(f);
00343 
00344   //Set up the distance per encoder ticks and the length between two wheels variable
00345   DistancePerCount = 1.0/(float)ticks_meter;
00346 
00347   // If 4 wheels, change the distance between the wheels
00348   // This is to compensate the fact that we don't have a differential drive but a skid system
00349   if(is4WheelDrive) {
00350     lengthBetweenTwoWheels *= 1.5;   
00351   }
00352 
00353   //Setup the subscriber and publisher of topics
00354   ros::Subscriber sub = n.subscribe("position_data", 1000, WheelCallback);
00355   ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odometry", 50);   
00356   tf::TransformBroadcaster odom_broadcaster;
00357 
00358   // Rate at which to check for encoder updates when odometry is activated
00359   ros::Rate r(10); 
00360 
00361   // Rate at which to check for encoder updates when odometry is deactivated by dynamic_reconfigure
00362   ros::Rate r_deactivated(2); 
00363 
00364   tf::TransformBroadcaster broadcaster;
00365 
00366 
00367   for(unsigned int i=0;i<3;i++) {
00368     dists.push_back(0);
00369   }
00370 
00371   while(n.ok()) {
00372     ros::spinOnce();
00373     if (odometry_activated) {
00374       publish_odometry(odom_pub, odom_broadcaster, broadcaster);
00375       r.sleep();
00376     }
00377     else
00378         r_deactivated.sleep();
00379   } //end while
00380 
00381   return 0;
00382 } 


corobot_state_tf
Author(s):
autogenerated on Sun Oct 5 2014 23:17:54