Skip to content

Commit

Permalink
rss-open-source-library-apollo-wrapper
Browse files Browse the repository at this point in the history
  • Loading branch information
@l1212s @ycool
l1212s authored and ycool committed Apr 4, 2019
1 parent b552b2a commit 7df20ba
Show file tree
Hide file tree
Showing 4 changed files with 138 additions and 109 deletions.
4 changes: 2 additions & 2 deletions modules/planning/tasks/rss/BUILD
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
],
)

Expand All @@ -30,7 +30,7 @@ cc_test(
],
deps = [
":decider_rss",
"//third_party/rss",
"@ad_rss_lib//:ad_rss",
"@gtest//:main",
],
)
Expand Down
132 changes: 75 additions & 57 deletions modules/planning/tasks/rss/decider_rss.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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 {
Expand All @@ -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();
Expand Down
4 changes: 2 additions & 2 deletions modules/planning/tasks/rss/decider_rss.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,8 @@
#include <string>

#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 {
Expand Down
107 changes: 59 additions & 48 deletions modules/planning/tasks/rss/decider_rss_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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

0 comments on commit 7df20ba

Please sign in to comment.