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
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:
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].
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
Comments