Sending Goals to the Navigation Stack Help

edit

The following code is in Python but it shows you how to use Waypoints with Husky, currently is an infinite loop, but you can modify accordingly:

Launch the following in separate terminals:

$ roslaunch husky_gazebo husky_empty_world
$ roslaunch husky_viz view_robot.launch
$ roslaunch husky_navigation move_base_mapless_demo.launch

Run the following python program: navigation_husky.py

#! /usr/bin/env python3

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

class MoveHusky(object):
  def __init__(self):
    self.client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
    self.client.wait_for_server()
    self.goal = MoveBaseGoal()

  def moveToDest(self, x, y):
    self.goal.target_pose.header.frame_id = "odom"
    self.goal.target_pose.pose.position.x = x
    self.goal.target_pose.pose.position.y = y
    self.goal.target_pose.pose.orientation.z = 0.7071
    self.goal.target_pose.pose.orientation.w = 0.7071
    rate = rospy.Rate(10)
    self.client.send_goal(self.goal)
    result = self.client.get_result()
    while result == None:
      result = self.client.get_result()
      status = self.client.get_state()
      rospy.loginfo("Status: {}".format(status))
      rospy.loginfo("Result: {}".format(result))
      rate.sleep()

if __name__ == "__main__":
  rospy.init_node("move_base_client")
  moveHusky = MoveHusky()

  while not rospy.is_shutdown():
    moveHusky.moveToDest(-4.0, 0.0)
    moveHusky.moveToDest(-4.0, -2.0)
    moveHusky.moveToDest(0.0, 0.0)