Ask Your Question
0

2D Nav Goal not working in Rviz

asked 2018-08-17 04:07:51 -0500

dkrivet gravatar image

updated 2018-08-20 09:46:22 -0500

I am using a Toyota HSR robot running ubuntu 16.04 and ROS kinetic. I am having issues getting the move_base node to work by sending nav goals in Rviz. When I do rostopic list | grep move I see the following topic: /move_base/move/goal. However when I run Rviz, Rviz sends its 2D nav goals to the topic /move_base_simple/goal. I thought about just switching the topic that the 2D Nav goal publishes to but unfortunately the two topics do not have the same message types. /move_base/move/goal is of type move_base_msgs/MoveBaseActionGoal whilst /move_base_simple/goal is of type geometry_msgs/PostStamped. Currently I am thinking of writing a node that subscribes to /move_base_simple/goal and publishes to /move_base/move/goal but I am wondering if there is an easier way to go about this. Thanks for all the help!

#!/usr/bin/env python

import rospy
import roslib
import actionlib

from std_msgs.msg import String, Empty
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseGoal
from actionlib_msgs.msg import GoalID


'''def goal_client():
    client = actionlib.SimpleActionClient('send_goal', GoalID)
    client.wait_for_server()'''



def callback(data):
    rospy.loginfo(rospy.get_caller_id() + ' I Heard %s', data.header.stamp)
    rospy.loginfo(rospy.get_caller_id() + ' I Heard %s', data)
    #rospy.loginfo(rospy.get_caller_id() + ' I Heard %s', data.pose.position)
    #rospy.loginfo(rospy.get_caller_id() + ' I heard %s', data.pose.orientation)
    #rospy.loginfo(rospy.get_caller_id() + ' I heard %s', data.header)
    pub = rospy.Publisher('move_base/move/goal', MoveBaseActionGoal, queue_size = 10)
    stuff_to_publish = MoveBaseActionGoal()
    stuff_to_publish.header = data.header
    stuff_to_publish.goal_id.stamp = data.header.stamp
    stuff_to_publish.goal_id.id = 'random_string'
    stuff_to_publish.goal.target_pose = data
    pub.publish(stuff_to_publish)        

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('navgoallistener', anonymous=True)

    rospy.Subscriber('move_base_simple/goal', PoseStamped, callback)

    #message = rospy.Subscriber('move_base_simple/goal', PoseStamped, callback)
    #print message.pose.orientation

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

"""def talker():
    pub = rospy.Publisher('move_base/move/goal', MoveBaseActionGoal, queue_size = 10)
    rospy.init_node('navgoalsetter', anonymous = True)"""


if __name__ == '__main__':
    try:
        listener()
    except rospy.ROSInterruptException:
        pass
edit retag flag offensive close merge delete

Comments

1

I think you're on the right track, I can't think of an easier way to do this myself. Making such a node should be straightforward at least.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-08-18 06:24:48 -0500 )edit

Thank you very much! I have begun writing the node although I am encountering issues because I do not know how to populate the header and goal_id sections of the move_base_msgs/MoveBaseActionGoal message

dkrivet gravatar imagedkrivet ( 2018-08-20 04:21:05 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-08-20 05:52:33 -0500

updated 2018-08-20 10:05:10 -0500

Regarding your comment above the the documentation for the goal_id part of the message says the stamp is just the current time and the id can be any unique string (you could prefix a string to a string of the current time for this).

The header you can copy directly from the geometry_msgs/PoseStamped message to the new move_base message.

Hope this helps.

UPDATE :

Okay, you're almost there. The problem is that you're creating a new publisher object each time that a message is received, you should only create the publisher once as a global variable. The reason why no messages are being received is that a publisher object takes a small amount of time to register with any subscribers, so if you create it and instantly publish a message to it no nodes will receive it.

If you move the line pub = rospy.Publisher('move_base/move/goal', MoveBaseActionGoal, queue_size = 10) to the top of the code so it's global it should start working.

edit flag offensive delete link more

Comments

I have edited my question to include the code where I try to do this. However it seems to be I am not doing it correctly as I get an error with this script. Can you suggest to me the correct way to publish the information I want to the new topic? Thanks a lot!

dkrivet gravatar imagedkrivet ( 2018-08-20 08:27:33 -0500 )edit

I've updated my code to get it work properly so that it publishes to the correct topic with the correct message type, however I still cannot get 2D Nav Goals from Rviz to work on my robot :(

dkrivet gravatar imagedkrivet ( 2018-08-20 09:44:09 -0500 )edit
1

This has worked! One final thing I had to change was the line stuff_to_publish.goal_id.id = 'random_string' to stuff_to_publish.goal_id.id = ''.join([random.choice(string.ascii_letters + string.digits) for n in xrange(15)]) so that the goal_id was truly unique and now it works!

dkrivet gravatar imagedkrivet ( 2018-08-20 10:39:55 -0500 )edit

Thank you so much for your help, I really appreciate it

dkrivet gravatar imagedkrivet ( 2018-08-20 10:40:06 -0500 )edit

Great. Glad you got it working!

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-08-20 11:09:15 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-08-17 04:07:51 -0500

Seen: 402 times

Last updated: Aug 20 '18