goto.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 goto.py is a simple navigation system that can go to a location
00005   based on a voice command. 
00006 """
00007 
00008 import roslib; roslib.load_manifest('maxwell_navigation')
00009 import rospy
00010 import math
00011 
00012 import tf
00013 from tf.transformations import euler_from_quaternion, quaternion_from_euler
00014 from geometry_msgs.msg import PoseStamped
00015 from std_msgs.msg import String
00016 
00017 goals = { "Nick's Office": [-1.271, 5.108, 0.000, 1.512],
00018           "Lynn's Office": [-4.112, 5.416, 0.000, 1.385],
00019           "Tomek's Office": [-5.792, 5.701, 0.000, 3.049],
00020           "Mike's Desk": [0.668, 2.886, 0.000, -1.136],
00021           "Ken's Desk": [3.148, 2.630, 0.000, -0.959],
00022           "Samira's Office": [4.705, 4.592, 0.000, 1.350],
00023           "The Lab": [3.027, -0.966, 0.000, 0.011],
00024           "The Hallway": [-0.785, -1.890, 0.000, -1.601] }
00025 
00026 class maxwell_goto:
00027 
00028     def __init__(self):
00029         self.msg = PoseStamped()
00030         self.msg.header.frame_id = "/map"
00031 
00032         self.pub_ = rospy.Publisher('/move_base_simple/goal', PoseStamped)
00033         rospy.Subscriber('/recognizer/output', String, self.speechCb)
00034         rospy.spin()
00035 
00036     def setMsg(self, goal):
00037         self.msg.pose.position.x = goals[goal][0]
00038         self.msg.pose.position.y = goals[goal][1]
00039         self.msg.pose.position.z = goals[goal][2]
00040         q = quaternion_from_euler(0, 0, goals[goal][3], 'sxyz')
00041         self.msg.pose.orientation.x = q[0]
00042         self.msg.pose.orientation.y = q[1]
00043         self.msg.pose.orientation.z = q[2]
00044         self.msg.pose.orientation.w = q[3]
00045         
00046     def speechCb(self, msg):
00047         rospy.loginfo(msg.data)
00048         self.msg.header.stamp = rospy.Time.now()
00049 
00050         if msg.data.find("lab") > -1: 
00051             self.setMsg("The Lab")
00052         elif msg.data.find("hallway") > -1:
00053             self.setMsg("The Hallway")
00054         elif msg.data.find("ken") > -1:
00055             self.setMsg("Ken's Desk")
00056         elif msg.data.find("nick") > -1:
00057             self.setMsg("Nick's Office")
00058         else:
00059             rospy.loginfo("unknown goal!")
00060             return
00061 
00062         self.pub_.publish(self.msg)
00063 
00064     def cleanup(self):
00065         # stop the robot!
00066         twist = Twist()
00067         self.pub_.publish(twist)
00068 
00069 if __name__=="__main__":
00070     rospy.init_node('maxwell_goto')
00071     try:
00072         maxwell_goto()
00073     except:
00074         pass
00075 


maxwell_navigation
Author(s): Michael Ferguson
autogenerated on Tue Jan 7 2014 11:22:48