Ask Your Question
0

Having problems with my odometry code [closed]

asked 2013-10-13 10:49:12 -0500

rnunziata gravatar image

updated 2013-10-15 05:48:31 -0500

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 ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by rnunziata
close date 2013-10-20 10:42:15

2 Answers

Sort by ยป oldest newest most voted
0

answered 2013-10-20 10:40:56 -0500

rnunziata gravatar image

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.

edit flag offensive delete link more
-1

answered 2013-10-13 20:48:45 -0500

dornhege gravatar image
  1. What exactly is wrong, i.e. what would you expect differently? When you look at rviz, what are you displaying - /odom -> /base_link? In that case, if the robot moves backwards in gazebo/real world and does the same in rviz based on the odometry, then everything is fine!

  2. You mentioned move_base. I assume you use that for control. If you are unsure about odometry or other basics, I'd highly recommend not using move_base. Use a joystick/keyboard control to manually send out cmd_vel messages, so you don't debug two things at once.

  3. Make the position outputs fixed instead of scientific notation. Besides that they look OK in principle. Maybe just a wrong scaling factor. Linear.x is negative and the x coord gets more negative. That looks alright.

edit flag offensive delete link more

Comments

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.

rnunziata gravatar imagernunziata ( 2013-10-14 01:57:42 -0500 )edit

Indeed, if everything looks ok in principle then why does the robot not follow the navigation path. How can I look at move_base to see what it thinks it is doing. The model is running in Gazebo is there a joystick / keyboard control for gazebo? In either case I want to use the move base for object avoidance and mapping.

rnunziata gravatar imagernunziata ( 2013-10-14 02:06:42 -0500 )edit

Negative velocity will move along the negative x-axis (relative to the robot!).

dornhege gravatar imagedornhege ( 2013-10-14 03:42:18 -0500 )edit

In any case you should debug things independently. First verify odometry is correct without move_base, afterwards turn to move_base. For velocity control you can use any of the generic teleoperation packages. They only send out /cmd_vel, which is independent of gazebo/reality.

dornhege gravatar imagedornhege ( 2013-10-14 03:43:32 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2013-10-13 10:49:12 -0500

Seen: 715 times

Last updated: Oct 20 '13