diff --git a/bwi_tasks/scripts/visit_door_list_phhp b/bwi_tasks/scripts/visit_door_list_phhp index ac650075..f733ed62 100644 --- a/bwi_tasks/scripts/visit_door_list_phhp +++ b/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