class CollisionMock():
def __init__(self, obstacles):
self.obstacles = obstacles
self.current_pos = None
self.collision_publisher = CollisionPublisher()
rospy.loginfo('CollisionMock: initializing with obstacles:\n %s' % self.obstacles)
def setup_odom_callback(self):
def odom_callback(data):
self.current_pos = data.pose.pose.position
return odom_callback
def check_for_collision(self):
if self.current_pos != None:
for obstacle in obstacles:
if (obstacle.type == 'C' and
math.fabs(self.current_pos.x-obstacle.coordinate.x) < settings.COLLISION_THRESHOLD and
math.fabs(self.current_pos.y-obstacle.coordinate.y) < settings.COLLISION_THRESHOLD):
rospy.loginfo('COLLISION!')
self.collision_publisher.publish_collision(True, False)
else:
self.collision_publisher.publish_collision(False, False)
else:
rospy.loginfo('Waiting for current position')
def __init__(self, obstacles):
self.obstacles = obstacles
self.current_pos = None
self.collision_publisher = CollisionPublisher()
rospy.loginfo('CollisionMock: initializing with obstacles:\n %s' % self.obstacles)
