Ask Your Question
0

Question on turtlesim tutorial: Go to Goal

asked 2017-09-21 10:04:53 -0500

prasgane gravatar image

updated 2017-09-21 11:02:22 -0500

Hello,

I am new to ROS and I am trying to implement the Go to Goal tutorial in C++ (the tutorial itself is in python but wanted to implemented it in C++). The part I am having trouble understanding is, in the function move2goal() there is a while loop who's condition depends on the magnitude of the error between the current pose and goal pose and it is in this loop that the commanded velocity is calculated. I ran the script and it works fine.

When I try to implement the same code in C++ I too have a function, velocity_calc() which has a while loop based on the magnitude error. It is in this loop that I calculate the commanded velocity (same as tutorial). The problem I am facing is, when I call this function in my main() (like they did in the tutorial) the compiler gets stuck in the while loop and is not able to update the current pose ie go to the call back function. I did over come this by replacing the while loop with an if condition in my callback and got it to work. But I am still curious as to why the while loop does not work for me. How is the python script able to update the current pose in the while loop?

turtle_cl.h


#ifndef TURTLECL_H
#define TURTLECL_H
#include <ros ros.h="">
#include <turtlesim pose.h="">
#include <geometry_msgs twist.h="">
#include <math.h>

namespace turtle_cl {   class turtleCL {

  public:
    turtleCL();
    void velocity_calc();



  private:
    ros::NodeHandle nh_;
    ros::NodeHandle nh_private_;

    ros::Subscriber pose_subscriber_;
    ros::Publisher vel_publisher_;

    void poseCallback(const turtlesim::PoseConstPtr &msg);


    turtlesim::Pose current_pose;
    geometry_msgs::Twist cmd_vel;

    float k_p_lin, k_p_ang, tol, goal_x, goal_y, mag;   };

}
#endif

turtle_cl.cpp


#include "turtle_cl/turtlecl.h"

namespace turtle_cl
{
  turtleCL::turtleCL() :
    nh_(ros::NodeHandle()),
    nh_private_(ros::NodeHandle("~"))
  {

    pose_subscriber_ = nh_.subscribe<turtlesim::pose>
                        ("/turtle1/pose",10, &turtleCL::poseCallback, this);

    vel_publisher_ = nh_.advertise<geometry_msgs::twist>
                        ("/turtle1/cmd_vel",10,this);

    nh_private_.param<float>("p_gain_linear",k_p_lin,0.1);
    nh_private_.param<float>("p_gain_ang",k_p_ang,0.1);
    nh_private_.param<float>("tolerance",tol,0.1);
    nh_private_.param<float>("xGoal",goal_x,1);
    nh_private_.param<float>("yGoal",goal_y,1);

  }

  void turtleCL::poseCallback(const turtlesim::PoseConstPtr &msg)
  {
    current_pose = *msg;
    float mag1, mag2;
    mag1 = pow((goal_x - msg->x),2);
    mag2 = pow((goal_y - msg->y),2);
    mag = sqrt(mag1 + mag2);
    // ROS_INFO("in callback");
    // if(mag >= tol)
    //   velocity_calc();

  }

  void turtleCL::velocity_calc()
  {

    //ROS_INFO("in velocity_cal %f",mag);
    while(mag >= tol)
    {
      float velx,angx,angy;
      angx = goal_x - current_pose.x;
      angy = goal_y - current_pose.y;
      cmd_vel.linear.x = k_p_lin * mag;
      cmd_vel.angular.z = k_p_ang * atan2(angy,angx) - current_pose.theta;
      vel_publisher_.publish(cmd_vel);
    }
    //ROS_INFO("in while %f",current_pose.x);

  }

}

turtle_cl_node.cpp


#include <ros ros.h="">
#include <turtle_cl turtlecl.h="">


int main(int argc, char** argv)
{
  ros::init(argc,argv,"turtle_cl_node");
  ros::NodeHandle nh;

  turtle_cl::turtleCL hello;
  hello.velocity_calc();

  ros::spin();
}

edit retag flag offensive close merge delete

Comments

Can you please post the relevant code here? If your code gets removed from that site this question won't be able to help anyone in the future.

jayess gravatar image jayess  ( 2017-09-21 10:38:06 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-09-25 08:38:49 -0500

prasgane gravatar image

Adding ros::spinOnce() in velocity_calc() fixes this. I also learnt that python supports multi threading which enables it to get both the current pose while being in the while loop.

edit flag offensive delete link more

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: 2017-09-21 10:04:53 -0500

Seen: 892 times

Last updated: Sep 25 '17