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)