通过代码 发布 goal

How to launch the navigation system¶
Now let’s launch the navigation system.

Open a new terminal
Type the following command
$ roslaunch rosbot_navigation amcl_demo.launch
Open another terminal and launch rviz with the following command
$ rosrun rviz rviz -d /home/user/rosbot_rviz.rviz
Set robot initial pose programmatically
First thing is to indicate to the navigation system, where the robot is located inside the map.

You can do that using Rviz

But here we want to do it using code.

Create the package
$ roscd
$ cd …/src
$ catkin_create_pkg autonomous_nav rospy
Add the code
Create a Python file in the src directory named set_init_pose.py of the newly created package. Put the following code inside:

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

rospy.init_node('set_init_pose', anonymous=True)
pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)

initpose_msg = PoseWithCovarianceStamped()
initpose_msg.header.frame_id = "map"
initpose_msg.pose.pose.position.x = 0.0
initpose_msg.pose.pose.position.y = 0.0
initpose_msg.pose.pose.orientation.x = 0.0
initpose_msg.pose.pose.orientation.y = 0.0
initpose_msg.pose.pose.orientation.z = 0.0549847822366
initpose_msg.pose.pose.orientation.w = 0.998487192568

rospy.sleep(1)

rospy.loginfo ( "Setting initial pose")
pub.publish(initpose_msg)
rospy.loginfo ( "Initial pose SET")

Provide execution permissions and execute.

Send navigation goal programmatically
On the same package, create a new Python file named set_goal.py with the following code

#!/usr/bin/env python

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

# Callbacks definition

def active_cb(extra):
    rospy.loginfo("Goal pose being processed")

def feedback_cb(feedback):
    rospy.loginfo("Current location: "+str(feedback))

def done_cb(status, result):
    if status == 3:
        rospy.loginfo("Goal reached")
    if status == 2 or status == 8:
        rospy.loginfo("Goal cancelled")
    if status == 4:
        rospy.loginfo("Goal aborted")
    

rospy.init_node('send_goal')

navclient = actionlib.SimpleActionClient('move_base',MoveBaseAction)
navclient.wait_for_server()

# Example of navigation goal
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()

goal.target_pose.pose.position.x = -2.16
goal.target_pose.pose.position.y = 0.764
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.662
goal.target_pose.pose.orientation.w = 0.750

navclient.send_goal(goal, done_cb, active_cb, feedback_cb)
finished = navclient.wait_for_result()

if not finished:
    rospy.logerr("Action server not available!")
else:
    rospy.loginfo ( navclient.get_result())

Nos execute in a terminal. You should see how the robot goes to the specified location.

While the robot is moving, you should see the feedback information provided by the feedback callback.

At the end, you should see the message Goal reached.