diff --git a/drake/automotive/BUILD b/drake/automotive/BUILD index 53de469abb22..bac4f49736b9 100644 --- a/drake/automotive/BUILD +++ b/drake/automotive/BUILD @@ -189,6 +189,15 @@ drake_cc_library( ], ) +drake_cc_library( + name = "lane_direction", + srcs = ["lane_direction.cc"], + hdrs = ["lane_direction.h"], + deps = [ + "//drake/automotive/maliput/api", + ], +) + drake_cc_library( name = "linear_car", srcs = ["linear_car.cc"], @@ -206,6 +215,7 @@ drake_cc_library( deps = [ ":calc_smooth_acceleration", ":generated_vectors", + ":lane_direction", "//drake/automotive/maliput/api", "//drake/math:geometric_transform", "//drake/systems/rendering:pose_vector", @@ -251,6 +261,22 @@ drake_cc_library( ], ) +drake_cc_library( + name = "road_path", + srcs = ["road_path.cc"], + hdrs = ["road_path.h"], + visibility = [], + deps = [ + ":lane_direction", + ":monolane_onramp_merge", + "//drake/automotive/maliput/api", + "//drake/common", + "//drake/common/trajectories:piecewise_polynomial", + "//drake/common/trajectories/qp_spline:spline_generation", + "//drake/math:saturate", + ], +) + drake_cc_library( name = "simple_car", srcs = ["simple_car.cc"], @@ -707,6 +733,16 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "road_path_test", + deps = [ + ":road_path", + "//drake/automotive/maliput/dragway", + "//drake/automotive/maliput/monolane", + "//drake/common:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "simple_powertrain_test", deps = [ diff --git a/drake/automotive/CMakeLists.txt b/drake/automotive/CMakeLists.txt index 2df8a61dd435..13538ad3d19c 100644 --- a/drake/automotive/CMakeLists.txt +++ b/drake/automotive/CMakeLists.txt @@ -49,6 +49,7 @@ add_library_with_exports(LIB_NAME drakeAutomotive SOURCE_FILES maliput_railcar.cc monolane_onramp_merge.cc pose_selector.cc + road_path.cc simple_car.cc simple_powertrain.cc single_lane_ego_and_agent.cc @@ -75,6 +76,7 @@ drake_install_headers( linear_car.h maliput_railcar.h monolane_onramp_merge.h + road_path.h pose_selector.h simple_car.h simple_car_to_euler_floating_joint.h diff --git a/drake/automotive/lane_direction.cc b/drake/automotive/lane_direction.cc new file mode 100644 index 000000000000..757fa3782fcb --- /dev/null +++ b/drake/automotive/lane_direction.cc @@ -0,0 +1,4 @@ +#include "drake/automotive/lane_direction.h" + +// For now, this is an empty .cc file that only serves to confirm that +// our .h file is a stand-alone header. diff --git a/drake/automotive/lane_direction.h b/drake/automotive/lane_direction.h new file mode 100644 index 000000000000..732d91575412 --- /dev/null +++ b/drake/automotive/lane_direction.h @@ -0,0 +1,35 @@ +#pragma once + +#include + +#include "drake/automotive/maliput/api/lane.h" + +namespace drake { +namespace automotive { + +/// LaneDirection holds the lane that a MaliputRailcar is traversing and the +/// direction in which it is moving. A MaliputRailcar can either travel in the +/// increasing-`s` direction or in the decreasing-`s` direction. +struct LaneDirection { + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(LaneDirection) + + /// Default constructor. + LaneDirection() {} + + /// A constructor that sets `with_s` to be `true`. + explicit LaneDirection(const maliput::api::Lane* lane_input) + : LaneDirection(lane_input, true) {} + + /// Fully parameterized constructor. + LaneDirection(const maliput::api::Lane* lane_input, bool with_s_input) + : lane(lane_input), with_s(with_s_input) {} + + const maliput::api::Lane* lane{nullptr}; + + /// True means that the MaliputRailcar's `s` coordinate increases when the + /// vehicle has positive speed. False means the opposite. + bool with_s{true}; +}; + +} // namespace automotive +} // namespace drake diff --git a/drake/automotive/maliput_railcar.h b/drake/automotive/maliput_railcar.h index 10170b15ffa5..a6c335ef4320 100644 --- a/drake/automotive/maliput_railcar.h +++ b/drake/automotive/maliput_railcar.h @@ -4,6 +4,7 @@ #include "drake/automotive/gen/maliput_railcar_params.h" #include "drake/automotive/gen/maliput_railcar_state.h" +#include "drake/automotive/lane_direction.h" #include "drake/automotive/maliput/api/lane.h" #include "drake/common/drake_copyable.h" #include "drake/systems/framework/leaf_system.h" @@ -13,28 +14,6 @@ namespace drake { namespace automotive { -/// LaneDirection holds the lane that a MaliputRailcar is traversing and the -/// direction in which it is moving. A MaliputRailcar can either travel in the -/// increasing-`s` direction or in the decreasing-`s` direction. -struct LaneDirection { - /// Default constructor. - LaneDirection() {} - - /// A constructor that sets `with_s` to be `true`. - explicit LaneDirection(const maliput::api::Lane* lane_input) - : LaneDirection(lane_input, true) {} - - /// Fully parameterized constructor. - LaneDirection(const maliput::api::Lane* lane_input, bool with_s_input) - : lane(lane_input), with_s(with_s_input) {} - - const maliput::api::Lane* lane{nullptr}; - - /// True means that the MaliputRailcar's `s` coordinate increases when the - /// vehicle has positive speed. False means the opposite. - bool with_s{true}; -}; - /// MaliputRailcar models a vehicle that follows a maliput::api::Lane as if it /// were on rails and neglecting all physics. /// diff --git a/drake/automotive/road_path.cc b/drake/automotive/road_path.cc new file mode 100644 index 000000000000..e12967a00b1c --- /dev/null +++ b/drake/automotive/road_path.cc @@ -0,0 +1,108 @@ +#include "drake/automotive/road_path.h" + +#include + +#include "drake/automotive/maliput/api/branch_point.h" +#include "drake/automotive/maliput/api/lane.h" +#include "drake/automotive/maliput/api/lane_data.h" +#include "drake/common/cond.h" +#include "drake/common/drake_assert.h" +#include "drake/math/saturate.h" + +namespace drake { +namespace automotive { + +using maliput::api::GeoPosition; +using maliput::api::Lane; +using maliput::api::LaneEnd; +using maliput::api::LaneEndSet; +using maliput::api::RoadGeometry; + +template +RoadPath::RoadPath(const LaneDirection& initial_lane_direction, + const T& step_size, int num_breaks) + : path_(MakePiecewisePolynomial(initial_lane_direction, step_size, + num_breaks)), + path_prime_(path_.derivative(1 /* 1st derivative */)), + path_double_prime_(path_.derivative(2 /* 2nd derivative */)) {} + +template +RoadPath::~RoadPath() {} + +template +const PiecewisePolynomial& RoadPath::get_path() const { + return path_; +} + +template +const T RoadPath::GetClosestPathPosition(const Vector3& geo_pos, + const T& s_guess) const { + DRAKE_ABORT(); +} + +template +const PiecewisePolynomial RoadPath::MakePiecewisePolynomial( + const LaneDirection& initial_lane_direction, const T& step_size, + int num_breaks) const { + std::vector s_breaks{}; + std::vector> geo_knots(num_breaks, MatrixX::Zero(3, 1)); + + LaneDirection ld = initial_lane_direction; + T s_lane{cond(ld.with_s, T(0.), T(ld.lane->length()))}; + T s_break{0.}; + + // Loop over all the breaks and extract the knot points. + for (int i = 0; i < num_breaks - 1; ++i) { + s_breaks.emplace_back(s_break); + s_break += T(step_size); + + GeoPosition geo_pos = + ld.lane->ToGeoPosition({s_lane /* s */, 0. /* r */, 0. /* h */}); + geo_knots[i] << T(geo_pos.x), T(geo_pos.y), T(geo_pos.z); + + // Take a step. + if (ld.with_s) { + s_lane += T(step_size); + } else { + s_lane -= T(step_size); + } + + // Compute the distance in excess of the lane boundary by taking this step. + const T out_distance{ + cond(ld.with_s, T(s_lane - ld.lane->length()), T(-s_lane))}; + if (out_distance >= 0.) { + const LaneEndSet* lane_end_set{ + ld.with_s ? ld.lane->GetOngoingBranches(LaneEnd::kFinish) + : ld.lane->GetOngoingBranches(LaneEnd::kStart)}; + if (lane_end_set->size() == 0) { // There are no more ongoing lanes. + // If needed, add another knot point to make up the remaining distance. + if (out_distance != 0.) { + s_breaks.emplace_back(s_break + T(step_size - out_distance)); + s_lane = cond(ld.with_s, T(ld.lane->length()), T(0.)); + geo_pos = + ld.lane->ToGeoPosition({s_lane /* s */, 0. /* r */, 0. /* h */}); + geo_knots[i + 1] << T(geo_pos.x), T(geo_pos.y), T(geo_pos.z); + } + break; + } + + // Always choose the first lane in the set as the new lane. + ld.lane = lane_end_set->get(0).lane; + ld.with_s = (lane_end_set->get(0).end == LaneEnd::kStart) ? true : false; + + // Correct for the distance overshoot. + s_lane = + cond(ld.with_s, out_distance, T(ld.lane->length()) - out_distance); + } + } + // Resize the vector of knot points, if necessary. + geo_knots.resize(s_breaks.size()); + + // Create the resulting piecewise polynomial. + return PiecewisePolynomial::Cubic(s_breaks, geo_knots); +} + +template class RoadPath; + +} // namespace automotive +} // namespace drake diff --git a/drake/automotive/road_path.h b/drake/automotive/road_path.h new file mode 100644 index 000000000000..5aa6b31f0e99 --- /dev/null +++ b/drake/automotive/road_path.h @@ -0,0 +1,72 @@ +#pragma once + +#include "drake/automotive/lane_direction.h" +#include "drake/automotive/maliput/api/road_geometry.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/trajectories/piecewise_polynomial.h" + +namespace drake { +namespace automotive { + +/// RoadPath converts a sequence of Maliput Lanes into a PiecewisePolynomial for +/// the purpose of generating a path for a car to follow. The path is created +/// from the start of a user-specified initial lane and direction of travel, and +/// proceeds in that direction until either the end of the road is reached +/// (there exist no ongoing lanes), or the given number of specified breaks have +/// been traversed. The resulting path is a cubic spline that matches the `r = +/// 0` coordinate of the lanes at each specified break point. The resulting +/// piecewise curve is C2-continuous throughout with zero first and second +/// derivatives at the start and end of the path. +/// +/// This class is explicitly instantiated for the following scalar types. No +/// other scalar types are supported. +/// - double +/// +/// @tparam T The vector element type, which must be a valid Eigen scalar. +/// Only double is supported. +template +class RoadPath { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RoadPath) + + /// Constructs a single RoadPath from a sequence of Maliput lanes based on the + /// following parameters: + /// @param initial_lane_direction contains the initial LaneDirection. This + /// must be a valid @p road Lane. + /// @param step_size is the size of each step (in the `s`-direction) between + /// knot points. + /// @param num_breaks are the number of breaks at which the knot points will + /// be evaluated. + RoadPath(const LaneDirection& initial_lane_direction, const T& step_size, + int num_breaks); + ~RoadPath(); + + const PiecewisePolynomial& get_path() const; + + /// Computes the closest `s`-position on the path to an arbitrary point in + /// the world frame of the provided Maliput Lanes. (Not yet implemented) + // TODO(jadecastro): Implement this. + const T GetClosestPathPosition(const Vector3& geo_position, + const T& s_guess) const; + + private: + // Traverse the road, starting from an initial LaneDirection, and build a + // cubic spline PiecewisePolynomial until 1) a given a number of segments has + // been traversed, or 2) the end of the road has been reached. + // + // If a BranchPoint is encountered in which there is more than one ongoing + // lane, the zero-index lane is always selected. + // TODO(jadecastro): Use Lane::GetDefaultBranch() to decide the ongoing Lane. + const PiecewisePolynomial MakePiecewisePolynomial( + const LaneDirection& initial_lane_direction, const T& step_size, + int num_breaks) const; + + PiecewisePolynomial path_; // The path representing the mid-curve of the + // road. + PiecewisePolynomial path_prime_; // First derivative of path_. + PiecewisePolynomial path_double_prime_; // Second derivative of path_. +}; + +} // namespace automotive +} // namespace drake diff --git a/drake/automotive/test/CMakeLists.txt b/drake/automotive/test/CMakeLists.txt index 78d6e227a2fd..ef114cfac09f 100644 --- a/drake/automotive/test/CMakeLists.txt +++ b/drake/automotive/test/CMakeLists.txt @@ -37,6 +37,9 @@ target_link_libraries(pose_selector_test drakeAutomotive) drake_add_cc_test(idm_controller_test) target_link_libraries(idm_controller_test drakeAutomotive) +drake_add_cc_test(road_path_test) +target_link_libraries(road_path_test drakeAutomotive) + if(lcm_FOUND) drake_add_cc_test(automotive_simulator_test) target_link_libraries(automotive_simulator_test drakeAutomotiveLcm) diff --git a/drake/automotive/test/road_path_test.cc b/drake/automotive/test/road_path_test.cc new file mode 100644 index 000000000000..6d516699a661 --- /dev/null +++ b/drake/automotive/test/road_path_test.cc @@ -0,0 +1,185 @@ +#include "drake/automotive/road_path.h" + +#include +#include + +#include "gtest/gtest.h" + +#include "drake/automotive/maliput/api/lane.h" +#include "drake/automotive/maliput/api/lane_data.h" +#include "drake/automotive/maliput/api/road_geometry.h" +#include "drake/automotive/maliput/dragway/road_geometry.h" +#include "drake/automotive/maliput/monolane/builder.h" +#include "drake/automotive/monolane_onramp_merge.h" +#include "drake/common/eigen_matrix_compare.h" + +namespace drake { +namespace automotive { +namespace { + +using maliput::api::GeoPosition; +using maliput::api::Lane; +using maliput::api::LaneEnd; +using maliput::api::LanePosition; +using maliput::api::RoadGeometry; +using maliput::monolane::Builder; +using maliput::monolane::Connection; +using maliput::monolane::Endpoint; +using maliput::monolane::EndpointZ; + +// The length of the straight lane segment. +const double kStraightRoadLength{10}; + +// The arc radius, angular displacement, and length of the curved road segment. +const double kCurvedRoadRadius{10}; +const double kCurvedRoadTheta{M_PI_2}; +const double kCurvedRoadLength{kCurvedRoadRadius * M_PI_2}; + +const double kTotalRoadLength{kStraightRoadLength + kCurvedRoadLength}; +const EndpointZ kEndZ{0, 0, 0, 0}; // Specifies zero elevation/super-elevation. + +// Build a road with two lanes in series. +std::unique_ptr MakeTwoLaneRoad(bool is_opposing) { + Builder builder(maliput::api::RBounds(-2, 2), /* lane_bounds */ + maliput::api::RBounds(-4, 4), /* driveable_bounds */ + 0.01, /* linear tolerance */ + M_PI_2 / 180.0); /* angular_tolerance */ + builder.Connect("0_fwd", /* id */ + Endpoint({0, 0, 0}, kEndZ), /* start */ + kStraightRoadLength, /* length */ + kEndZ); /* z_end */ + + if (is_opposing) { + // Construct a curved segment that is directionally opposite the straight + // lane. + builder.Connect("1_rev", /* id */ + Endpoint({kStraightRoadLength + kCurvedRoadRadius, + kCurvedRoadRadius, 1.5 * M_PI}, + kEndZ), /* start */ + {kCurvedRoadRadius, -kCurvedRoadTheta}, /* arc */ + kEndZ); /* z_end */ + } else { + // Construct a curved segment that is directionally confluent with the + // straight lane. + builder.Connect("1_fwd", /* id */ + Endpoint({kStraightRoadLength, 0, 0}, kEndZ), /* start */ + {kCurvedRoadRadius, kCurvedRoadTheta}, /* arc */ + kEndZ); /* z_end */ + } + + return builder.Build(maliput::api::RoadGeometryId({"TwoLaneStretchOfRoad"})); +} + +const Lane* GetLaneById(const RoadGeometry& road, const std::string& lane_id) { + for (int i = 0; i < road.num_junctions(); ++i) { + if (road.junction(i)->id().id == lane_id) { + return road.junction(i)->segment(0)->lane(0); + } + } + throw std::runtime_error("No matching junction name in the road network"); +} + +static double path_radius(const Vector3 value) { + Vector3 result; + result << value(0) - kStraightRoadLength, value(1) - kCurvedRoadRadius, + value(2); + return result.norm(); +} + +// Tests the constructor given a sufficient number of points. +GTEST_TEST(IdmControllerTest, ConstructOpposingSegments) { + const double kStepSize{0.5}; + // Instantiate a road with opposing segments. + auto road_opposing = MakeTwoLaneRoad(true); + // Start in the straight segment and progress in the positive-s-direction. + const LaneDirection initial_lane_dir = + LaneDirection(GetLaneById(*road_opposing, "j:0_fwd"), /* lane */ + true); /* with_s */ + // Create a finely-discretized path with a sufficent number of segments to + // cover the full length. + const auto path = + RoadPath(initial_lane_dir, /* initial_lane_direction */ + kStepSize, /* step_size */ + 100); /* num_breaks */ + ASSERT_LE(kTotalRoadLength, + path.get_path().getEndTime() - path.get_path().getStartTime()); + + // Expect the lane boundary values to match. + Vector3 expected_value{}; + Vector3 actual_value{}; + expected_value << 0., 0., 0.; + actual_value = path.get_path().value(0.); + EXPECT_TRUE(CompareMatrices(expected_value, actual_value, 1e-3)); + // N.B. Using tolerance of 1e-3 to account for possible interpolation errors. + + // Derive s-position of the straight road segment from the number of break + // point steps taken to reach kStraightRoadLength from the end of the road. + const double straight_length{ + path.get_path().getStartTime(std::ceil(kStraightRoadLength / kStepSize))}; + expected_value << 10., 0., 0.; + actual_value = path.get_path().value(straight_length); + EXPECT_TRUE(CompareMatrices(expected_value, actual_value, 1e-3)); + + const double total_length{path.get_path().getEndTime()}; + expected_value << 20., 10., 0.; + actual_value = path.get_path().value(total_length); + EXPECT_TRUE(CompareMatrices(expected_value, actual_value, 1e-3)); + + // Pick a few arbitrary points on the curved section, expect them to trace the + // arc, hence demonstrating the interpolation is working. + actual_value = path.get_path().value(4. / 7. * kTotalRoadLength); + EXPECT_NEAR(kCurvedRoadRadius, path_radius(actual_value), 1e-3); + + actual_value = path.get_path().value(5. / 7. * kTotalRoadLength); + EXPECT_NEAR(kCurvedRoadRadius, path_radius(actual_value), 1e-3); + + actual_value = path.get_path().value(6. / 7. * kTotalRoadLength); + EXPECT_NEAR(kCurvedRoadRadius, path_radius(actual_value), 1e-3); + + // Check that the number of segments created is well below the max + // number specified. + EXPECT_GT(1000, path.get_path().getNumberOfSegments()); +} + +GTEST_TEST(IdmControllerTest, ConstructConfluentSegments) { + const double kStepSize{0.5}; + // Instantiate a road with confluent segments. + auto road_confluent = MakeTwoLaneRoad(false); + // Start in the curved segment, and progress in the negative-s-direction. + const LaneDirection initial_lane_dir = + LaneDirection(GetLaneById(*road_confluent, "j:1_fwd"), /* lane */ + false); /* with_s */ + // Create a finely-discretized path with a sufficent number of segments to + // cover the full length. + const auto path = + RoadPath(initial_lane_dir, /* initial_lane_direction */ + kStepSize, /* step_size */ + 100); /* num_breaks */ + ASSERT_LE(kTotalRoadLength, + path.get_path().getEndTime() - path.get_path().getStartTime()); + + // Expect the lane boundary values to match. + Vector3 expected_value{}; + Vector3 actual_value{}; + expected_value << 20., 10., 0.; + actual_value = path.get_path().value(0.); + EXPECT_TRUE(CompareMatrices(expected_value, actual_value, 1e-3)); + + double total_length{path.get_path().getEndTime()}; + // Derive s-position of the straight road segment from the number of break + // point steps taken to reach kStraightRoadLength from the start of the road. + double straight_length{ + path.get_path().getEndTime(std::ceil(kStraightRoadLength / kStepSize))}; + double curved_length{total_length - straight_length}; + expected_value << 10., 0., 0.; + actual_value = path.get_path().value(curved_length); + EXPECT_TRUE(CompareMatrices(expected_value, actual_value, 1e-3)); + + expected_value << 0., 0., 0.; + actual_value = path.get_path().value(total_length); + EXPECT_TRUE(CompareMatrices(expected_value, actual_value, 1e-3)); +} + +} // namespace +} // namespace automotive +} // namespace drake