Robotics StackExchange | Archived questions

Writing a cmd_vel node with amcl_pose to move robot

So I am writing a cpp code to make the robot move. This code basically depends on the amclpose topic so I can calculate the cmdvel 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              
        }
      }

      cmd_vel_pub.publish(cmd);
      loop_rate.sleep();
    }
    ros::spinOnce();
  }
  return 0;
}

[[attachment: a file with 2019-06-14-19-00-34.bag.active]]

Asked by Usui on 2019-06-13 15:07:41 UTC

Comments

Answers

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.

Asked by Geoff on 2019-06-13 19:00:05 UTC

Comments

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

Asked by Usui on 2019-06-13 20:57:35 UTC

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.

Asked by Geoff on 2019-06-14 03:08:45 UTC

for /odom? or amcl_pose?

Asked by Usui on 2019-06-14 16:18:04 UTC

@Geoff Here's the rosbag: https://github.com/SKT-Peanut/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

Asked by Usui on 2019-06-14 21:25:58 UTC

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.

Asked by Geoff on 2019-06-19 20:24:14 UTC

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. http://answers.ros.org/question/326213/amcl-localizing-offset-problem/ for this problem, I am wondering since I used some black stuff, does the black absorb the laser scanner making it harder to localize?

Asked by Usui on 2019-06-19 21:30:08 UTC