Having problems with my odometry code [closed]

edit

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.

#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 ...