How to find out other robots finished goal?

asked 2020-04-30 12:23:19 -0500

MarosROS gravatar image

Hi, my goal is to send a goal on one node and find out the state of the goal in other node. I am working with subscribers and publishers. However, this simple task is not as simple as i thought it would be, because there are these errors. Is it possible to do something similar to this by other methods or is there a mistake in this code? Thank you for any help.

ROS_ERROR("Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
ROS_ERROR("Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient");

Below is the first file, in which i send goal for robot.

def movebase_client(data):
  target_position = data
  client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
  client.wait_for_server()

  goal = MoveBaseGoal()
  goal.target_pose.header.frame_id = "map"
  goal.target_pose.header.stamp = rospy.Time.now()
  goal.target_pose.pose.position = target_position
  goal.target_pose.pose.orientation.w = 1.0

  client.send_goal(goal)

def listener():
  rospy.init_node('listener', anonymous=True)
  rospy.Subscriber("tb3_1/chatter", Point, movebase_client)
  rospy.spin()

And below this is second file, in which my intention is to check whether the robot has got to the goal.

ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<geometry_msgs::Point>("chatter", 10);
ros::Rate loop_rate(1);

MoveBaseClient ac("move_base", true);

while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
while (ros::ok()){
   if(chatter_pub.getNumSubscribers() > 0){
       chatter_pub.publish(target_position);
       loop_rate.sleep();
       ac.waitForResult();

       if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
           ROS_INFO("Hooray");
       }else{
           ROS_INFO("Failure);
       }
   }
}
edit retag flag offensive close merge delete