Ask Your Question

Writing a cmd_vel node with amcl_pose to move robot

asked 2019-06-13 15:07:41 -0600

Usui gravatar image

updated 2019-06-14 23:47:26 -0600

jayess gravatar image

So I am writing a cpp code to make the robot move. This code basically depends on the amcl_pose topic so I can calculate the cmd_vel angular and linear and also when to stop whenever I am at the goal. Basically, what I think is happening is that the robot that I am using keeps trying to reach the goal because it is circling around it, but never stops because the amcl_pose's number never reached the goal's (x,y). I am thinking that my localizing system is not accurate, how should I make it more accurate? And if I am wrong, what's the problem. Any help is appreciated thanks!

Here's my code:

#include <ros/ros.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/Joy.h>
#include <std_msgs/Float32.h>
#include <cmath>
#include <cstdlib>
#include <std_msgs/String.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

float current_x, current_y, current_theta;
int in = 0;
#define M_PI 3.14159265358979323846 /* pi */

void poseCallBack(const nav_msgs::Odometry::ConstPtr & msg) {
  current_x = msg - > pose.pose.position.x;
  current_y = msg - > pose.pose.position.y;
  current_theta = tf::getYaw(msg - > pose.pose.orientation);
  in = 1;

int main(int argc, char ** argv) {
  ros::init(argc, argv, "goal");
  ros::NodeHandle nh_;
  ros::Subscriber odom_sub = nh_.subscribe("/odom", 100, & poseCallBack);
  ros::Publisher cmd_vel_pub = nh_.advertise < geometry_msgs::Twist > ("/cmd_vel", 100);
  geometry_msgs::Twist cmd;
  double distance;
  ros::Rate loop_rate(100);
  geometry_msgs::Point goal;
  goal.x = -0.4266;
  goal.y = -0.7747;

  while (ros::ok()) {
    if ( in > 0) {
      double x = goal.x - current_x;
      double y = goal.y - current_y;
      distance = sqrt(pow(y, 2) + pow(x, 2)); // distance formula
      double angleToGoal = atan2(y, x); // angle
      ROS_INFO("Current angle: %f, Angle to goal: %f", current_theta * 180 / M_PI, angleToGoal * 180 / M_PI);

      if (distance < 0.05) // if goal is reach
        cmd.linear.x = 0.0;

      } else {
        if (abs(angleToGoal - current_theta) > 0.1) // if angle isn't lined up
          cmd.angular.z = 0.1;
        // once it is lined up, it will go straight the next time it loop
        else {
          cmd.angular.z = 0.0;
          cmd.linear.x = 0.1; // linear speed              

  return 0;

[[attachment: a file with]]

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2019-06-13 19:00:05 -0600

Geoff gravatar image

You can never exactly reach the goal, except by some great stroke of luck. You need to use a delta so that when your robot gets sufficiently close to the goal, it accepts that as the same as having reached the goal and stops. How large that delta will be (1 cm, 10 cm, 1 m, etc.) will depend on your application (how accurate you need to be) and your robot (how accurately it can control its position.)

Decide how big to make your delta, then where you calculate if the goal has been reached, instead of checking if x and y match the goal, calculate the current distance from the goal and check if it is less than the delta.

edit flag offensive delete link more


I tried that with 0.09 error range but the main problem is for some reason, the amcl just messes up hard every time the robot turns. Is it because amcl needs move_base? Also I had to move the robot a lot before the amcl_pose changes

Usui gravatar image Usui  ( 2019-06-13 20:57:35 -0600 )edit

It would help to see what your robot is actually doing. Provide a bag file so we can replay it and see what's happening.

Geoff gravatar image Geoff  ( 2019-06-14 03:08:45 -0600 )edit

for /odom? or amcl_pose?

Usui gravatar image Usui  ( 2019-06-14 16:18:04 -0600 )edit

@Geoff Here's the rosbag:

basically it kept going so I had to stop it from running into a wall so rosbag might be a little short. also updated some changes to my code

Usui gravatar image Usui  ( 2019-06-14 21:25:58 -0600 )edit

Your bag file does not include any sensor data that I can see. How are you localising the robot? We need to see everything. Your bag file also includes just a few seconds of activity followed by 1000 seconds of nothing. Please provide a bag file that does not have unnecessary time so we can help you faster. You should also provide an rviz configuration file that we can use to see exactly what you want us to see to understand your problem, rather than having to guess at relevant topics to vizualise ourselves.

Geoff gravatar image Geoff  ( 2019-06-19 20:24:14 -0600 )edit

Okay so I am able to have it move counterclockwise to move for everything. I have 2 question:

  1. I can't think of a way to get it to know if spinning clockwise or counter is more time efficiency because the range is [-180, 180].

  2. for this problem, I am wondering since I used some black stuff, does the black absorb the laser scanner making it harder to localize?

Usui gravatar image Usui  ( 2019-06-19 21:30:08 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2019-06-13 15:07:41 -0600

Seen: 525 times

Last updated: Jun 14 '19