mostly finish kadai7

src/ghostbuster/launch/all.launch

@@ -9,6 +9,6 @@
   <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"/>
+  <!--node pkg="ghostbuster" type="ghost_buster.py" name="ghost_buster" output="screen"/-->
 
 </launch>

src/ghostbuster/src/ghost_buster.py

@@ -1,61 +1,102 @@
 #!/usr/bin/env python
 
 import rospy
-from geometry_msgs.msg import PolygonStamped, PoseStamped, Quaternion
+
+from sub_funs import chooseTarget, eucliDist
+
+from geometry_msgs.msg import PolygonStamped, PoseStamped
+from nav_msgs.msg import Odometry
+from std_srvs.srv import Empty
 
 header = None
 ghost_locations = None  # type : Point32[]
+buster_pos = None   # type : Point (Point64)
+published = False
 
 
-# callbuck function of "/ghost" to update ghost locations
+# callback function of "/ghost" to update ghost locations
 def ghostCb(msg):
 
-  global header, ghost_locations
+  global header, ghost_locations, published
 
   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):
+  published = False
+  rospy.logerr("ghost_locations updated in ghostCb (%s remain)", len(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)
+# callback function of "/odom" to update player location
+def odomCb(msg):
+
+  global buster_pos
 
-  return goal_pos
+  buster_pos = msg.pose.pose.position
 
 
 # main function to buster ghosts
 def main():
 
-  global ghost_locations
+  global ghost_locations, published
 
   rospy.init_node("ghost_buster")
 
+  # subscribe /ghost and /odom
   rospy.Subscriber("ghost", PolygonStamped, ghostCb)
+  rospy.Subscriber("odom", Odometry, odomCb)
 
-  pub_goal = rospy.Publisher("move_base_simple/goal", PoseStamped)
+  # publisher of goal position
+  pub_goal = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=10)
 
-  rate = rospy.Rate(20.0)
 
-  while not rospy.is_shutdown():
-
-    if ghost_locations is not None:
+  rate = rospy.Rate(1.0)
 
-      goal_pos = createTarget(ghost_locations)
-      pub_goal.publish(goal_pos)
+  while not rospy.is_shutdown():
+
+    # choose next target from list
+    goal_pos = chooseTarget(ghost_locations)
+
+    # distance between goal and player
+    if goal_pos is not None:
+
+      # pubish goal position if not yet published.
+      if not published:
+
+        pub_goal.publish(goal_pos)
+        published = True
+        rospy.logerr("target pubished in main()")
+
+      # distance to the target
+      d = eucliDist(
+        goal_pos.pose.position.x,
+        goal_pos.pose.position.y,
+        buster_pos.x,
+        buster_pos.y
+        )
+
+      rospy.loginfo("d = %s", d)
+
+      # if reached the target, use "buster"
+      if d < 0.1:
+
+        try:
+          service = rospy.ServiceProxy("buster", Empty)
+          service()
+          rospy.logerr("BUSTER CALLED")
+
+        except rospy.ServiceException as e:
+          rospy.logerr(e)
+
+        """
+        # remove current goal from list
+        ghost_locations = ghost_locations[1:]
+        published = False
+        rospy.loginfo("Ghost list (%s reamin): %s", len(ghost_locations), ghost_locations)
+        """
+
+    else:
+
+      rospy.logerr("no target")
 
     rate.sleep()
 

src/ghostbuster/src/sub_funs.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+
+import numpy as np
+from geometry_msgs.msg import PoseStamped, Quaternion
+
+"""
+  Function to calculate next goal.
+  If there is no ghost, return is None.
+  param  : ghost_locations  (Point32[])
+  return : goal_pos         (PoseStamped)
+"""
+def chooseTarget(ghost_locations):
+
+  # if ghost exists
+  if (ghost_locations is not None) and len(ghost_locations) > 0:
+
+    goal_pos = PoseStamped()
+
+    goal_pos.header.frame_id = "map"
+    goal_pos.pose.position = ghost_locations[0]
+    goal_pos.pose.orientation = Quaternion(0,0,0,1)
+
+  else:
+
+    goal_pos = None
+
+  return goal_pos
+
+
+def eucliDist(x1,y1,x2,y2):   
+	# np.abs(pose.position.x - ball["x"]) + np.abs(pose.position.y - ball["y"])
+	return np.sqrt((x2-x1)**2+(y2-y1)**2)