[move_base, Noetic]I want to retrieve status from /move_base/status.
I want to make a code that outputs status from /move_base/status
and when the robot reaches the target point, it will go to the next target point.
However, I don't know how to extract only status.
How can I get it out?
What I did
Below is the code that is being changed.
#! /usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped
import numpy as np
import math
import time
from geometry_msgs.msg import Quaternion
from actionlib_msgs.msg import GoalStatusArray
# Define publisher, subscriber topic name
DEFAULT_SUBSCRIBER_TOPIC_1 = "/move_base/status"
class Goal:
#def __init__(self): 初期設定
def __init__(self):
rospy.init_node('set_goal_test5', anonymous=True)
self.ps_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1)
# From ROS parameter (set in launch file)
subscriber_topic_1 = rospy.get_param("~subscriber_topic_1", DEFAULT_SUBSCRIBER_TOPIC_1)
# Creating subscriber, subscribe (recieve) from topic
self.sub_1 = rospy.Subscriber(subscriber_topic_1, GoalStatusArray, self.callback_robot)
self.time = 1
def callback_robot(self, robot):
#robot_status=robot.header.seq
#robot_status=robot.status_list
#robot_status=robot.status_list[0].status
#robot_status=robot.status_list[3]
robot_status=robot.status_list.status
print(robot_status)
if self.time == 1:
self.time = self.time + 1
#mone_base_simple/goal
rospy.sleep(1.0)
now = rospy.Time.now()
goal_point = PoseStamped()
goal_point.pose.position.x = 0.5
goal_point.pose.position.y = 0
goal_point.pose.position.z = 0.0
goal_point.pose.orientation.w = 1.0
goal_point.header.stamp = now
goal_point.header.frame_id = 'map'
self.ps_pub.publish(goal_point)
if __name__ == '__main__':
try:
set_goal_node = Goal()
rospy.spin()
except rospy.ROSInterruptException: pass
I have tried the following five in the code above.
#robot_status=robot.header.seq
#robot_status=robot.status_list
#robot_status=robot.status_list[0].status
#robot_status=robot.status_list[3]
robot_status=robot.status_list.status
The first and second outputs from the top were accurate.
The third and fourth gave the following errors.
IndexError: list index out of range
The fifth error was as follows.
AttributeError: 'list' object has no attribute 'status'
How can I retrieve status from /move_base/status?
Asked by donguri on 2022-12-02 02:11:53 UTC
Answers
Typically /move_base/status
is a topic associated with the move_base ActionServer. Why are you not using the SimpleActionClient
class, which does exactly what you want to do?
See http://wiki.ros.org/actionlib, section 6.2 for a generic python example. For move_base use:
client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
goal = MoveBaseGoal()
/move_base_simple/goal
is completely unrelated to the move_base ActionServer. Do not use it if you use the ActionServer.
Update: there is a more complete example in @hskramer answer to #q80646
Asked by Mike Scheutzow on 2022-12-03 08:40:57 UTC
Comments
Thank you, Mike!!!
I have seen the code made in C++ but could not decipher it because I only learned python. I sincerely appreciate your thoughtful response!! :)
Asked by donguri on 2022-12-03 10:46:18 UTC
Comments