From b59f8ce49da71575d76c2da8f8bb5ba3e4c7977f Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sun, 19 Jul 2015 20:27:20 +0200
Subject: [PATCH 1/1] added waypoint script

---
 launch/move_base.launch |  2 +-
 scripts/waypoint.py     | 37 +++++++++++++++++++++++++++++++++++++
 2 files changed, 38 insertions(+), 1 deletion(-)
 create mode 100755 scripts/waypoint.py

diff --git a/launch/move_base.launch b/launch/move_base.launch
index 47c2369..b6924bd 100644
--- a/launch/move_base.launch
+++ b/launch/move_base.launch
@@ -23,7 +23,7 @@
 		<param name="odom_frame" value="odom"/>
 	</node>
 
-	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" unless="$(arg slam_gmapping)">
+	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
 		<rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
 		<rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
 		<rosparam file="$(find wild_thumper)/config/global_costmap_params.yaml" command="load" />
diff --git a/scripts/waypoint.py b/scripts/waypoint.py
new file mode 100755
index 0000000..ac82b36
--- /dev/null
+++ b/scripts/waypoint.py
@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import rospy
+import actionlib
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from actionlib_msgs.msg import GoalStatus
+
+class Waypoint:
+	def __init__(self):
+		rospy.init_node('waypoint')
+		self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+		while not self.move_base.wait_for_server(rospy.Duration(5)):
+			rospy.loginfo("Waiting for the move_base action server to come up")
+
+	def send_goal(self):
+		goal = MoveBaseGoal()
+
+		# we'll send a goal to the robot to move 1 meter forward
+		goal.target_pose.header.frame_id = "base_footprint"
+		goal.target_pose.header.stamp = rospy.Time.now()
+
+		goal.target_pose.pose.position.x = 1.0;
+		goal.target_pose.pose.orientation.w = 1.0;
+
+		self.move_base.send_goal(goal)
+
+		self.move_base.wait_for_result()
+
+		if self.move_base.get_state() == GoalStatus.SUCCEEDED:
+			rospy.loginfo("The base moved 1 meter forward")
+		else:
+			rospy.logerr("The base failed to move forward 1 meter")
+
+if __name__ == "__main__":
+	p = Waypoint()
+	p.send_goal()
-- 
2.39.2