def test_odom(self):
origin = np.asarray([10.0, 0.0, 0.0])
inc = np.asarray([100, 0.0, 0.0])
bus = Bus(MagicMock(write=MagicMock(return_value=timedelta())))
loc = Localization({}, bus.handle('loc'))
loc.xyz = origin
tester = bus.handle('tester')
tester.register('orientation', 'odom')
bus.connect('loc.pose3d', 'tester.pose3d')
bus.connect('tester.orientation', 'loc.orientation')
bus.connect('tester.odom', 'loc.odom')
loc.start()
tester.publish('orientation', quaternion.identity())
tester.publish('odom', 0 * inc)
tester.publish('odom', 1 * inc)
tester.publish('odom', 2 * inc)
dt, channel, pose3d = tester.listen()
self.assertEqual(channel, 'pose3d')
self.assertPose3dEqual(pose3d, [list(origin), quaternion.identity()])
dt, channel, pose3d = tester.listen()
self.assertPose3dEqual(
pose3d, [list(origin + inc / 1000),
quaternion.identity()])
dt, channel, pose3d = tester.listen()
self.assertPose3dEqual(
pose3d, [list(origin + 2 * inc / 1000),
quaternion.identity()])
loc.request_stop()
loc.join()
def test_breadcrumbs_dist(self):
bus = bus=MagicMock()
bread = Breadcrumbs(bus=bus, config={'radius':10})
self.assertEqual(bread.locations, [[0, 0, 0]])
bread.on_pose3d([[1, 0, 0], quaternion.identity()])
self.assertEqual(bread.locations, [[0, 0, 0]])
bus.publish.assert_not_called()
bread.on_pose3d([[11, 0, 0], quaternion.identity()])
self.assertEqual(bread.locations, [[0, 0, 0], [11, 0, 0]])
bus.publish.assert_called()
def __init__(self, config, bus):
self.bus = bus
self.start_pose = None
self.traveled_dist = 0.0
self.time = None
self.max_speed = 0.3 #0.5 #1.0 # TODO load from config
self.max_angular_speed = math.radians(45)
self.last_position = (
0, 0, 0) # proper should be None, but we really start from zero
self.xyz = (0, 0, 0) # 3D position for mapping artifacts
self.xyz_quat = [0, 0, 0]
self.orientation = quaternion.identity()
self.yaw, self.pitch, self.roll = 0, 0, 0
self.is_moving = None # unknown
self.scan = None # I should use class Node instead
self.stat = defaultdict(int)
self.voltage = []
self.artifacts = []
self.trace = Trace()
self.collision_detector_enabled = False
self.sim_time_sec = 0
self.use_right_wall = config.get('right_wall', True)
self.local_planner = None # hack LocalPlanner()
def __init__(self, config, bus):
self.bus = bus
self.start_pose = None
self.traveled_dist = 0.0
self.time = None
self.max_speed = 0.3 #0.5 #1.0 # TODO load from config
self.max_angular_speed = math.radians(45)
self.last_position = (0, 0, 0) # proper should be None, but we really start from zero
self.xyz = (0, 0, 0) # 3D position for mapping artifacts
self.xyz_quat = [0, 0, 0]
self.orientation = quaternion.identity()
self.yaw, self.pitch, self.roll = 0, 0, 0
self.is_moving = None # unknown
self.scan = None # I should use class Node instead
self.stat = defaultdict(int)
self.voltage = []
self.artifacts = []
self.trace = Trace()
self.collision_detector_enabled = False
self.sim_time_sec = 0
self.use_right_wall = config.get('right_wall', True)
self.local_planner = None # hack LocalPlanner()
def __init__(self, config, bus):
super().__init__(config, bus)
bus.register("desired_speed", "artf_cmd", "pose2d", "pose3d",
"request_origin")
self.monitors = []
self.last_position = None
self.max_speed = 1.0 # oficial max speed is 1.5m/s
self.max_angular_speed = math.radians(60)
self.origin = None # unknown initial position
self.origin_quat = quaternion.identity()
self.yaw_offset = 0.0 # on Moon is the rover placed randomly, but orientation corresponds to IMU
self.yaw, self.pitch, self.roll = 0, 0, 0
self.xyz = (0, 0, 0) # 3D position for mapping artifacts
self.xyz_quat = [0, 0, 0] # pose3d using Quaternions
self.offset = (0, 0, 0)
self.score = None
self.orientation = None # channel data
self.last_artf = None
self.last_volatile_distance = None
self.last_vol_index = None
self.last_status_timestamp = None
self.rand = Random(0)
self.verbose = False
def test_limited_size(self):
bus = MagicMock()
bread = Breadcrumbs(bus=bus, config={'radius':10})
self.assertEqual(bread.locations, [[0, 0, 0]])
for x in range(15, 1000, 15):
bread.on_pose3d([[x, 0, 0], quaternion.identity()])
self.assertEqual(len(bread.locations), 1 + 6)
def __init__(self, bus, end_condition, world=None):
bus.register('scan', 'rot', 'orientation', 'sim_time_sec', 'origin', 'robot_name', 'pose2d', 'acc', 'artf', 'pose3d')
self.bus = bus
self.end_condition = end_condition
self.world = world if world is not None else MultiLineString()
self._handlers = collections.defaultdict(lambda: self.on_default)
self.set_handler('desired_speed', self.on_desired_speed)
self.set_handler('pose2d', self.on_pose)
self.set_handler('pose3d', self.on_pose)
self.origin = [7.0, 3.0, 0.0]
self.xyz = [0.0, 0.0, 0.0]
self.orientation = quaternion.identity()
self.speed_forward = 0.0
self.speed_angular = 0.0
self.time = datetime.timedelta()
def __init__(self, config, bus):
super().__init__(config, bus)
bus.register("desired_speed", "artf_xyz", "artf_cmd", "pose2d",
"pose3d", "driving_recovery", "request")
self.monitors = []
self.last_position = None
self.max_speed = 1.0 # oficial max speed is 1.5m/s
self.max_angular_speed = math.radians(60)
self.min_safe_dist = config.get('min_safe_dist', 2.0)
self.dangerous_dist = config.get('dangerous_dist', 1.5)
self.safety_turning_coeff = config.get('safety_turning_coeff', 0.8)
scan_subsample = config.get('scan_subsample', 1)
obstacle_influence = config.get('obstacle_influence', 0.8)
direction_adherence = math.radians(
config.get('direction_adherence', 90))
self.local_planner = LocalPlanner(
obstacle_influence=obstacle_influence,
direction_adherence=direction_adherence,
max_obstacle_distance=4.0,
scan_subsample=scan_subsample,
max_considered_obstacles=100)
self.origin = None # unknown initial position
self.origin_quat = quaternion.identity()
self.start_pose = None
self.yaw_offset = None
self.yaw, self.pitch, self.roll = 0, 0, 0
self.xyz = (0, 0, 0) # 3D position for mapping artifacts
self.xyz_quat = [0, 0, 0]
self.offset = (0, 0, 0)
self.use_gimbal = False # try to keep the camera on level as we go over obstacles
self.brakes_on = False
self.camera_change_triggered_time = None
self.camera_angle = 0.0
self.score = 0
self.current_driver = None
self.inException = False
self.last_status_timestamp = None
self.virtual_bumper = None
self.rand = Random(0)
def __init__(self, config, bus):
self.bus = bus
bus.register("desired_speed", "pose2d", "artf_xyz", "pose3d", "stdout", "request_origin")
self.start_pose = None
self.traveled_dist = 0.0
self.time = None
self.max_speed = config['max_speed']
self.max_angular_speed = math.radians(60)
self.walldist = config['walldist']
self.timeout = timedelta(seconds=config['timeout'])
self.symmetric = config['symmetric'] # is robot symmetric?
self.dangerous_dist = config.get('dangerous_dist', 0.3)
self.min_safe_dist = config.get('min_safe_dist', 0.75)
self.safety_turning_coeff = config.get('safety_turning_coeff', 0.8)
virtual_bumper_sec = config.get('virtual_bumper_sec')
self.virtual_bumper = None
if virtual_bumper_sec is not None:
virtual_bumper_radius = config.get('virtual_bumper_radius', 0.1)
self.virtual_bumper = VirtualBumper(timedelta(seconds=virtual_bumper_sec), virtual_bumper_radius)
self.last_position = (0, 0, 0) # proper should be None, but we really start from zero
self.xyz = (0, 0, 0) # 3D position for mapping artifacts
self.xyz_quat = [0, 0, 0]
self.orientation = quaternion.identity()
self.yaw, self.pitch, self.roll = 0, 0, 0
self.yaw_offset = None # not defined, use first IMU reading
self.is_moving = None # unknown
self.scan = None # I should use class Node instead
self.flipped = False # by default use only front part
self.joint_angle_rad = [] # optinal angles, needed for articulated robots flip
self.stat = defaultdict(int)
self.voltage = []
self.artifacts = []
self.trace = Trace()
self.collision_detector_enabled = False
self.sim_time_sec = 0
self.lora_cmd = None
self.emergency_stop = None
self.monitors = [] # for Emergency Stop Exception
self.use_right_wall = config['right_wall']
self.use_center = False # navigate into center area (controlled by name ending by 'C')
self.is_virtual = config.get('virtual_world', False) # workaround to handle tunnel differences
self.front_bumper = False
self.rear_bumper = False
self.last_send_time = None
self.origin = None # unknown initial position
self.origin_quat = quaternion.identity()
self.offset = (0, 0, 0)
if 'init_offset' in config:
x, y, z = [d/1000.0 for d in config['init_offset']]
self.offset = (x, y, z)
self.init_path = None
if 'init_path' in config:
pts_s = [s.split(',') for s in config['init_path'].split(';')]
self.init_path = [(float(x), float(y)) for x, y in pts_s]
self.origin_error = False
self.robot_name = None # received with origin
scan_subsample = config.get('scan_subsample', 1)
obstacle_influence = config.get('obstacle_influence', 0.8)
direction_adherence = math.radians(config.get('direction_adherence', 90))
self.local_planner = LocalPlanner(
obstacle_influence=obstacle_influence,
direction_adherence=direction_adherence,
max_obstacle_distance=2.5,
scan_subsample=scan_subsample,
max_considered_obstacles=100)
self.use_return_trace = config.get('use_return_trace', True)
self.ref_scan = None
self.pause_start_time = None
if config.get('start_paused', False):
self.pause_start_time = timedelta() # paused from the very beginning