Week06 day1 morning

src/ghostbuster/launch/all.launch

@@ -5,6 +5,10 @@
 
   <include file="$(find ghostbuster)/launch/navigate.launch" />
 
+  <!-- game logic -->
   <node pkg="ghostbuster" type="ghost_logic.py" name="ghost_logic" output="screen" required="true" />
 
+  <!-- game player -->
+  <node pkg="ghostbuster" type="ghost_buster.py" name="ghost_buster" output="screen"/>
+
 </launch>

src/ghostbuster/src/ghost_buster.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+
+import rospy
+from geometry_msgs.msg import PolygonStamped, PoseStamped, Quaternion
+
+header = None
+ghost_locations = None  # type : Point32[]
+
+
+# callbuck function of "/ghost" to update ghost locations
+def ghostCb(msg):
+
+  global header, ghost_locations
+
+  header = msg.header
+  ghost_locations = msg.polygon.points
+
+  rospy.logerr("ghost_locations updated. (%s ghosts remain)", len(ghost_locations))
+
+
+"""
+  function to calculate next goal.
+  param  : ghost_locations  (Point32[])
+  return : goal_pos         (PoseStamped)
+"""
+def createTarget(ghost_locations):
+
+  goal_pos = PoseStamped()
+
+  # if ghost exists
+  if len(ghost_locations) != 0:
+
+    goal_pos.header.frame_id = "map"
+    goal_pos.pose.position = ghost_locations[0]
+    goal_pos.pose.orientation = Quaternion(0,0,0,1)
+
+  return goal_pos
+
+
+# main function to buster ghosts
+def main():
+
+  global ghost_locations
+
+  rospy.init_node("ghost_buster")
+
+  rospy.Subscriber("ghost", PolygonStamped, ghostCb)
+
+  pub_goal = rospy.Publisher("move_base_simple/goal", PoseStamped)
+
+  rate = rospy.Rate(20.0)
+
+  while not rospy.is_shutdown():
+
+    if ghost_locations is not None:
+
+      goal_pos = createTarget(ghost_locations)
+      pub_goal.publish(goal_pos)
+
+    rate.sleep()
+
+
+if __name__ == '__main__':
+  main()