Ask Your Question
0

Send goal code

asked 2019-06-10 12:20:50 -0600

Usui gravatar image

updated 2019-06-12 16:06:10 -0600

So I am writing a code that sends goal to robot then send another goal but it is not working

Here is my code:

    #include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_datatypes.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <math.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

 float current_x,current_y; // current pose
 int i = 0;
void poseCallBack(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg){
current_x = msg->pose.pose.position.x;
current_y = msg->pose.pose.position.y;
 int i = 1;
}
    int main(int argc, char** argv){
 ros::init(argc, argv, "simple_navigation_goals");
 ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/amcl_pose", 1, poseCallBack); // subscribe to amcl_pose/current pose

 //tell the action client that we want to spin a thread by default
 MoveBaseClient ac("move_base", true);

 //wait for the action server to come up
 while(!ac.waitForServer(ros::Duration(5.0))){
   ROS_INFO("Waiting for the move_base action server to come up");
 }

 move_base_msgs::MoveBaseGoal goal;

//we'll send a goal to the robot to move 1 meter forward
 goal.target_pose.header.frame_id = "base_link";
if (i == 1)
{
 goal.target_pose.header.stamp = ros::Time::now();

 goal.target_pose.pose.position.x = 0.01;

 /*calculate angle*/
 double x = 0.01 - current_x;
 double y = 0 - current_y;
 double theta = atan2(y,x);
 // convert angle to quarternion
 tf::Quaternion quaternion;
 quaternion = tf::createQuaternionFromYaw(theta);
 geometry_msgs::Quaternion qMsg;
 tf::quaternionTFToMsg(quaternion, qMsg);
 // set quarternion to goal
 goal.target_pose.pose.orientation = qMsg;
 ROS_INFO("Sending goal");
 ac.sendGoal(goal);

 ac.waitForResult();

 if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
 ROS_INFO("Hooray, the base moved 1 meter forward");
else
   ROS_INFO("The base failed to move forward 1 meter for some reason");

 ros::spinOnce();
 }
  return 0;
}

basically what I want to do is calculate the quaternion by using the current pose by subscribing to the amcl_pose topic then I use the current pose and the goal pose to calculate the quaternion and sendgoal. But it didn't work and said trajectory error.

What did I do something wrong? Thank you!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-06-10 12:54:58 -0600

The first thing to do with any problem like this is you print out the values you're working with on the terminal so you can check if they look sensible. I would check are the values of current_x and current_y first, you're not checking that the call back has been called before using them, so they may well be huge uninitialized values in the billions. You may want to add a spin loop waiting for this call back before starting your main loop.

Also atan2 returns a value in radians, so you don't need to covert it.

Finally if that doesn't fix it then print out the whole value of goal just before you send it.

edit flag offensive delete link more

Comments

I did check the current_x and current_y and it is right. when You said add a spin loop, do you mean add it after the subscribe line?

Usui gravatar imageUsui ( 2019-06-10 13:07:56 -0600 )edit

Then you'll need to print the whole value of the goal out then.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-06-11 06:05:17 -0600 )edit

I updated my code

Usui gravatar imageUsui ( 2019-06-12 16:06:29 -0600 )edit

Can you copy and paste the exact output you're getting. Also printing out the full goal message just before it's sent would be really useful. How far from the current position is the goal set. We could do with more information to be able to help you

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-06-12 16:49:35 -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

2 followers

Stats

Asked: 2019-06-10 12:20:50 -0600

Seen: 63 times

Last updated: Jun 12