0
$\begingroup$

Rosanswers logo

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 
#include 
#include 
#include 

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
                        ("/turtle1/pose",10, &turtleCL::poseCallback, this);

    vel_publisher_ = nh_.advertise
                        ("/turtle1/cmd_vel",10,this);

    nh_private_.param("p_gain_linear",k_p_lin,0.1);
    nh_private_.param("p_gain_ang",k_p_ang,0.1);
    nh_private_.param("tolerance",tol,0.1);
    nh_private_.param("xGoal",goal_x,1);
    nh_private_.param("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 
#include 


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();
}


Originally posted by prasgane on ROS Answers with karma: 16 on 2017-09-21

Post score: 0


Original comments

Comment by jayess on 2017-09-21:
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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by prasgane with karma: 16 on 2017-09-25

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.