Subcriber block Actionlib in the same node

asked 2020-11-10 22:27:27 -0500

frizh
3 ●3 ●6 ●6

Hello I was tried to combining node with subscribe and actionlib in the same node.

I want to get 5 data from callback subscribe and then run it to actionlib. but my program stuck on the callback and never run to actionlib

I'm quite new to actionlib, so please anyone can help me? I will appreciate every help that i can get thank you best regards

here is the code i use :

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

bool moveToGoal(double xGoal, double yGoal);
double x[5];
double y[5];
int u = 0;
int n = 0;
bool waypoint = false;
bool goalReached = false;

void chatterCallback(const geometry_msgs::PointStamped& msg)
{
  ROS_INFO("I heard: [%f, %f]", msg.point.x, msg.point.y);

  if (u < 3)
  {
    x[u] = msg.point.x;
    y[u] = msg.point.y;

    ROS_INFO("Position X[ and Y: [%f, %f]", x[u], y[u]);
    u += 1;
  }
  if (u >= 3)
  {
    waypoint = true;
    ROS_INFO("waypoint done");
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "map_waypoint");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("clicked_point", 1000, chatterCallback);

  while (waypoint == true)
  {
    for (int i = 0; i <= 2; i++)
    {
      goalReached = moveToGoal(x[i], y[i]);
      ROS_INFO("start moving");
      ros::spinOnce();
      if (goalReached)
      {
        ROS_INFO("Congratulations![%f, %f]", x[i], y[i]);
        ros::spinOnce();
        // sc.playWave(path_to_sounds+"ship_bell.wav");
        // ros::spinOnce();
      }
      else
      {
        ROS_INFO("Hard Luck!");
            // sc.playWave(path_to_sounds+"short_buzzer.wav");
          }
        }
      }
      ros::spin();
      return 0;
    }

bool moveToGoal(double xGoal, double yGoal)
{
  // define a client for to send goal requests to the move_base server through a SimpleActionClient
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> 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;

  // set up the frame parameters
  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();

  /* moving towards the goal*/

  goal.target_pose.pose.position.x = xGoal;
  goal.target_pose.pose.position.y = yGoal;
  goal.target_pose.pose.position.z = 0.0;
  goal.target_pose.pose.orientation.x = 0.0;
  goal.target_pose.pose.orientation.y = 0.0;
  goal.target_pose.pose.orientation.z = 0.0;
  goal.target_pose.pose.orientation.w = 1.0;

  ROS_INFO("Sending goal location ...");
  ac.sendGoal(goal);

  ac.waitForResult();

  if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
  {
    ROS_INFO("You have reached the destination");
    return true;
  }
  else
  {
    ROS_INFO("The robot failed to reach the destination");
    return false;
  }
}

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2020-11-11 05:16:37 -0500

miura

1908 ●1 ●171 ●24 https://twitter.com/miura

If you don't do spinOnce() or spin(), the callback will not work and the waypoint will remain false.

A simple solution is to use

while (waypoint == true)

indicates an area of expertise

while(waypoint == false)
{
    ros::spinOnece();
}
while (waypoint == true)

You can do this by

edit flag offensive delete link

Comments

hii thanky you for your answer I have recently do what you suggest, but it still not working. It always subcribing my subscriber and cannot go on to my actionlib

I have tried to use spin or spinOnce also but when use spinOnce, subscriber won't listen my /clickedpoint. but when i use spin, it always listening my topic and cannot go on to actionlib

here is the code i modified

ros::init(argc, argv, "map_waypoint");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("clicked_point",1000 ,chatterCallback);
if (donewaypoint == false){ 

    ROS_INFO("Waiting Waypoint!");
    ros::spin();

}

frizh ( 2020-11-11 23:51:14 -0500 )edit

Do you have the following output when you click on it?

ROS_INFO("I heard: [%f, %f]", msg.point.x, msg.point.y);

If there is no output, you may need to change clicked_point to clickedpoint in the following code

ros::Subscriber sub = n.subscribe("clicked_point",1000 ,chatterCallback);

miura ( 2020-11-12 01:22:46 -0500 )edit

hii tahnks for your reply yeah i got the rosinfo like that. in the code, when we got 4 point, it should change bool waypoint to true and starting goalreached but in my case, after i goit 4 point, it not start goalreached program and always looping back to subscriber

frizh ( 2020-11-12 01:28:51 -0500 )edit

Once spin() is called, it doesn't come back. Therefore, I think it's better to use the method I showed before, which uses spinOnce(). Alternatively, you can run spin() in another thread.

miura ( 2020-11-12 02:25:07 -0500 )edit

Hii miura thanks for the reply I have also tried that, but when use spinOnce, it only appears like this (even i publish the topic.

[ INFO] [1605171006.582843595]: Waiting Waypoint!

any refrence to do the spin() in another thread? thanks in advance

frizh ( 2020-11-12 02:51:38 -0500 )edit

Let's try.

#include <pthread.h>
// ...
void *spin_thread(void *argv)
{
    ros::spin();
}

int main(int argc, char** argv)
{
    // ...
    pthread_t spin_tid;
    pthread_create(&spin_tid, NULL, spin_thread, NULL);
    while(waypoint == false)
    {
        ROS_INFO("Waiting Waypoint!");
    }
    while (waypoint == true)
    // ...

miura ( 2020-11-12 03:06:46 -0500 )edit

Hi thank you for the refrence I have figured it out with combination of your answer regards

frizh ( 2020-11-13 03:51:02 -0500 )edit

I'm glad it moved.

miura ( 2020-11-18 08:14:52 -0500 )edit

add a comment

answered 2020-11-13 03:53:44 -0500

frizh
3 ●3 ●6 ●6

here is the right code thanks to @miura who help me

int main(int argc, char** argv)
{
  ros::init(argc, argv, "map_waypoint");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("clicked_point", 1000, chatterCallback);

  while (ros::ok())
  {

if (donewaypoint == false)
    {
      ros::spinOnce();
    }

if (donewaypoint == true)
    for (int i = 0; i <= 2; i++)
    {
      goalReached = moveToGoal(x[i], y[i]);
      ROS_INFO("start moving");
      ros::spinOnce();
      if (goalReached)
      {
        ROS_INFO("Congratulations![%f, %f]", x[i], y[i]);
        ros::spinOnce();
        // sc.playWave(path_to_sounds+"ship_bell.wav");
        // ros::spinOnce();
      }
      else
      {
        ROS_INFO("Hard Luck!");
            // sc.playWave(path_to_sounds+"short_buzzer.wav");
          }
        }
      }
      ros::spin();
      return 0;
    }

edit flag offensive delete link

Comments

Congratulations. I'm glad you moved.

miura ( 2020-11-13 04:21:07 -0500 )edit

add a comment

Subcriber block Actionlib in the same node

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Subcriber block Actionlib in the same node edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Subcriber block Actionlib in the same node