0
$\begingroup$

Rosanswers logo

Updated my odometry: new code follows: Still robot not following trajectory. Not sure why map is following base_link? odom stays fixed.

image description

#include <string>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <control_msgs/JointControllerState.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/JointState.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>


using namespace sensor_msgs;
using namespace message_filters;

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;
 
     // set starting point
     theta = msg.pose.pose.orientation.z;
     x = msg.pose.pose.position.x;
     y = msg.pose.pose.position.y;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{

// based on: http://rossum.sourceforge.net/papers/DiffSteer/

     double wheel_track = .21;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;   // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   
     
     if (msg1->set_point == 0) j1 =0;
     if (msg2->set_point == 0) j2 =0;

     double  dt = msg1->time_step;                        // elapsed time in seconds
     float   dx     =   ( ((j1 + j2)*dt ) / 2.0);         // distance in meters
     float   dtheta =   ( ((j1 - j2)*dt ) / wheel_track);  // rotation
                                         
     if (abs(dtheta)>.001) 
     {
        x = x + (dx/dtheta)*(dx*sin(theta + dtheta)-sin(theta));                                    
        y = y - (dx/dtheta)*(dx*cos(theta + dtheta)-cos(theta));  
        theta = theta + dtheta;                                   
        theta = fmodf(theta + M_PI, 2*M_PI) - M_PI ; 
     } 
     else // straight line dtheta == 0
     {
        x = x - dx*cos(theta);
        y = y + dx*sin(theta);
     }

 

//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = y;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


     ROS_INFO("pv1=[%f] pv2=[%f]",msg1->process_value, msg2->process_value);
     ROS_INFO("er1=[%f] er2=[%f] ",msg1->error, msg2->error);
     ROS_INFO("sp1=[%f] sp2=[%f] ",msg1->set_point, msg2->set_point);
     ROS_INFO("dx =[%f] z=[%f]  w=[%f] theta=[%f]", dx, quaternion.z, quaternion.w, theta);
     ROS_INFO("x =[%e] ,y=[%e]", x, y);
//**************************************************************


     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = y;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);              // only seems to confuse orientation 

}

private:
 
   double x,y;
   float theta;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



int main(int argc, char **argv)
{ 
  ros::init(argc, argv, "robot_odometry");

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

Update: the velocity commands are calculated from /cmd_vel from move base. For some reason they are negative as it thinks the robot is facing the wrong way. If it wanted to turn around it would rotate , yes.

I am trying to write a odometry node for a two wheel car running in gazebo. The robot keeps running backward as you can see from the png file i've attached. It moves away from the odom frame and at the same time the map frame moves in the oppiste direction (correct direction and the correct forward scanner direction).

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;
 
     // set starting point
     theta = msg.pose.pose.orientation.z;
     x =  msg.pose.pose.position.x;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{
     double wheel_track = .26;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;     // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     double  dt = (msg1->header.stamp - timeold).toSec();      // elapsed time in seconds
     timeold = msg1->header.stamp;  
     
     float   dx     =   ( ((j1 + j2)) / 2.0) * dt;             // distance in meters
     float   dtheta =   ( ((j1 - j2)) / wheel_track) * dt;     // rotation???????????????????????????????????

                                          
     theta = theta + dtheta;                                   // theta and x set on initialpose
     theta = fmodf(theta + M_PI, 2*M_PI) - M_PI;               // adj to range 
     x = x + dx;                                               // adj distance to angle


//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = -cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = 0;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


//**************************************************************

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg1->header.stamp;

     odometry.pose.pose.position.x  = x;
     odometry.pose.pose.position.y  = 0;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = dx/dt;       // convet back to velocity
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = dtheta/dt;   // convet back to velocity

     pubOdometry.publish(odometry);             

}

private:
 
   double x;
   float theta;
   ros::Time timeold;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   ros::Subscriber subInitialPose;


   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j1_sub;
   boost::shared_ptr<Subscriber<control_msgs::JointControllerState> > j2_sub;
   boost::shared_ptr<TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState> > sync;


};  // Enodof Class



int main(int argc, char **argv)
{ 
  ros::init(argc, argv, "robot_odometry");

  // publish static latch transformations 

  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1.0;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "camera_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.2;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "camera_link";   
  msg.transform.translation.x = 0;
  msg.transform.translation.y = 0;
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);

  PublishOdometry sp;

  ros::spin();

  return 0;
}

image description

Originally posted by rnunziata on ROS Answers with karma: 713 on 2013-10-07

Post score: 0

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

This problem as been reworked and I am now posting it under a new question. I know this is not the best way to do this but there is so much supporting data the problem text gets too long. Rather then replace it I am opening a new question. Hope that is ok.

http://answers.ros.org/question/90316/having-problems-with-my-odometry-code/

Originally posted by rnunziata with karma: 713 on 2013-10-13

This answer was ACCEPTED on the original site

Post score: 0

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.