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
#include
#include
#include
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include
typedef actionlib::SimpleActionClient MoveBaseClient;
float current_x,current_y; // current pose
void poseCallBack(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
current_x = msg->pose.pose.position.x;
current_y = msg->pose.pose.position.y;
}
int main(int argc, char** argv){
ros::init(argc, argv, "simple_navigation_goals");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/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;
// x and y position, we want to go diagonal
float xy_ar[3] = {0.01, 0.02, 0.03};
int index = 0;
//we'll send a goal to the robot to move 1 meter forward
goal.target_pose.header.frame_id = "base_link";
while(ros::ok()){
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = xy_ar[index];
goal.target_pose.pose.position.y = xy_ar[index];
// calculate the angle
double x = xy_ar[index] - current_x;
double y = xy_ar[index] - current_y;
double theta = atan2(y,x);
// convert angle to quarternion
double radians = theta * (M_PI/180);
tf::Quaternion quaternion;
quaternion = tf::createQuaternionFromYaw(radians);
geometry_msgs::Quaternion qMsg;
tf::quaternionTFToMsg(quaternion, qMsg);
// set quarternion to goal
goal.target_pose.pose.orientation = qMsg;
index++;
if (index == 3) {index = 0;}
ROS_INFO("Sending goal");
ac.sendGoal(goal);
ac.waitForResult();
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("Hooray");
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!
↧