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_pure_pursuit(self): rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pure_pursuit = PurePursuitController() context = pure_pursuit.CreateDefaultContext() output = pure_pursuit.AllocateOutput() # Fix the inputs. ld_value = framework.AbstractValue.Make( LaneDirection(lane=lane, with_s=True)) lane_index = pure_pursuit.lane_input().get_index() context.FixInputPort(lane_index, ld_value) pos = [1., 2., 3.] # An arbitrary position with the lane. pose_vector = PoseVector() pose_vector.set_translation(pos) pose_index = pure_pursuit.ego_pose_input().get_index() context.FixInputPort(pose_index, pose_vector) # Verify the inputs. pose_vector_eval = pure_pursuit.EvalVectorInput(context, pose_index) self.assertTrue(np.allclose(pose_vector.get_translation(), pose_vector_eval.get_translation())) # Verify the outputs. pure_pursuit.CalcOutput(context, output) command_index = pure_pursuit.steering_command_output().get_index() steering = output.get_vector_data(command_index) self.assertEqual(len(steering.get_value()), 1) self.assertTrue(steering.get_value() < 0.)
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. self.assertTrue(isinstance( value.get_isometry(), Isometry3)) self.assertTrue(isinstance( value.get_rotation(), Quaternion)) self.assertTrue(isinstance( value.get_translation(), np.ndarray)) # - Value. self.assertTrue(np.allclose( value.get_isometry().matrix(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = Isometry3(q, p) value.set_translation(p) value.set_rotation(q) self.assertTrue(np.allclose( value.get_isometry().matrix(), X_expected.matrix())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) self.assertTrue(np.allclose( value1.get_isometry().matrix(), X_expected.matrix()))
def test_pure_pursuit(self): rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pure_pursuit = PurePursuitController() context = pure_pursuit.CreateDefaultContext() output = pure_pursuit.AllocateOutput() # Fix the inputs. ld_value = framework.AbstractValue.Make( LaneDirection(lane=lane, with_s=True)) lane_index = pure_pursuit.lane_input().get_index() context.FixInputPort(lane_index, ld_value) pos = [1., 2., 3.] # An arbitrary position with the lane. pose_vector = PoseVector() pose_vector.set_translation(pos) pose_index = pure_pursuit.ego_pose_input().get_index() context.FixInputPort(pose_index, pose_vector) # Verify the inputs. pose_vector_eval = pure_pursuit.EvalVectorInput(context, pose_index) self.assertTrue( np.allclose(pose_vector.get_translation(), pose_vector_eval.get_translation())) # Verify the outputs. pure_pursuit.CalcOutput(context, output) command_index = pure_pursuit.steering_command_output().get_index() steering = output.get_vector_data(command_index) self.assertEqual(len(steering.get_value()), 1) self.assertTrue(steering.get_value() < 0.)
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_rotation(), Quaternion)) self.assertTrue(isinstance(value.get_translation(), np.ndarray)) # - Value. self.assertTrue( np.allclose(value.get_isometry().matrix(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = Isometry3(q, p) value.set_translation(p) value.set_rotation(q) self.assertTrue( np.allclose(value.get_isometry().matrix(), X_expected.matrix())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) self.assertTrue( np.allclose(value1.get_isometry().matrix(), X_expected.matrix()))
def test_pose_selector(self): kScanDistance = 4. rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pose0 = PoseVector() pose0.set_translation([1., 0., 0.]) pose1 = PoseVector() # N.B. Set pose1 3 meters ahead of pose0. pose1.set_translation([4., 0., 0.]) bundle = PoseBundle(num_poses=2) bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation())) bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation())) closest_pose = PoseSelector.FindSingleClosestPose( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, side=AheadOrBehind.kAhead, path_or_branches=ScanStrategy.kPath) self.assertEqual(closest_pose.odometry.lane.id().string(), lane.id().string()) self.assertTrue(closest_pose.distance == 3.) closest_pair = PoseSelector.FindClosestPair( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath) self.assertEqual( closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.) self.assertEqual( closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance, float('inf')) lane_pos = LanePosition(s=1., r=0., h=0.) road_pos = RoadPosition(lane=lane, pos=lane_pos) w = [1., 2., 3.] v = [-4., -5., -6.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry( road_position=road_pos, frame_velocity=frame_velocity) sigma_v = PoseSelector.GetSigmaVelocity(road_odom) self.assertEqual(sigma_v, v[0])
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. with catch_drake_warnings(expected_count=1): self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_transform(), RigidTransform)) self.assertTrue(isinstance( value.get_rotation(), Quaternion)) self.assertTrue(isinstance( value.get_translation(), np.ndarray)) # - Value. with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value.get_isometry().matrix(), np.eye(4, 4))) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = RigidTransform(quaternion=q, p=p) value.set_translation(p) value.set_rotation(q) with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value.get_isometry().matrix(), X_expected.GetAsMatrix4())) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value1.get_isometry().matrix(), X_expected.GetAsMatrix4())) self.assertTrue(np.allclose( value1.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4())) # Test mutation via RigidTransform p2 = [10, 20, 30] q2 = Quaternion(wxyz=normalized([0.2, 0.3, 0.5, 0.8])) X2_expected = RigidTransform(quaternion=q2, p=p2) value.set_transform(X2_expected) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), X2_expected.GetAsMatrix4()))