def test_road_odometry(self): RoadOdometry() rg = make_two_lane_road() lane_0 = rg.junction(0).segment(0).lane(0) lane_1 = rg.junction(0).segment(0).lane(1) lane_pos = LanePosition(s=1., r=2., h=3.) road_pos = RoadPosition(lane=lane_0, pos=lane_pos) w = [5., 7., 9.] v = [11., 13., 15.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry( road_position=road_pos, frame_velocity=frame_velocity) self.assertEqual(road_odom.lane.id().string(), lane_0.id().string()) self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos.srh())) self.assertTrue(np.allclose( frame_velocity.get_velocity().translational(), road_odom.vel.get_velocity().translational())) lane_pos_new = LanePosition(s=10., r=20., h=30.) v_new = [42., 43., 44.] frame_velocity_new = FrameVelocity(SpatialVelocity(w=w, v=v_new)) road_odom.lane = lane_1 road_odom.pos = lane_pos_new road_odom.vel = frame_velocity_new self.assertEqual(road_odom.lane.id().string(), lane_1.id().string()) self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos_new.srh())) self.assertTrue(np.allclose( frame_velocity_new.get_velocity().translational(), road_odom.vel.get_velocity().translational()))
def test_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort(traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue( np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
def test_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort( traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue(np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue(np.allclose( frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue(np.allclose( frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
def test_frame_velocity(self): frame_velocity = FrameVelocity() self.assertTrue(isinstance(frame_velocity, BasicVector)) self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity)) self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity)) self.assertEqual(frame_velocity.size(), FrameVelocity.kSize) # - Accessors. self.assertTrue( isinstance(frame_velocity.get_velocity(), SpatialVelocity)) # - Value. self.assertTrue( np.allclose(frame_velocity.get_velocity().rotational(), 3 * [0.])) self.assertTrue( np.allclose(frame_velocity.get_velocity().translational(), 3 * [0.])) # - Mutators. w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity.set_velocity(SpatialVelocity(w=w, v=v)) self.assertTrue( np.allclose(frame_velocity.get_velocity().rotational(), w)) self.assertTrue( np.allclose(frame_velocity.get_velocity().translational(), v)) # - Ensure ordering is ((wx, wy, wz), (vx, vy, vz)) velocity_expected = np.hstack((w, v)) velocity_actual = frame_velocity.get_value() self.assertTrue(np.allclose(velocity_actual, velocity_expected)) # - Fully-parameterized constructor. frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), w)) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), v))
def test_frame_velocity(self): frame_velocity = FrameVelocity() self.assertTrue(isinstance(frame_velocity, BasicVector)) self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity)) self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity)) self.assertEqual(frame_velocity.size(), FrameVelocity.kSize) # - Accessors. self.assertTrue(isinstance( frame_velocity.get_velocity(), SpatialVelocity)) # - Value. self.assertTrue(np.allclose( frame_velocity.get_velocity().rotational(), 3 * [0.])) self.assertTrue(np.allclose( frame_velocity.get_velocity().translational(), 3 * [0.])) # - Mutators. w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity.set_velocity(SpatialVelocity(w=w, v=v)) self.assertTrue(np.allclose( frame_velocity.get_velocity().rotational(), w)) self.assertTrue(np.allclose( frame_velocity.get_velocity().translational(), v)) # - Ensure ordering is ((wx, wy, wz), (vx, vy, vz)) velocity_expected = np.hstack((w, v)) velocity_actual = frame_velocity.get_value() self.assertTrue(np.allclose(velocity_actual, velocity_expected)) # - Fully-parameterized constructor. frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) self.assertTrue(np.allclose( frame_velocity1.get_velocity().rotational(), w)) self.assertTrue(np.allclose( frame_velocity1.get_velocity().translational(), v))