class TwoState(object):
"""Switches between wall following and person following if there's a person in front of it"""
def __init__(self):
rospy.init_node('follow_person')
rospy.Subscriber('/scan', LaserScan, self.process_scan)
self.scan_front = []
self.person_follower = FollowPerson(False)
self.wall_follower = WallFollow(False)
def process_scan(self, msg):
"""Processes scan data in front of it"""
self.scan_front = msg.ranges[-10:] + msg.ranges[:11]
def person_in_front(self):
"""Decides whether there is a person in front of it that it cant follow"""
not_clear = [scan != 0 and scan < 3 for scan in self.scan_front]
for bool in not_clear:
if bool == True:
return True
return False
def run(self):
"""Calls the other object's runs to switch states"""
r = rospy.Rate(5)
while not rospy.is_shutdown():
self.wall_follower.run2(True)
if self.person_in_front():
self.wall_follower.run2(False)
self.person_follower.run2(True)
else:
self.person_follower.run2(False)
self.wall_follower.run2(True)
r.sleep()
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, amcl_pose_callback)
rospy.Subscriber("/odom", Odometry, odometry_callback)
start_session_pub = rospy.Publisher('/photo/start_session', Bool, queue_size=10)
session_state_msg = Bool()
session_num_pub = rospy.Publisher('/photo/session_num', UInt32, queue_size=10)
session_num_msg = UInt32()
rack_id_pub = rospy.Publisher('/photo/rack_id', UInt32, queue_size=10)
rack_id_msg = UInt32()
navigator = GoToPose()
camera = TakePhoto()
wall = WallFollow(LEFT)
wall.stop()
for obj in dataMap:
if rospy.is_shutdown():
break
name = obj['filename']
session_num_msg = obj['session']
rack_id_msg = obj['rackid']
rospy.loginfo("Go to %s pose of rack %s", name[:-4], rack_id_msg)
if obj['rackid'] != 0:
def __init__(self):
rospy.init_node('follow_person')
rospy.Subscriber('/scan', LaserScan, self.process_scan)
self.scan_front = []
self.person_follower = FollowPerson(False)
self.wall_follower = WallFollow(False)