Update visit_door_list_phhp

Add "convert to right-lane-following baseline mode" depending on the wifi connection status

bwi_tasks/scripts/visit_door_list_phhp

@@ -5,12 +5,23 @@ import random
 
 import rospy
 from actionlib import SimpleActionClient
+from std_msgs.msg import String
 from std_srvs.srv import Empty
 from nav_msgs.srv import GetPlan, GetPlanRequest
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from geometry_msgs.msg import Point32, PolygonStamped
 from move_base_msgs.msg import MoveBaseGoal, MoveBaseAction
 
+CONFIG = dict(phhp={"radius": 0.5122,
+                    "dr": 0.5661,
+                    "k_begin": 0.4842,
+                    "k_end": 0.5001},
+              baseline={"radius":1.000,
+                        "dr": 1.075,
+                        "k_begin":0.000,
+                        "k_end": 1.000})
+"""
+# Disable rosbag record system
 class Record:
     def __init__(self):
         import time
@@ -44,18 +55,14 @@ class Record:
 
     def comms_cb(self, msg):
         self.bag.write("comms/amcl_pose", msg)
-
+"""
 class BWIbot:
     def __init__(self):
-        self.rec = Record()
+        # self.rec = Record()
         # Define PHHP-related parameters
-        self.phhp = False
-        self.detection_range = 8.0
-        self.phhp_radius  = 0.5122
-        self.phhp_dr      = 0.5661
-        self.phhp_k_begin = 0.4842
-        self.phhp_k_end   = 0.5001
-
+        self.mode = "phhp"
+        self.vo_installed = False
+        self.detection_range = {"phhp": 8.0, "baseline": np.inf}
         self.opponent_location = np.zeros(2)    # (x, y)
 
         # Define move_base
@@ -73,6 +80,7 @@ class BWIbot:
 
         # Define subscribers
         self.__sub_opponent_amcl = rospy.Subscriber("/comms/amcl_pose", PoseWithCovarianceStamped, self.amcl_cb)
+        self.__sub_wifi_condition = rospy.Subscriber("/iswifion", String, self.wifi_cb)
 
         # Define publishers
         self.__pub_hallucination = rospy.Publisher("/add_circles", PolygonStamped, queue_size=10)
@@ -81,6 +89,21 @@ class BWIbot:
         # subscribe to other robot's location
         self.opponent_location[0] = amcl.pose.pose.position.x
         self.opponent_location[1] = amcl.pose.pose.position.y
+
+    def wifi_cb(self, wifi_msg):
+        if not wifi_msg in [str(True), str(False)]:
+            raise ValueError("{} is not a valid /iswifion msg. Please check $comms_log_pkg.".format(wifi_msg))
+
+        # Convert mode when wifi status is changed
+        if self.mode=="baseline" and wifi_msg == str(True):
+            # Use PHHP mode and remove all virtual obstacles placed during the baseline mode
+            self.clear_hallucination_srv()
+            rospy.sleep(0.5)    # Make sure vos are completely removed.
+            self.mode = "phhp"
+            self.vo_installed = False
+        elif self.mode=="phhp" and wifi_msg == str(False):
+            # Use baseline mode
+            self.mode = "baseline"
 
     def clear_costmaps(self):
         rospy.wait_for_service("/move_base/clear_costmaps")
@@ -103,7 +126,8 @@ class BWIbot:
         self.goal.target_pose.pose.orientation.z = math.sin(yaw/2.)
         self.goal.target_pose.pose.orientation.w = math.cos(yaw/2.)
 
-        self.phhp = False
+        self.mode = "phhp"
+        self.vo_installed = False
         start_time = rospy.Time.now()
         self.move_base.send_goal(
             goal        = self.goal,
@@ -119,20 +143,21 @@ class BWIbot:
     def feedback_cb(self, feedback):
         curr_pose = feedback.base_position.pose
 
-        if self.phhp is False:
+        if self.vo_installed is False:
             plan = self.get_plan_to_goal(curr_pose)
 
             try:
                 # check if plan overlap with other robot
                 idx = np.argwhere( np.linalg.norm(plan-self.opponent_location, axis=1) < 0.5 )[0][0]
                 dist = np.linalg.norm(plan[1:idx+1] - plan[0:idx], axis=1).sum()
-                if self.phhp is False and dist < self.detection_range:
+                if self.vo_installed is False and dist < self.detection_range[self.mode]:
                     self.generate_vo(                       # Right lane following baseline
                         center=plan[:idx+1],                # plan[:idx+1]
-                        radius=self.phhp_radius,            # 1.0
-                        dr=self.phhp_dr-self.phhp_radius,   # 0.075
-                        k_begin=self.phhp_k_begin,          # 0.2
-                        k_end=self.phhp_k_end               # 0.8
+                        **CONFIG[self.mode]
+                        # radius=self.phhp_radius,            # 1.0
+                        # dr=self.phhp_dr-self.phhp_radius,   # 0.075
+                        # k_begin=self.phhp_k_begin,          # 0.2
+                        # k_end=self.phhp_k_end               # 0.8
                     )
             except IndexError as e:
                 # No conflict!
@@ -157,8 +182,8 @@ class BWIbot:
 
     def generate_vo(self, center, radius, dr, k_begin, k_end):
         dist = np.cumsum(np.linalg.norm( center[1:] - center[:-1], axis=1))
-        d_begin = dist[-1] * k_begin
-        d_end   = dist[-1] * k_end
+        d_begin = max(dist[-1] * k_begin, dist[0]  + 2.0)
+        d_end   = min(dist[-1] * k_end,   dist[-1] - 2.0)
         idx_begin, idx_end = np.searchsorted(dist, [d_begin, d_end])
 
         # calculate center of virtual circles
@@ -167,13 +192,13 @@ class BWIbot:
         idx = slice(idx_begin, idx_end, 4)
         center = center[idx]
         theta = theta[idx]
-        vos = center + (radius + dr) * np.array([np.cos(theta), np.sin(theta)]).T
+        vos = center + dr * np.array([np.cos(theta), np.sin(theta)]).T
 
         msg = PolygonStamped()
         msg.header.stamp = rospy.Time.now() + rospy.Duration(9999.9)
         msg.polygon.points = [Point32(x, y, radius) for (x, y) in vos]
         self.__pub_hallucination.publish(msg)
-        self.phhp = True
+        self.vo_installed = True
 
 # Later, changed with LSTM?
 # Use corridor-side door location (?00)
@@ -211,4 +236,5 @@ if __name__ == '__main__':
             rospy.sleep(5.0)
     finally:
         # wrapup
-        robot.rec.bag.close()
+        # robot.rec.bag.close()
+        pass