diff --git a/modules/planning/tasks/rss/BUILD b/modules/planning/tasks/rss/BUILD index b44fe9e2734..c4839e94eb1 100644 --- a/modules/planning/tasks/rss/BUILD +++ b/modules/planning/tasks/rss/BUILD @@ -18,7 +18,7 @@ cc_library( "//modules/planning/common:reference_line_info", "//modules/planning/proto:planning_proto", "//modules/planning/tasks:task", - "//third_party/rss", + "@ad_rss_lib//:ad_rss", ], ) @@ -30,7 +30,7 @@ cc_test( ], deps = [ ":decider_rss", - "//third_party/rss", + "@ad_rss_lib//:ad_rss", "@gtest//:main", ], ) diff --git a/modules/planning/tasks/rss/decider_rss.cc b/modules/planning/tasks/rss/decider_rss.cc index 93b4f233aca..ee262eb0c9c 100644 --- a/modules/planning/tasks/rss/decider_rss.cc +++ b/modules/planning/tasks/rss/decider_rss.cc @@ -28,6 +28,13 @@ namespace planning { using apollo::common::ErrorCode; using apollo::common::Status; +using apollo::common::VehicleConfigHelper; + +using ad_rss::world::Object; +using ad_rss::world::RoadArea; +using ad_rss::world::Scene; +using ad_rss::physics::Distance; +using ad_rss::physics::ParametricValue; RssDecider::RssDecider(const TaskConfig &config) : Task(config) { SetName("RssDecider"); @@ -125,94 +132,104 @@ Status RssDecider::Process(Frame *frame, << " left_width_obs = " << left_width_obs << " lane_length = " << total_lane_length; - rss::world::Object followingObject; - rss::world::Object leadingObject; - rss::world::RoadArea roadArea; - rss::world::Scene scene; + Object followingObject; + Object leadingObject; + RoadArea roadArea; + Scene scene; - scene.setSituationType(rss::situation::SituationType::SameDirection); + scene.situationType = ad_rss::situation::SituationType::SameDirection; leadingObject = - rss::createObject(nearest_obs_speed, 0.0); // TODO(l1212s): refine + ad_rss::createObject(nearest_obs_speed, 0.0); // TODO(l1212s): refine leadingObject.objectId = 0; - rss::world::OccupiedRegion occupiedRegion_leading; + ad_rss::world::OccupiedRegion occupiedRegion_leading; occupiedRegion_leading.segmentId = 0; occupiedRegion_leading.lonRange.minimum = - nearest_obs_s_start / total_lane_length; + ParametricValue(nearest_obs_s_start / total_lane_length); occupiedRegion_leading.lonRange.maximum = - nearest_obs_s_end / total_lane_length; - occupiedRegion_leading.latRange.minimum = std::max( - (left_width_obs - nearest_obs_l_end) / total_lane_width_obs, 0.0); - occupiedRegion_leading.latRange.maximum = std::min( - (left_width_obs - nearest_obs_l_start) / total_lane_width_obs, 1.0); + ParametricValue(nearest_obs_s_end / total_lane_length); + occupiedRegion_leading.latRange.minimum = ParametricValue( + std::max((left_width_obs - nearest_obs_l_end) / + total_lane_width_obs, 0.0)); + occupiedRegion_leading.latRange.maximum = ParametricValue( + std::min((left_width_obs - nearest_obs_l_start) / + total_lane_width_obs, 1.0)); leadingObject.occupiedRegions.push_back(occupiedRegion_leading); ADEBUG << " occupiedRegion_leading.lonRange.minimum = " - << occupiedRegion_leading.lonRange.minimum + << double(occupiedRegion_leading.lonRange.minimum) << " occupiedRegion_leading.lonRange.maximum = " - << occupiedRegion_leading.lonRange.maximum + << double(occupiedRegion_leading.lonRange.maximum) << " occupiedRegion_leading.latRange.minimum = " - << occupiedRegion_leading.latRange.minimum + << double(occupiedRegion_leading.latRange.minimum) << " occupiedRegion_leading.latRange.maximum = " - << occupiedRegion_leading.latRange.maximum; + << double(occupiedRegion_leading.latRange.maximum); followingObject = - rss::createObject(adc_velocity, 0.0); // TODO(l1212s): refine + ad_rss::createObject(adc_velocity, 0.0); // TODO(l1212s): refine followingObject.objectId = 1; - rss::world::OccupiedRegion occupiedRegion_following; + ad_rss::world::OccupiedRegion occupiedRegion_following; occupiedRegion_following.segmentId = 0; - occupiedRegion_following.lonRange.minimum = ego_v_s_start / total_lane_length; - occupiedRegion_following.lonRange.maximum = ego_v_s_end / total_lane_length; + occupiedRegion_following.lonRange.minimum = + ParametricValue(ego_v_s_start / total_lane_length); + occupiedRegion_following.lonRange.maximum = + ParametricValue(ego_v_s_end / total_lane_length); occupiedRegion_following.latRange.minimum = - (left_width_ego - ego_v_l_end) / total_lane_width_ego; + ParametricValue((left_width_ego - ego_v_l_end) / total_lane_width_ego); occupiedRegion_following.latRange.maximum = - (left_width_ego - ego_v_l_start) / total_lane_width_ego; + ParametricValue((left_width_ego - ego_v_l_start) / total_lane_width_ego); followingObject.occupiedRegions.push_back(occupiedRegion_following); ADEBUG << " adc_velocity = " << adc_velocity << " occupiedRegion_following.lonRange.minimum = " - << occupiedRegion_following.lonRange.minimum + << double(occupiedRegion_following.lonRange.minimum) << " occupiedRegion_following.lonRange.maximum = " - << occupiedRegion_following.lonRange.maximum + << double(occupiedRegion_following.lonRange.maximum) << " occupiedRegion_following.latRange.minimum = " - << occupiedRegion_following.latRange.minimum + << double(occupiedRegion_following.latRange.minimum) << " occupiedRegion_following.latRange.maximum = " - << occupiedRegion_following.latRange.maximum; + << double(occupiedRegion_following.latRange.maximum); - rss::world::RoadSegment roadSegment; - rss::world::LaneSegment laneSegment; + ad_rss::world::RoadSegment roadSegment; + ad_rss::world::LaneSegment laneSegment; laneSegment.id = 0; - laneSegment.length.minimum = total_lane_length; - laneSegment.length.maximum = total_lane_length; - laneSegment.width.minimum = - std::min(total_lane_width_ego, total_lane_width_obs); - laneSegment.width.maximum = - std::max(total_lane_width_ego, total_lane_width_obs); + laneSegment.length.minimum = Distance(total_lane_length); + laneSegment.length.maximum = Distance(total_lane_length); + laneSegment.width.minimum = Distance(std::min(total_lane_width_ego, + total_lane_width_obs)); + laneSegment.width.maximum = Distance(std::max(total_lane_width_ego, + total_lane_width_obs)); roadSegment.push_back(laneSegment); roadArea.push_back(roadSegment); - ADEBUG << " laneSegment.length.min = " << laneSegment.length.minimum - << " laneSegment.length.maximum = " << laneSegment.length.maximum - << " laneSegment.width.minimum = " << laneSegment.width.minimum - << " laneSegment.width.maximum = " << laneSegment.width.maximum; - - rss::world::WorldModel worldModel; - worldModel.egoVehicle = followingObject; + ADEBUG << " laneSegment.length.min = " + << double(laneSegment.length.minimum) + << " laneSegment.length.maximum = " + << double(laneSegment.length.maximum) + << " laneSegment.width.minimum = " + << double(laneSegment.width.minimum) + << " laneSegment.width.maximum = " + << double(laneSegment.width.maximum); + + ad_rss::world::WorldModel worldModel; + worldModel.egoVehicle = ad_rss::objectAsEgo(followingObject); scene.object = leadingObject; scene.egoVehicleRoad = roadArea; worldModel.scenes.push_back(scene); worldModel.timeIndex = 1; - double dMin_lon = - rss::calculateLongitudinalMinSafeDistance(followingObject, leadingObject); - rss::world::AccelerationRestriction accelerationRestriction; - rss::core::RssCheck rssCheck; - rssCheck.calculateAccelerationRestriction(worldModel, - accelerationRestriction); + Distance dMin_lon = + ad_rss::calculateLongitudinalMinSafeDistance( + followingObject, leadingObject); + + ad_rss::world::AccelerationRestriction accelerationRestriction; + ad_rss::core::RssCheck rssCheck; + rssCheck.calculateAccelerationRestriction( + worldModel, accelerationRestriction); - if (front_obstacle_distance > dMin_lon) { + if (double(front_obstacle_distance) > double(dMin_lon)) { ADEBUG << "Task " << Name() << " Distance is RSS-Safe"; reference_line_info->mutable_rss_info()->set_is_rss_safe(true); } else { @@ -228,22 +245,23 @@ Status RssDecider::Process(Frame *frame, if (front_obstacle_distance < kTrajectoryLenFactor * reference_line_info->TrajectoryLength()) { reference_line_info->mutable_rss_info()->set_cur_dist_lon( - front_obstacle_distance); + front_obstacle_distance); } - reference_line_info->mutable_rss_info()->set_rss_safe_dist_lon(dMin_lon); + reference_line_info->mutable_rss_info()->set_rss_safe_dist_lon( + double(dMin_lon)); reference_line_info->mutable_rss_info()->set_acc_lon_range_minimum( - accelerationRestriction.longitudinalRange.minimum); + double(accelerationRestriction.longitudinalRange.minimum)); reference_line_info->mutable_rss_info()->set_acc_lon_range_maximum( - accelerationRestriction.longitudinalRange.maximum); + double(accelerationRestriction.longitudinalRange.maximum)); reference_line_info->mutable_rss_info()->set_acc_lat_left_range_minimum( - accelerationRestriction.lateralLeftRange.minimum); + double(accelerationRestriction.lateralLeftRange.minimum)); reference_line_info->mutable_rss_info()->set_acc_lat_left_range_maximum( - accelerationRestriction.lateralLeftRange.maximum); + double(accelerationRestriction.lateralLeftRange.maximum)); reference_line_info->mutable_rss_info()->set_acc_lat_right_range_minimum( - accelerationRestriction.lateralRightRange.minimum); + double(accelerationRestriction.lateralRightRange.minimum)); reference_line_info->mutable_rss_info()->set_acc_lat_right_range_maximum( - accelerationRestriction.lateralRightRange.maximum); + double(accelerationRestriction.lateralRightRange.maximum)); ADEBUG << " is_rss_safe : " << reference_line_info->rss_info().is_rss_safe(); ADEBUG << " cur_dist_lon: " << reference_line_info->rss_info().cur_dist_lon(); diff --git a/modules/planning/tasks/rss/decider_rss.h b/modules/planning/tasks/rss/decider_rss.h index afa825f81af..b8f9fc5739c 100644 --- a/modules/planning/tasks/rss/decider_rss.h +++ b/modules/planning/tasks/rss/decider_rss.h @@ -25,8 +25,8 @@ #include #include "modules/planning/tasks/task.h" -#include "rss/core/RssCheck.hpp" -#include "rss/test_support/TestSupport.hpp" +#include "ad_rss/core/RssCheck.hpp" +#include "TestSupport.hpp" namespace apollo { namespace planning { diff --git a/modules/planning/tasks/rss/decider_rss_test.cc b/modules/planning/tasks/rss/decider_rss_test.cc index d7c1884babb..d8424b8c811 100644 --- a/modules/planning/tasks/rss/decider_rss_test.cc +++ b/modules/planning/tasks/rss/decider_rss_test.cc @@ -18,51 +18,59 @@ * @file **/ -#include "rss/core/RssCheck.hpp" -#include "rss/test_support/TestSupport.hpp" -namespace rss { +#include "ad_rss/core/RssCheck.hpp" +#include "TestSupport.hpp" + +namespace ad_rss { namespace core { +using ad_rss::physics::Distance; +using ad_rss::physics::ParametricValue; +using ad_rss::world::OccupiedRegion; + class RssCheckSameDirectionTests : public testing::Test { protected: virtual void SetUp() { - scene.setSituationType(rss::situation::SituationType::SameDirection); - leadingObject = createObject(0., 0.); + scene.situationType = ad_rss::situation::SituationType::SameDirection; + // leadingObject = createObject(0., 0.); // lateral speed 0 + leadingObject = createObject(0., 0.5); // lateral speed toward right leadingObject.objectId = 0; { - ::rss::world::OccupiedRegion occupiedRegion; - occupiedRegion.lonRange.minimum = 0.94; - occupiedRegion.lonRange.maximum = 1.0; + OccupiedRegion occupiedRegion; + occupiedRegion.lonRange.minimum = ParametricValue(0.94); + occupiedRegion.lonRange.maximum = ParametricValue(1.0); occupiedRegion.segmentId = 0; - occupiedRegion.latRange.minimum = 0.; - // occupiedRegion.latRange.maximum = 0.089; // rss safe - occupiedRegion.latRange.maximum = 0.15; // rss unsafe + occupiedRegion.latRange.minimum = ParametricValue(0.); + occupiedRegion.latRange.maximum = ParametricValue(0.089); // rss safe + // occupiedRegion.latRange.maximum = ParametricValue(0.15); // rss unsafe + // occupiedRegion.latRange.maximum = ParametricValue(0.3); // rss unsafe leadingObject.occupiedRegions.push_back(occupiedRegion); } - followingObject = createObject(11.0, 0.); + // followingObject = createObject(11.0, 0.); // lateral speed 0 + followingObject = createObject(11.0, -0.5); // lateral speed toward left followingObject.objectId = 1; { - ::rss::world::OccupiedRegion occupiedRegion; - occupiedRegion.lonRange.minimum = 0.7; - occupiedRegion.lonRange.maximum = 0.82; + OccupiedRegion occupiedRegion; + occupiedRegion.lonRange.minimum = ParametricValue(0.7); + occupiedRegion.lonRange.maximum = ParametricValue(0.82); occupiedRegion.segmentId = 0; - occupiedRegion.latRange.minimum = 0.3; - occupiedRegion.latRange.maximum = 0.66; + occupiedRegion.latRange.minimum = ParametricValue(0.3); + occupiedRegion.latRange.maximum = ParametricValue(0.66); followingObject.occupiedRegions.push_back(occupiedRegion); } { - ::rss::world::RoadSegment roadSegment; - ::rss::world::LaneSegment laneSegment; + world::RoadSegment roadSegment; + world::LaneSegment laneSegment; laneSegment.id = 0; - laneSegment.length.minimum = 41.4; - laneSegment.length.maximum = 41.4; - laneSegment.width.minimum = 5.98; - laneSegment.width.maximum = 5.98; + laneSegment.length.minimum = Distance(41.4); + laneSegment.length.maximum = Distance(41.4); + laneSegment.width.minimum = Distance(5.98); + laneSegment.width.maximum = Distance(5.98); roadSegment.push_back(laneSegment); roadArea.push_back(roadSegment); @@ -74,41 +82,44 @@ class RssCheckSameDirectionTests : public testing::Test { leadingObject.occupiedRegions.clear(); scene.egoVehicleRoad.clear(); } - ::rss::world::Object followingObject; - ::rss::world::Object leadingObject; - ::rss::world::RoadArea roadArea; - ::rss::world::Scene scene; + ::ad_rss::world::Object followingObject; + ::ad_rss::world::Object leadingObject; + ::ad_rss::world::RoadArea roadArea; + ::ad_rss::world::Scene scene; }; TEST_F(RssCheckSameDirectionTests, OtherLeading_FixDistance) { - ::rss::world::WorldModel worldModel; + ::ad_rss::world::WorldModel worldModel; - worldModel.egoVehicle = followingObject; + worldModel.egoVehicle = ad_rss::objectAsEgo(followingObject); scene.object = leadingObject; scene.egoVehicleRoad = roadArea; worldModel.scenes.push_back(scene); worldModel.timeIndex = 1; - ::rss::world::AccelerationRestriction accelerationRestriction; - ::rss::core::RssCheck rssCheck; - - double dMin = calculateLongitudinalMinSafeDistance( - worldModel.egoVehicle, worldModel.scenes[0].object); - - ASSERT_TRUE(rssCheck.calculateAccelerationRestriction( - worldModel, accelerationRestriction)); - ASSERT_EQ(accelerationRestriction.longitudinalRange.minimum, - -1. * worldModel.egoVehicle.dynamics.alphaLon.brakeMax); - ASSERT_EQ(accelerationRestriction.longitudinalRange.maximum, - -1. * worldModel.egoVehicle.dynamics.alphaLon.brakeMin); - - printf("dMin = %f\n", dMin); - printf("accelerationRestriction.longitudinalRange.minimum=%f\n", - accelerationRestriction.longitudinalRange.minimum); - printf("accelerationRestriction.longitudinalRange.maximum=%f\n", - accelerationRestriction.longitudinalRange.maximum); + ::ad_rss::world::AccelerationRestriction accelerationRestriction; + ::ad_rss::core::RssCheck rssCheck; + + ASSERT_TRUE(rssCheck.calculateAccelerationRestriction(worldModel, + accelerationRestriction)); + + if (accelerationRestriction.longitudinalRange.maximum == + -1. * worldModel.egoVehicle.dynamics.alphaLon.brakeMin) { + printf("[----------] RSS unsafe!!!\n"); + EXPECT_EQ(accelerationRestriction.longitudinalRange.maximum, + -1. * worldModel.egoVehicle.dynamics.alphaLon.brakeMin); + } else { + printf("[----------] RSS safe!!!\n"); + EXPECT_EQ(accelerationRestriction.longitudinalRange.maximum, + worldModel.egoVehicle.dynamics.alphaLon.accelMax); + } + // following judgement only true for leading-v v(0,0.5) and ego-v v(11,-0.5) + EXPECT_EQ(accelerationRestriction.lateralLeftRange.maximum, -1 * + worldModel.egoVehicle.dynamics.alphaLat.brakeMin); + EXPECT_EQ(accelerationRestriction.lateralRightRange.maximum, + worldModel.egoVehicle.dynamics.alphaLat.accelMax); } } // namespace core -} // namespace rss +} // namespace ad_rss