ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Subcriber block Actionlib in the same node

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

frizh gravatar image

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

2 Answers

Sort by ยป oldest newest most voted
1

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

miura gravatar image

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 more

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 gravatar image frizh  ( 2020-11-11 23:51:14 -0600 )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 gravatar image miura  ( 2020-11-12 01:22:46 -0600 )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 gravatar image frizh  ( 2020-11-12 01:28:51 -0600 )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 gravatar image miura  ( 2020-11-12 02:25:07 -0600 )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 gravatar image frizh  ( 2020-11-12 02:51:38 -0600 )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 gravatar image miura  ( 2020-11-12 03:06:46 -0600 )edit
1

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

frizh gravatar image frizh  ( 2020-11-13 03:51:02 -0600 )edit

I'm glad it moved.

miura gravatar image miura  ( 2020-11-18 08:14:52 -0600 )edit
0

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

frizh gravatar image

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 more

Comments

1

Congratulations. I'm glad you moved.

miura gravatar image miura  ( 2020-11-13 04:21:07 -0600 )edit

Question Tools

Stats

Asked: 2020-11-10 22:27:27 -0600

Seen: 156 times

Last updated: Nov 13 '20