Skip to content

Commit

Permalink
Merge branch 'main' into francocipollone/eval_motion_derivatives
Browse files Browse the repository at this point in the history
  • Loading branch information
@francocipollone
francocipollone committed Oct 3, 2022
2 parents fe86938 + 119fd06 commit 689611a
Show file tree
Hide file tree
Showing 3 changed files with 214 additions and 53 deletions.
53 changes: 42 additions & 11 deletions src/base/lane.cc
View file
Expand Up @@ -44,8 +44,23 @@ double Lane::do_length() const { return lane_geometry_->ArcLength(); }
maliput::api::RBounds Lane::do_lane_bounds(double s) const { return lane_geometry_->RBounds(s); }

maliput::api::RBounds Lane::do_segment_bounds(double s) const {
// TODO: Implement
MALIPUT_THROW_MESSAGE("Not implemented");
const double bound_left = ComputeDistanceToSegmentBoundary(kToLeft, s);
const double bound_right = ComputeDistanceToSegmentBoundary(kToRight, s);
const double tolerance = lane_geometry_->linear_tolerance();
return {bound_right < tolerance ? -tolerance : -bound_right, bound_left < tolerance ? tolerance : bound_left};
}

double Lane::ComputeDistanceToSegmentBoundary(bool to_left, double s) const {
const maliput::api::RBounds lane_bounds = do_lane_bounds(s);
double lane_bound = to_left ? lane_bounds.max() : -lane_bounds.min();
maliput::api::Lane const* other_lane = to_left ? this->to_left() : this->to_right();
while (other_lane != nullptr) {
const double s_other_lane = s * other_lane->length() / length();
const maliput::api::RBounds other_lane_bounds = other_lane->lane_bounds(s_other_lane);
lane_bound += other_lane_bounds.max() - other_lane_bounds.min();
other_lane = to_left ? other_lane->to_left() : other_lane->to_right();
}
return lane_bound;
}

maliput::api::HBounds Lane::do_elevation_bounds(double, double) const { return elevation_bounds_; }
Expand All @@ -64,32 +79,48 @@ maliput::api::LanePositionResult Lane::ToLanePositionBackend(const maliput::api:
return {lane_position, maliput::api::InertialPosition::FromXyz(nearest_backend_pos), distance};
}

void Lane::DoToLanePositionBackend(const maliput::math::Vector3& backend_pos, maliput::api::LanePosition* lane_position,
maliput::math::Vector3* nearest_backend_pos, double* distance) const {
maliput::api::LanePositionResult Lane::ToSegmentPositionBackend(
const maliput::api::InertialPosition& backend_pos) const {
maliput::api::LanePosition lane_position;
maliput::math::Vector3 nearest_backend_pos;
double distance{};
DoToSegmentPositionBackend(backend_pos.xyz(), &lane_position, &nearest_backend_pos, &distance);
return {lane_position, maliput::api::InertialPosition::FromXyz(nearest_backend_pos), distance};
}

void Lane::InertialToLaneSegmentPositionBackend(bool use_lane_boundaries, const maliput::math::Vector3& backend_pos,
maliput::api::LanePosition* lane_position,
maliput::math::Vector3* nearest_backend_pos, double* distance) const {
MALIPUT_THROW_UNLESS(lane_position != nullptr);
MALIPUT_THROW_UNLESS(nearest_backend_pos != nullptr);
MALIPUT_THROW_UNLESS(distance != nullptr);
// Obtains srh coordinates without saturation in r and h.
const maliput::math::Vector3 unsaturated_srh = lane_geometry_->WInverse(backend_pos);

// Saturates r coordinates to the lane bounds.
const maliput::api::RBounds r_bounds = lane_geometry_->RBounds(unsaturated_srh.x());
const double saturated_r = std::clamp(unsaturated_srh.y(), r_bounds.min(), r_bounds.max());
const maliput::api::RBounds r_bounds =
use_lane_boundaries ? lane_geometry_->RBounds(unsaturated_srh[0]) : do_segment_bounds(unsaturated_srh[0]);
const double saturated_r = std::clamp(unsaturated_srh[1], r_bounds.min(), r_bounds.max());

// Saturates h coordinates to the elevation bounds.
const maliput::api::HBounds h_bounds = do_elevation_bounds(unsaturated_srh.x(), saturated_r);
const double saturated_h = std::clamp(unsaturated_srh.z(), h_bounds.min(), h_bounds.max());
const maliput::api::HBounds h_bounds = do_elevation_bounds(unsaturated_srh[0], saturated_r);
const double saturated_h = std::clamp(unsaturated_srh[2], h_bounds.min(), h_bounds.max());

*lane_position = maliput::api::LanePosition(unsaturated_srh.x(), saturated_r, saturated_h);
*lane_position = maliput::api::LanePosition(unsaturated_srh[0], saturated_r, saturated_h);
*nearest_backend_pos = DoToBackendPosition(*lane_position);
*distance = (backend_pos - *nearest_backend_pos).norm();
}

void Lane::DoToLanePositionBackend(const maliput::math::Vector3& backend_pos, maliput::api::LanePosition* lane_position,
maliput::math::Vector3* nearest_backend_pos, double* distance) const {
InertialToLaneSegmentPositionBackend(kUseLaneBoundaries, backend_pos, lane_position, nearest_backend_pos, distance);
}

void Lane::DoToSegmentPositionBackend(const maliput::math::Vector3& backend_pos,
maliput::api::LanePosition* lane_position,
maliput::math::Vector3* nearest_backend_pos, double* distance) const {
// TODO: Implement
MALIPUT_THROW_MESSAGE("Not implemented");
InertialToLaneSegmentPositionBackend(kUseSegmentBoundaries, backend_pos, lane_position, nearest_backend_pos,
distance);
}

maliput::api::Rotation Lane::DoGetOrientation(const maliput::api::LanePosition& lane_pos) const {
Expand Down
16 changes: 16 additions & 0 deletions src/base/lane.h
View file
Expand Up @@ -55,10 +55,16 @@ class Lane : public maliput::geometry_base::Lane {
}

maliput::api::LanePositionResult ToLanePositionBackend(const maliput::api::InertialPosition& backend_pos) const;
maliput::api::LanePositionResult ToSegmentPositionBackend(const maliput::api::InertialPosition& backend_pos) const;

const geometry::LaneGeometry* lane_geometry() const { return lane_geometry_.get(); }

private:
static constexpr bool kUseLaneBoundaries{true};
static constexpr bool kUseSegmentBoundaries{!kUseLaneBoundaries};
static constexpr bool kToLeft{true};
static constexpr bool kToRight{!kToLeft};

// maliput::api::Lane private virtual method implementations.
//@{
double do_length() const override;
Expand All @@ -76,6 +82,16 @@ class Lane : public maliput::geometry_base::Lane {
const maliput::api::IsoLaneVelocity& velocity) const override;
//@}

void InertialToLaneSegmentPositionBackend(bool use_lane_boundaries, const maliput::math::Vector3& backend_pos,
maliput::api::LanePosition* lane_position,
maliput::math::Vector3* nearest_backend_pos, double* distance) const;

/// Computes distance from the centerline of the lane at certain @p s to the boundary of the segment according
/// to the @p to_left parameter.
/// To calculate this the lane bounds at s_adjacent of the adjacent lanes are used. The @p s coordinate is scaled to
/// the length of the adjacent lane.
double ComputeDistanceToSegmentBoundary(bool to_left, double s) const;

const maliput::api::HBounds elevation_bounds_;
std::unique_ptr<geometry::LaneGeometry> lane_geometry_;
};
Expand Down
198 changes: 156 additions & 42 deletions test/base/lane_test.cc
View file
Expand Up @@ -33,6 +33,7 @@
#include <maliput/api/lane.h>
#include <maliput/api/lane_data.h>
#include <maliput/common/assertion_error.h>
#include <maliput/geometry_base/segment.h>
#include <maliput/math/vector.h>
#include <maliput/test_utilities/maliput_math_compare.h>
#include <maliput/test_utilities/maliput_types_compare.h>
Expand All @@ -46,8 +47,10 @@ namespace {
using geometry::LineString3d;
using maliput::api::InertialPosition;
using maliput::api::IsoLaneVelocity;
using maliput::api::LaneId;
using maliput::api::LanePosition;
using maliput::api::LanePositionResult;
using maliput::api::RBounds;
using maliput::api::Rotation;
using maliput::api::test::IsInertialPositionClose;
using maliput::api::test::IsLanePositionClose;
Expand Down Expand Up @@ -165,51 +168,149 @@ TEST_P(LaneTest, Test) {

INSTANTIATE_TEST_CASE_P(LaneTestGroup, LaneTest, ::testing::ValuesIn(LaneTestCases()));

// left / right
using LaneLimits = std::pair<LineString3d, LineString3d>;

struct ToLaneSegmentPositionTestCase {
LineString3d left{};
LineString3d right{};
std::vector<LaneLimits> lanes;
double expected_length{};
RBounds segment_bounds{}; //-> at s=0
std::vector<InertialPosition> backend_pos{};
std::vector<LanePositionResult> expected_lane_position_result{};
std::vector<LanePositionResult> expected_segment_lane_position_result{};
};

std::vector<ToLaneSegmentPositionTestCase> ToLaneSegmentPositionTestCases() {
return {{
// Arc-like lane:
// | | --> no elevation
// __/ / --> no elevation
// __ / --> linear elevation
LineString3d{{0., 2., 0.}, {100., 2., 100.}, {200., 102., 100.}, {200., 200., 100.}} /* left*/,
LineString3d{{0., -2., 0.}, {100., -2., 100.}, {204., 102., 100.}, {204., 200., 100.}} /* right*/,
// Centerline : {0., 0., 0.}, {100., 0., 100.}, {202., 102, 100.}, {202., 200., 100.}
100. * std::sqrt(2.) + 102. * std::sqrt(2.) + 98. /* expected_length */,
{
{50., 0., 50.},
{50., 2., 50.},
{50., 10., 50.},
} /* backend_pos */,
{
// In the centerline.
{
{50. * std::sqrt(2.), 0., 0.} /* lane_position */,
{50., 0., 50.} /* nearest_position */,
0. /* distance */
},
// At the edge of the lane.
{
{50. * std::sqrt(2.), 2., 0.} /* lane_position */,
{50., 2., 50.} /* nearest_position */,
0. /* distance */
// 5-lane straight road:
// _____________
// 4_____________
// 3_____________
// 2_____________
// 1_____________
// 0_____________
//
{{
LineString3d{{0., -4., 0.}, {100., -4., 0.}} /* left */,
LineString3d{{0., -8., 0.}, {100., -8., 0.}} /* right */,
},
{
LineString3d{{0., 0., 0.}, {100., 0., 0.}} /* left */,
LineString3d{{0., -4., 0.}, {100., -4., 0.}} /* right */,
},
{
LineString3d{{0., 4., 0.}, {100., 4., 0.}} /* left */,
LineString3d{{0., 0., 0.}, {100., 0., 0.}} /* right */,
},
{
LineString3d{{0., 8., 0.}, {100., 8., 0.}} /* left */,
LineString3d{{0., 4., 0.}, {100., 4., 0.}} /* right */,
},
{
LineString3d{{0., 12., 0.}, {100., 12., 0.}} /* left */,
LineString3d{{0., 8., 0.}, {100., 8., 0.}} /* right */,
}},
100. /* expected_length */,
{-10., 10.} /* segment_bounds */,
{
// In the central lane's centerline.
{50., 2., 0.},
// In the adjacent's lane's centerline.
{50., 6., 0.},
// In the rightmost lane's right boundary.
{50., -8., 0.},
// Outside the segment boundaries.
{50., -80., 0.},
} /* backend_pos */,
{
{
{50., 0., 0.} /* lane_position */, {50., 2., 0.} /* nearest_position */, 0. /* distance */
},
{
{50., 2., 0.} /* lane_position */, {50., 4., 0.} /* nearest_position */, 2. /* distance */
},
{
{50., -2., 0.} /* lane_position */, {50., 0., 0.} /* nearest_position */, 8. /* distance */
},
{
{50., -2., 0.} /* lane_position */, {50., 0., 0.} /* nearest_position */, 80. /* distance */
},
} /* expected_lane_position_result */,
{
{
{50., 0., 0.} /* lane_position */, {50., 2., 0.} /* nearest_position */, 0. /* distance */
},
{
{50., 4., 0.} /* lane_position */, {50., 6., 0.} /* nearest_position */, 0. /* distance */
},
{
{50., -10., 0.} /* lane_position */, {50., -8., 0.} /* nearest_position */, 0. /* distance */
},
{
{50., -10., 0.} /* lane_position */, {50., -8., 0.} /* nearest_position */, 72. /* distance */
},
} /* expected_segment_lane_position_result */
},
// Outside boundary of the lane.
// Because of the scaling of the boundaries' linestring the r value is slightly different.
{
{50. * std::sqrt(2.), 2.066817, 0.} /* lane_position */,
{50., 2.066817, 50.} /* nearest_position */,
7.9331829811625667, /* distance */
},
} /* expected_lane_position_result */
}};
// Arc-like lane:
// | | --> no elevation
// __/ / --> no elevation
// __ / --> linear elevation
{{
LineString3d{{0., 2., 0.}, {100., 2., 100.}, {200., 102., 100.}, {200., 200., 100.}} /* left*/,
LineString3d{{0., -2., 0.}, {100., -2., 100.}, {204., 102., 100.}, {204., 200., 100.}} /* right*/,
// Centerline : {0., 0., 0.}, {100., 0., 100.}, {202., 102, 100.}, {202., 200., 100.}
}},
100. * std::sqrt(2.) + 102. * std::sqrt(2.) + 98. /* expected_length */,
{-2., 2.} /* segment_bounds */,
{
{50., 0., 50.},
{50., 2., 50.},
{50., 10., 50.},
} /* backend_pos */,
{
// In the centerline.
{
{50. * std::sqrt(2.), 0., 0.} /* lane_position */,
{50., 0., 50.} /* nearest_position */,
0. /* distance */
},
// At the edge of the lane.
{
{50. * std::sqrt(2.), 2., 0.} /* lane_position */,
{50., 2., 50.} /* nearest_position */,
0. /* distance */
},
// Outside boundary of the lane.
// Because of the scaling of the boundaries' linestring the r value is slightly different.
{
{50. * std::sqrt(2.), 2.066817, 0.} /* lane_position */,
{50., 2.066817, 50.} /* nearest_position */,
7.9331829811625667, /* distance */
},
} /* expected_lane_position_result */,
{
// In the centerline.
{
{50. * std::sqrt(2.), 0., 0.} /* lane_position */,
{50., 0., 50.} /* nearest_position */,
0. /* distance */
},
// At the edge of the lane.
{
{50. * std::sqrt(2.), 2., 0.} /* lane_position */,
{50., 2., 50.} /* nearest_position */,
0. /* distance */
},
// Outside boundary of the lane.
// Because of the scaling of the boundaries' linestring the r value is slightly different.
{
{50. * std::sqrt(2.), 2.066817, 0.} /* lane_position */,
{50., 2.066817, 50.} /* nearest_position */,
7.9331829811625667, /* distance */
},
} /* expected_segment_lane_position_result */
}};
}

class ToLaneSegmentPositionTest : public ::testing::TestWithParam<ToLaneSegmentPositionTestCase> {
Expand All @@ -219,21 +320,34 @@ class ToLaneSegmentPositionTest : public ::testing::TestWithParam<ToLaneSegmentP

void SetUp() override {
ASSERT_EQ(case_.backend_pos.size(), case_.expected_lane_position_result.size()) << ">>>>> Test case is ill-formed.";
for (int i = 0; i < static_cast<int>(case_.lanes.size()); ++i) {
auto lane_geometry = std::make_unique<geometry::LaneGeometry>(case_.lanes[i].first, case_.lanes[i].second,
kTolerance, kScaleLength);
auto lane = std::make_unique<Lane>(LaneId(std::to_string(i)), kHBounds, std::move(lane_geometry));
segment_.AddLane(std::move(lane));
}
// Store pointer of the lane in the middle of the segment.
// When having odd number of lanes, from the two lanes in the middle the leftmost lane is picked.
dut_ = dynamic_cast<const Lane*>(segment_.lane(segment_.num_lanes() / 2));
}

const maliput::api::LaneId kLaneId{"dut id"};
const maliput::api::HBounds kHBounds{-5., 5.};
ToLaneSegmentPositionTestCase case_ = GetParam();
std::unique_ptr<geometry::LaneGeometry> lane_geometry_ =
std::make_unique<geometry::LaneGeometry>(case_.left, case_.right, kTolerance, kScaleLength);
const Lane* dut_{nullptr};
const ToLaneSegmentPositionTestCase case_ = GetParam();
maliput::geometry_base::Segment segment_{maliput::api::SegmentId{"segment_id"}};
};

TEST_P(ToLaneSegmentPositionTest, Test) {
std::unique_ptr<Lane> dut = std::make_unique<Lane>(kLaneId, kHBounds, std::move(lane_geometry_));
EXPECT_DOUBLE_EQ(case_.expected_length, dut->length());
EXPECT_DOUBLE_EQ(case_.expected_length, dut_->length());
const RBounds seg_bounds = dut_->segment_bounds(0.);
EXPECT_DOUBLE_EQ(case_.segment_bounds.min(), seg_bounds.min());
EXPECT_DOUBLE_EQ(case_.segment_bounds.max(), seg_bounds.max());
for (std::size_t i = 0; i < case_.backend_pos.size(); ++i) {
const auto lane_position_result = dut->ToLanePositionBackend(case_.backend_pos[i]);
const LanePositionResult lane_position_result = dut_->ToLanePositionBackend(case_.backend_pos[i]);
EXPECT_TRUE(IsLanePositionResultClose(case_.expected_lane_position_result[i], lane_position_result, kTolerance));
const LanePositionResult segment_position_result = dut_->ToSegmentPositionBackend(case_.backend_pos[i]);
EXPECT_TRUE(
IsLanePositionResultClose(case_.expected_segment_lane_position_result[i], segment_position_result, kTolerance));
}
}

Expand Down

0 comments on commit 689611a

Please sign in to comment.