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

[move_base, Noetic]I want to retrieve status from /move_base/status.

asked 2022-12-02 01:11:53 -0500

donguri gravatar image

updated 2023-06-18 09:44:22 -0500

lucasw gravatar image

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?

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2022-12-03 07:40:57 -0500

Mike Scheutzow gravatar image

updated 2022-12-03 08:17:21 -0500

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

edit flag offensive delete link more

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!! :)

donguri gravatar image donguri  ( 2022-12-03 09:46:18 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2022-12-02 01:11:53 -0500

Seen: 261 times

Last updated: Dec 03 '22