0
$\begingroup$

Rosanswers logo

Update: Added retrieve pose from gazebo on setting initial pose in rviz.

I am having two problems with this code if someone has the time to look at it I would greatly appreciate their time.I tried to document the code as clearly as possible so it would be easy to follow.

First: The robot in both gazebo and rviz run backwards. This is based on the values return by move_base as can be seen in the code for cmd_vel. It is my understanding that a negative value should cause the robot to move forward and that move_base navigation is not trying to move away from the goal. The setting of fixed frame does not affect the direction of the robot. Why is move base sending negative values if it expects the robot to move forward. If move_base thought the goal was behind the robot then why not rotate?

Second: The robot moves in a straight line regardless of the trajectory path. As if it things the goal is directly in front of it at all times. The initial pose I set is 45 degrees off the goal line. There is no attempt to rotate toward the goal. At some point it stops moving and starts rotating in a circle. I believe after it thinks it has reached the goal.

Below is a pic the of rviz image and the output from the ROS_INFO in the code showing the values as given by both move_base and ros-gazebo plugin.

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 <std_msgs/Float64.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/JointState.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <control_msgs/JointControllerState.h>
#include <gazebo_msgs/GetModelState.h>
#include <gazebo_msgs/ModelState.h>


using namespace sensor_msgs;
using namespace message_filters;
const double wheel_track = .40;  // distance between two wheels in meters  (1-base) sub meters
const double wheel_diameter = .2; 


//*************************************************************
//*****  Class: process initial pose
//*****         process cmd_vel topic (twist msgs) from move_base
//*************************************************************
class PublishOdometry
{
public:
  PublishOdometry()
  {
     // Listen and wait for initial pose from rviz navigation panel
     // Nothing hanppens until we recieve the initialpose message
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     // Sends static latch transforms and initial odometry on recipt of intial pose
     // Sets up sequence handeler for left and right wheels cmd_vel msgs

     // advertise topics used in handelerJointControllerState
     pubOdometry    = n.advertise<nav_msgs::Odometry>("/odom", 10);
     pubJointState  = n.advertise<sensor_msgs::JointState>("/joint_states", 10);


     // take pose from gazebo: ignore rviz pose: Should override initialpose topic and re-issue it here.
     ros::ServiceClient client = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
     gazebo_msgs::GetModelState getmodelstate;
     getmodelstate.request.model_name ="rrbot";
     client.call(getmodelstate);


     // set starting data x,y and theta for odometry used in handelerJointControllerState
     theta = getmodelstate.response.pose.orientation.z;
     x     = getmodelstate.response.pose.position.x; 
     y     = getmodelstate.response.pose.position.y;
 

     // set up data for transform and odometry
     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 = getmodelstate.response.pose.position.x;
     odom_trans.transform.translation.y = getmodelstate.response.pose.position.y;
     odom_trans.transform.translation.z = getmodelstate.response.pose.position.z;
     odom_trans.transform.rotation      = getmodelstate.response.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.pose = getmodelstate.response.pose;
 

     // publish initial pose
     odom_broadcaster.sendTransform(odom_trans);  // publish initial odom->base_link transform
     pubOdometry.publish(odometry);               // publish initial pose odom->base_link 
     publishLatchTransforms();                    // publish static latch transforms for fixed links

     // Setup sync'd joint state callbacks : topics published by ros-gazebo-plugin
     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::handelerJointControllerState, this, _1, _2));
}

void publishLatchTransforms()
{
  // Send static link transforms
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.transform.translation.x = 0.05;
  msg.transform.translation.y = 0;
  msg.transform.rotation.x =  0.0;
  msg.transform.rotation.y =  0.0;
  msg.transform.rotation.z =  0.0;
  msg.transform.rotation.w =  1;

  msg.header.frame_id = "base_link";   
  msg.transform.translation.z = 0.1;
  msg.child_frame_id = "tower_link";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "tower_link";   
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "hokuyo_frame";
  static_broadcaster.sendTransform(msg);

  msg.header.frame_id = "tower_link";   
  msg.transform.translation.z = 0.3;
  msg.child_frame_id = "camera_frame";
  static_broadcaster.sendTransform(msg);
}


void handelerJointControllerState(const control_msgs::JointControllerStateConstPtr& msg1,
                                  const control_msgs::JointControllerStateConstPtr& msg2)
{
     // Called when both a right and left joint cmd velocity msg is avaialbe from ros-gazebo-plugin 
     // Based on Dead Reckoning in : http://rossum.sourceforge.net/papers/DiffSteer/
               
     double  dt = msg1->time_step;             // elapsed time in seconds
     float   j1 = msg1->process_value*dt;      // calc distance using velocity in meters/sec
     float   j2 = msg2->process_value*dt;  
     
     if (msg1->set_point == 0) j1 = 0.0;       // ignore none commands
     if (msg2->set_point == 0) j2 = 0.0;

     float distance  =   ( (j1 + j2) / 2.0);          // distance in meters
     float dtheta    =   ( (j1 - j2) / wheel_track);  // rotation                                                
     float dx = distance*cos(theta);
     float dy = distance*sin(theta);     

     theta = theta + dtheta; 
     x = x + dx;
     y = y + dy; 

    //***** publish base_link->odom transform

     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 = theta;
     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("ts1=[%f] ts2=[%f] ",msg1->time_step, msg2->time_step);
     ROS_INFO("x =[%f], y  =[%f] ",x, y);

    //***** publish base_link->odom odometry

     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;       // convert back to velocities
     odometry.twist.twist.linear.y  = dy/dt; 
     odometry.twist.twist.angular.z = dtheta/dt;   

     pubOdometry.publish(odometry);              

     //***** send joint states to robot_state_publisher
     sendJointStates(msg1,msg2);
}



void sendJointStates(const control_msgs::JointControllerStateConstPtr& msg1, 
                     const control_msgs::JointControllerStateConstPtr& msg2)
{
  // Send joint states to robot_state_publisher
  sensor_msgs::JointState jointStateMsg;

  jointStateMsg.header.stamp = msg1->header.stamp;
  jointStateMsg.name.resize(2);
  jointStateMsg.position.resize(2);

  jointStateMsg.name[0] = "Joint1";
  jointStateMsg.position[0] = msg1->process_value;

  jointStateMsg.name[1] = "Joint2";
  jointStateMsg.position[1] = msg2->process_value;

  pubJointState.publish(jointStateMsg);
}

private:
 
   float x,y;
   float theta;

   ros::NodeHandle n;
   ros::Publisher pubOdometry;
   ros::Publisher pubJointState;
   tf2_ros::TransformBroadcaster odom_broadcaster;
   tf2_ros::StaticTransformBroadcaster static_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;

};  //************** PublishOdometry endof Class




//*************************************************************
//*****  Class: process cmd_vel  (twist messages)  and send as joint cmd vel to wheels
//*****  Listen and wait on cmd_vel topic from move_base navigation
//*****  move_base will send cmd_vel (twist messages) after recieving at set goal. 
//*************************************************************
class PublishCmdVel
{
public:
  PublishCmdVel()
  {  
     // Advertize joint vel command topic to ros-gazebo-plugin.
     pubRightVel = n.advertise<std_msgs::Float64>("/rrbot/joint1_position_controller/command", 10);
     pubLeftVel  = n.advertise<std_msgs::Float64>("/rrbot/joint2_position_controller/command", 10);

     // Subscribe to cmd_vel published by move_base.
     // This topic is not published until a set goal is sent to move_base 
     subTwist    = n.subscribe("/cmd_vel", 10, &PublishCmdVel::handelerTwist,this);
  } 



void handelerTwist(const geometry_msgs::Twist msg)
{ 
   // Process twist msg : send right and left cmd velocity msg to joints.
   std_msgs::Float64 cmdVel1;
   std_msgs::Float64 cmdVel2;  

   ROS_INFO("liner.x[%f]  angular.z[%f]",msg.linear.x , msg.angular.z);
     
   float vel1 =  (msg.linear.x - msg.angular.z * (wheel_track/2) )*(2/wheel_diameter);
   float vel2 =  (msg.linear.x + msg.angular.z * (wheel_track/2) )*(2/wheel_diameter);

   cmdVel1.data = vel1; 
   cmdVel2.data = vel2;
   pubLeftVel.publish(cmdVel1);
   pubRightVel.publish(cmdVel2);
} 

private:
   ros::NodeHandle n;

   ros::Publisher pubRightVel;
   ros::Publisher pubLeftVel;
   ros::Subscriber subTwist;


};  //**************** PublishCmdVel Endof Class


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

  PublishCmdVel pc;   // Move base naviagtion to gazebo robot
  PublishOdometry po; // Gazebo robot to move base navigation

  ros::spin();

  return 0;
}

Data::********************************************************

[ INFO] [1381690469.603390691, 12.264000000]: pv1=[-1.001473] pv2=[-1.001638]
[ INFO] [1381690469.603450491, 12.264000000]: er1=[0.001473] er2=[0.001638] 
[ INFO] [1381690469.603484294, 12.264000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690469.603515735, 12.264000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690469.603546256, 12.264000000]: x =[-4.965168e-01], y =[-6.101050e-03]
[ INFO] [1381690469.837938544, 12.287000000]: liner.x[-0.100000]  angular.z[0.000000]

[ INFO] [1381690470.599817740, 12.364000000]: pv1=[-1.001474] pv2=[-1.001633]
[ INFO] [1381690470.599885755, 12.364000000]: er1=[0.001474] er2=[0.001633] 
[ INFO] [1381690470.599920818, 12.364000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690470.599956022, 12.364000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690470.599990882, 12.364000000]: x =[-5.065324e-01], y =[-6.083413e-03]
[ INFO] [1381690470.988638996, 12.402000000]: liner.x[-0.100000]  angular.z[0.000000]

[ INFO] [1381690471.601222457, 12.464000000]: pv1=[-1.001474] pv2=[-1.001627]
[ INFO] [1381690471.601291815, 12.464000000]: er1=[0.001474] er2=[0.001627] 
[ INFO] [1381690471.601326997, 12.464000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690471.601364980, 12.464000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690471.601400043, 12.464000000]: x =[-5.165478e-01], y =[-6.065815e-03]
[ INFO] [1381690472.115598898, 12.515000000]: liner.x[-0.100000]  angular.z[0.000000]

[ INFO] [1381690472.602594895, 12.564000000]: pv1=[-1.001472] pv2=[-1.001621]
[ INFO] [1381690472.602675567, 12.564000000]: er1=[0.001472] er2=[0.001621] 
[ INFO] [1381690472.602770657, 12.564000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690472.602852917, 12.564000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690472.602937731, 12.564000000]: x =[-5.265633e-01], y =[-6.048255e-03]
[ INFO] [1381690473.296609439, 12.632000000]: liner.x[-0.100000]  angular.z[0.000000]

[ INFO] [1381690473.606637420, 12.664000000]: pv1=[-1.001471] pv2=[-1.001615]
[ INFO] [1381690473.606697236, 12.664000000]: er1=[0.001471] er2=[0.001615] 
[ INFO] [1381690473.606728971, 12.664000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690473.606763009, 12.664000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690473.606794329, 12.664000000]: x =[-5.365787e-01], y =[-6.030733e-03]
[ INFO] [1381690474.548521253, 12.758000000]: liner.x[-0.100000]  angular.z[0.000000]

[ INFO] [1381690474.606357784, 12.764000000]: pv1=[-1.001468] pv2=[-1.001610]
[ INFO] [1381690474.606425920, 12.764000000]: er1=[0.001468] er2=[0.001610] 
[ INFO] [1381690474.606460867, 12.764000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690474.606482509, 12.764000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690474.606499956, 12.764000000]: x =[-5.465941e-01], y =[-6.013248e-03]

[ INFO] [1381690475.604766829, 12.864000000]: pv1=[-0.995905] pv2=[-0.995618]
[ INFO] [1381690475.604821548, 12.864000000]: er1=[-0.004095] er2=[-0.004382] 
[ INFO] [1381690475.604851440, 12.864000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690475.604878770, 12.864000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690475.604909148, 12.864000000]: x =[-5.565517e-01], y =[-5.995898e-03]
[ INFO] [1381690475.726373970, 12.875000000]: liner.x[-0.100000]  angular.z[0.000000]

[ INFO] [1381690476.605963143, 12.964000000]: pv1=[-0.993403] pv2=[-0.993588]
[ INFO] [1381690476.606026469, 12.964000000]: er1=[-0.006597] er2=[-0.006412] 
[ INFO] [1381690476.606058612, 12.964000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690476.606092447, 12.964000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690476.606123914, 12.964000000]: x =[-5.664866e-01], y =[-5.978517e-03]

[ INFO] [1381690476.928189215, 12.996000000]: liner.x[-0.100000]  angular.z[0.000000]
[ INFO] [1381690477.603723783, 13.064000000]: pv1=[-0.996015] pv2=[-0.995133]
[ INFO] [1381690477.603789266, 13.064000000]: er1=[-0.003985] er2=[-0.004867] 
[ INFO] [1381690477.603826042, 13.064000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690477.603859148, 13.064000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690477.603893081, 13.064000000]: x =[-5.764423e-01], y =[-5.961146e-03]

[ INFO] [1381690478.098446977, 13.113000000]: liner.x[-0.100000]  angular.z[0.000000]
[ INFO] [1381690478.616513187, 13.164000000]: pv1=[-1.000925] pv2=[-1.000402]
[ INFO] [1381690478.616573683, 13.164000000]: er1=[0.000925] er2=[0.000402] 
[ INFO] [1381690478.616609597, 13.164000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690478.616644218, 13.164000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690478.616674062, 13.164000000]: x =[-5.864490e-01], y =[-5.943465e-03]

[ INFO] [1381690479.265940182, 13.229000000]: liner.x[-0.100000]  angular.z[0.000000]
[ INFO] [1381690479.613516892, 13.264000000]: pv1=[-1.001179] pv2=[-1.000929]
[ INFO] [1381690479.613612931, 13.264000000]: er1=[0.001179] er2=[0.000929] 
[ INFO] [1381690479.613667930, 13.264000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690479.613711488, 13.264000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690479.613757163, 13.264000000]: x =[-5.964595e-01], y =[-5.925646e-03]
[ INFO] [1381690480.484175752, 13.350000000]: liner.x[-0.100000]  angular.z[0.000000]


[ INFO] [1381690480.617057104, 13.364000000]: pv1=[-1.001615] pv2=[-1.001495]
[ INFO] [1381690480.617127616, 13.364000000]: er1=[0.001615] er2=[0.001495] 
[ INFO] [1381690480.617167269, 13.364000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690480.617202100, 13.364000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690480.617237475, 13.364000000]: x =[-6.064750e-01], y =[-5.907756e-03]


[ INFO] [1381690481.609987358, 13.464000000]: pv1=[-1.001427] pv2=[-1.001442]
[ INFO] [1381690481.610059319, 13.464000000]: er1=[0.001427] er2=[0.001442] 
[ INFO] [1381690481.610099430, 13.464000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690481.610133888, 13.464000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690481.610167974, 13.464000000]: x =[-6.164894e-01], y =[-5.889838e-03]
[ INFO] [1381690481.720369313, 13.474000000]: liner.x[-0.100000]  angular.z[0.000000]

[ INFO] [1381690482.612124481, 13.564000000]: pv1=[-1.001405] pv2=[-1.001476]
[ INFO] [1381690482.612259451, 13.564000000]: er1=[0.001405] er2=[0.001476] 
[ INFO] [1381690482.612299657, 13.564000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690482.612339847, 13.564000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690482.612377116, 13.564000000]: x =[-6.265037e-01], y =[-5.871924e-03]
[ INFO] [1381690482.962601634, 13.599000000]: liner.x[-0.100000]  angular.z[0.000000]

[ INFO] [1381690483.614193993, 13.664000000]: pv1=[-1.001387] pv2=[-1.001493]
[ INFO] [1381690483.614296509, 13.664000000]: er1=[0.001387] er2=[0.001493] 
[ INFO] [1381690483.614364448, 13.664000000]: sp1=[-1.000000] sp2=[-1.000000] 
[ INFO] [1381690483.614438756, 13.664000000]: ts1 =[0.010000] ts2 =[0.010000]
[ INFO] [1381690483.614485431, 13.664000000]: x =[-6.365181e-01], y =[-5.854027e-03]

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

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Changing to ros::Time:now instead of message header time fixed a few problems. Also removed joint states to robot state publisher and issue my own static transforms for fixed rotation wheels. Which is less costly and robot state publisher never actually published correct transforms for the wheels. Also corrected dx dy calculation.


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

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

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