No rotation seen in going from one place to another program!

Hello everyone, I am not very good at C++, however I am learning so much with ROS that I want to improve it (I don't know if I can ask this here but I do it cause I don't know what else to try). Right now I am making a robot to go from an original to an aim position. I am getting the estimate coordinates and yaw in every iteration as you can see in my program

#define pi 3.14159265359

//Gets the current estimated position from the robot

void Guess_what::poseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{

y_estimated = msg->pose.pose.position.y;

x_estimated = msg->pose.pose.position.x;

yaw_estimated = tf::getYaw(msg->pose.pose.orientation);
}

// robot shall stop, in case anything is closer than ...

void Guess_what::calculateCommand() {

aim_x = 33.5;
aim_y = 14.5;

dist = sqrt(pow(aim_y - y_estimated,2) + pow(aim_x - x_estimated,2));

double delta_angle = asin((aim_x - x_estimated)/dist);

double aim_angle = yaw_estimated - delta_angle;

/*ROS_INFO("estimate_yaw %f, delta_angle %f, aim_angle %f", yaw_estimated, delta_angle, aim_angle);*/

if(yaw_estimated > (aim_angle + 2.5/pi) || yaw_estimated < (aim_angle - 2.5/pi))
{
if(yaw_estimated > aim_angle)
{
m_roombaCommand.linear.x  = 0.2 ;
m_roombaCommand.angular.z = 0.0 ;
}
else
{
m_roombaCommand.linear.x  = -0.2 ;
m_roombaCommand.angular.z = 0.0 ;
}
}
else
{
m_roombaCommand.linear.x  = 0.2 ;
m_roombaCommand.angular.z = 0.0 ;
/*ROS_INFO("estimate_yaw %f, delta_angle %f, aim_angle %f", yaw_estimated, delta_angle, aim_angle);*/
}

if(dist <= 0.15)
{
m_roombaCommand.linear.x  = 0.0 ;
m_roombaCommand.angular.z = 0.0 ;
}
}

//
void Guess_what::mainLoop() {
// determines the number of loops per second
ros::Rate loop_rate(20); // it was 20

while (m_nodeHandle.ok())
{
calculateCommand();
emergencyStop();

ROS_INFO("POS estimated_x = %f, aim_X= %f, estimated_y =%f, aim_Y= %f, dist = %f, estimated_Yaw %f",x_estimated,aim_x,y_estimated,aim_y,dist, yaw_estimated);

// send the command to the roomrider for execution
m_commandPublisher.publish(m_roombaCommand);

ros::spinOnce();
loop_rate.sleep();
}

int main(int argc, char** argv) {

ros::init(argc, argv, "Guess_what");
ros::Time::init();
ros::Duration(5).sleep(); //*/
Guess_what dude  ;
dude.mainLoop();

return 1;
}

However when I run it, it turns out that the robot just makes a rotation in the beginning of the program and later it just keeps going forward.

The initial position is at (33.0, 0.0). I also used the parameter /use_sim_time (in case it can give more information)

I tried several combinations, however either the robot stays turning around with linnear speed 0 or it just ignores the place to go. I was going to copy the whole file, however I guess that these are the most important parts.

Could you please tell me if there is something missing in my function calculateCommand (I think it's fine, it should not be that complicated to go from one place to another one) or if there is a problem with the arcsin function that I don't know?

Thank you.

edit retag close merge delete

Sort by » oldest newest most voted I haven't done an in-depth analysis of all of your program logic, but the obvious bug seems to be that you never set the angular.z component of the motion command to a non-zero value, which means that you never send a turning command to the robot.

(linear.x is forward/backward velocity. angular.z is left/right)

more

Ahh I've got it... It was always in front of me :( . Thanks for your help.