Skip to content

Commit

Permalink
rolled back to right lane following for visit door list phhp
Browse files Browse the repository at this point in the history
  • Loading branch information
@nikunjparmar828
nikunjparmar828 committed Aug 12, 2024
2 parents 3679bf2 + 3048da9 commit 8212124
Showing 1 changed file with 59 additions and 88 deletions.
147 changes: 59 additions & 88 deletions bwi_tasks/scripts/visit_door_list_phhp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +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

#Nikunj
from actionlib_msgs.msg import GoalStatusArray
from std_msgs.msg import String

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
Expand Down Expand Up @@ -48,23 +55,16 @@ 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)

# Nikunj
self.is_wifi_on = "False"

# Define move_base
self.move_base = SimpleActionClient("/move_base", MoveBaseAction)
connected = self.move_base.wait_for_server(timeout=rospy.Duration(60.0))
Expand All @@ -80,24 +80,32 @@ 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)

# Nikunj
self.robot_moving = False
self.was_stopped = False
self.__sub_move_base_status = rospy.Subscriber('/move_base/status', GoalStatusArray, self.move_base_status_cb)
self.__sub_wifi_condition = rospy.Subscriber("/iswifion", String, self.wifi_cb)

# Collision handeling
self.in_collision = str(False)
self.__sub_collision_status = rospy.Subscriber("/v2collision", String, self.collision_cb)

def amcl_cb(self, amcl):
# 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.data in [str(True), str(False)]:
raise ValueError("{} is not a valid /iswifion msg. Please check $comms_log_pkg.".format(wifi_msg))
wifi_connection = wifi_msg.data

# Convert mode when wifi status is changed
if self.mode=="baseline" and wifi_connection == str(True):
# Use PHHP mode and remove all virtual obstacles placed during the baseline mode
self.clear_hallucination()
rospy.sleep(0.5) # Make sure vos are completely removed.
self.mode = "phhp"
self.vo_installed = False
elif self.mode=="phhp" and wifi_connection == str(False):
# Use baseline mode
self.mode = "baseline"
self.clear_costmaps()

def clear_costmaps(self):
rospy.wait_for_service("/move_base/clear_costmaps")
Expand All @@ -113,14 +121,15 @@ class BWIbot:
except rospy.ServiceException as e:
raise ConnectionError("clear_hallucination does not respond.")

def send_goal_and_wait(self, x, y, yaw):
def send_goal_and_wait(self, x, y, yaw):
self.goal.target_pose.header.stamp = rospy.Time.now()
self.goal.target_pose.pose.position.x = x
self.goal.target_pose.pose.position.y = y
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,
Expand All @@ -136,20 +145,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!
Expand All @@ -174,8 +184,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
Expand All @@ -184,44 +194,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

# Nikunj
def move_base_status_cb(self, msg):
self.robot_moving = any(goal_status.status == 1 for goal_status in msg.status_list)

# Nikunj
def wifi_cb(self, wifi_msg):
if not wifi_msg.data in [str(True), str(False)]:
raise ValueError("{} is not a valid /iswifion msg. Please check $comms_log_pkg.".format(wifi_msg))
self.is_wifi_on = wifi_msg.data

if self.is_wifi_on == str(False) and self.robot_moving:
print("Canceling the goal")
self.move_base.cancel_goal()
self.was_stopped = True

if self.is_wifi_on == str(True) and self.robot_moving == False and self.was_stopped:
self.was_stopped = False

def collision_cb(self, collision_msg):
if not collision_msg.data in [str(True), str(False)]:
raise ValueError("{} is not a valid /v2collision msg. Please check $Collision_monitor package.".format(collision_msg))
# self.is_wifi_on = wifi_msg.data

if collision_msg.data == str(True):
self.in_collision = collision_msg.data
print("Canceling the goal due to collision")
self.move_base.cancel_goal()



self.vo_installed = True

# Later, changed with LSTM?
# Use corridor-side door location (?00)
Expand All @@ -246,26 +225,18 @@ if __name__ == '__main__':
# Move to random door
prev_door = None
while not rospy.is_shutdown():

# Nikunj
if robot.is_wifi_on == str(True) and robot.robot_moving == False and robot.in_collision == str(False):
next_door = random.choice( DOOR_LIST.keys() )
if prev_door == next_door:
# robot must move to somewhere
continue

prev_door = next_door
x, y, yaw = DOOR_LIST[ next_door ]
rospy.loginfo("Heading to {}".format(next_door))

robot.send_goal_and_wait(x, y, yaw)
rospy.sleep(5.0)
elif robot.is_wifi_on == str(False):
print("No WiFi connection! Waiting for WiFi...")
rospy.sleep(2.0)
else:
next_door = random.choice( DOOR_LIST.keys() )
if prev_door == next_door:
# robot must move to somewhere
continue

prev_door = next_door
x, y, yaw = DOOR_LIST[ next_door ]
rospy.loginfo("Heading to {}".format(next_door))

robot.send_goal_and_wait(x, y, yaw)
rospy.sleep(5.0)
finally:
# wrapup
robot.rec.bag.close()
# robot.rec.bag.close()
pass

0 comments on commit 8212124

Please sign in to comment.