Skip to content

Commit

Permalink
add unit test to plan_delegator
Browse files Browse the repository at this point in the history
  • Loading branch information
@MishkaMN
MishkaMN committed Jun 13, 2024
1 parent d11eda0 commit f80dfc7
Show file tree
Hide file tree
Showing 3 changed files with 145 additions and 21 deletions.
29 changes: 15 additions & 14 deletions plan_delegator/include/plan_delegator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,9 +76,9 @@ namespace plan_delegator
double max_trajectory_duration = 6.0;
double min_crawl_speed = 2.2352; // Min crawl speed in m/s
double duration_to_signal_before_lane_change = 2.5; // (Seconds) If an upcoming lane change will begin in under this time threshold, a turn signal activation command will be published.
int tactical_plugin_service_call_timeout = 100; // (Milliseconds) The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory
int tactical_plugin_service_call_timeout = 100; // (Milliseconds) The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory
// generation takes longer than this, then planning will immediately end for the current trajectory planning iteration.

// Stream operator for this config
friend std::ostream &operator<<(std::ostream &output, const Config &c)
{
Expand All @@ -102,7 +102,7 @@ namespace plan_delegator
double starting_downtrack; // The starting downtrack of the lane change
bool is_right_lane_change; // Flag to indicate whether lane change is a right lane change; false if it is a left lane change
};

class PlanDelegator : public carma_ros2_utils::CarmaLifecycleNode
{
public:
Expand All @@ -111,7 +111,7 @@ namespace plan_delegator
static const constexpr double MILLISECOND_TO_SECOND = 0.001;

/**
* \brief PlanDelegator constructor
* \brief PlanDelegator constructor
*/
explicit PlanDelegator(const rclcpp::NodeOptions &);

Expand Down Expand Up @@ -158,8 +158,8 @@ namespace plan_delegator
void lookupFrontBumperTransform();

/**
* \brief Update the starting downtrack, ending downtrack, and maneuver-specific Lanelet ID parameters associated
* with a given maneuver. These updates are required since the starting and ending downtrack values of each maneuver
* \brief Update the starting downtrack, ending downtrack, and maneuver-specific Lanelet ID parameters associated
* with a given maneuver. These updates are required since the starting and ending downtrack values of each maneuver
* are shifted based on the distance between the base_link frame and the vehicle_front frame.
* \param maneuver The maneuver to be updated.
*/
Expand All @@ -174,7 +174,7 @@ namespace plan_delegator
protected:
// Node configuration
Config config_;

// map to store service clients
std::unordered_map<std::string, carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory>> trajectory_planners_;
// local storage of incoming messages
Expand Down Expand Up @@ -257,8 +257,8 @@ namespace plan_delegator
LaneChangeInformation getLaneChangeInformation(const carma_planning_msgs::msg::Maneuver& lane_change_maneuver);

/**
* \brief Function for processing an optional LaneChangeInformation object pertaining to an upcoming lane change. If not empty,
* an UpcomingLaneChangeStatus message is created and published based on the contents of the LaneChangeInformation. The published
* \brief Function for processing an optional LaneChangeInformation object pertaining to an upcoming lane change. If not empty,
* an UpcomingLaneChangeStatus message is created and published based on the contents of the LaneChangeInformation. The published
* UpcomingLaneChangeStatus message is stored in upcoming_lane_change_status_.
* \param upcoming_lane_change_information An optional LaneChangeInformation object. Empty if no upcoming lane change exists.
*/
Expand All @@ -267,13 +267,13 @@ namespace plan_delegator
/**
* \brief Function for processing an optional LaneChangeInformation object pertaining to the currently-occurring lane change
* and an UpcomingLaneChangeStatus message. If the optional object pertaining to the currently-occurring lane change is not empty,
* then a turn signal command is published based on the current lane change direction. Otherwise, a turn signal command in the direction
* then a turn signal command is published based on the current lane change direction. Otherwise, a turn signal command in the direction
* of the UpcomingLaneChangeStatus message is published if the vehicle is estimated to begin that lane change in under the time
* threshold defined by config_.duration_to_signal_before_lane_change. The published TurnSignalComand message is stored in
* threshold defined by config_.duration_to_signal_before_lane_change. The published TurnSignalComand message is stored in
* latest_turn_signal_command_.
* \param current_lane_change_information An optional LaneChangeInformation object pertaining to the current lane
* \param current_lane_change_information An optional LaneChangeInformation object pertaining to the current lane
* change. Empty if vehicle is not currently changing lanes.
* \param upcoming_lane_change_status An UpcomingLaneChangeStatus message containing the lane change direction of an upcoming lane change, along
* \param upcoming_lane_change_status An UpcomingLaneChangeStatus message containing the lane change direction of an upcoming lane change, along
* with the downtrack distance to that lane change.
*/
void publishTurnSignalCommand(const boost::optional<LaneChangeInformation>& current_lane_change_information, const carma_planning_msgs::msg::UpcomingLaneChangeStatus& upcoming_lane_change_status);
Expand All @@ -283,5 +283,6 @@ namespace plan_delegator
FRIEND_TEST(TestPlanDelegator, TestPlanDelegator);
FRIEND_TEST(TestPlanDelegator, TestLaneChangeInformation);
FRIEND_TEST(TestPlanDelegator, TestUpcomingLaneChangeAndTurnSignals);
FRIEND_TEST(TestPlanDelegator, TestUpdateManeuverParameters);
};
}
}
4 changes: 2 additions & 2 deletions plan_delegator/src/plan_delegator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -506,7 +506,7 @@ namespace plan_delegator
previous_lanelet_to_add = llt_on_route_optional.value();
}
else{
RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "When adjusting maneuver for lane follow, no previous lanelet found on the shortest path for lanelet "
RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "When adjusting maneuver for lane follow, no previous lanelet found on the shortest path for lanelet "
<< original_starting_lanelet.id() << ". Picking arbitrary lanelet: " << previous_lanelets[0].id() << ", instead");
previous_lanelet_to_add = previous_lanelets[0];
}
Expand Down Expand Up @@ -557,7 +557,7 @@ namespace plan_delegator
previous_lanelet_to_add = llt_on_route_optional.value();
}
else{
RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "When adjusting non-lane follow maneuver, no previous lanelet found on the shortest path for lanelet "
RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "When adjusting non-lane follow maneuver, no previous lanelet found on the shortest path for lanelet "
<< original_starting_lanelet.id() << ". Picking arbitrary lanelet: " << previous_lanelets[0].id() << ", instead");
previous_lanelet_to_add = previous_lanelets[0];
}
Expand Down
133 changes: 128 additions & 5 deletions plan_delegator/test/test_plan_delegator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ namespace plan_delegator{
auto pd = std::make_shared<plan_delegator::PlanDelegator>(node_options);
pd->configure();
pd->activate();

carma_planning_msgs::msg::TrajectoryPlan res_plan;

auto maneuver_pub = pd->create_publisher<carma_planning_msgs::msg::ManeuverPlan>("final_maneuver_plan", 5);
Expand Down Expand Up @@ -181,7 +181,7 @@ namespace plan_delegator{

// Create pose message with vehicle placed in lanelet 1210
geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.pose.position.x = 5.0;
pose_msg.pose.position.x = 5.0;
pose_msg.pose.position.y = 10.0;

std::unique_ptr<geometry_msgs::msg::PoseStamped> pose_msg_ptr = std::make_unique<geometry_msgs::msg::PoseStamped>(pose_msg);
Expand Down Expand Up @@ -285,7 +285,7 @@ namespace plan_delegator{

// Create pose message with vehicle placed in lanelet 1210
geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.pose.position.x = 5.0;
pose_msg.pose.position.x = 5.0;
pose_msg.pose.position.y = 10.0;

std::unique_ptr<geometry_msgs::msg::PoseStamped> pose_msg_ptr = std::make_unique<geometry_msgs::msg::PoseStamped>(pose_msg);
Expand Down Expand Up @@ -345,7 +345,7 @@ namespace plan_delegator{
ASSERT_EQ(pd->latest_turn_signal_command_.l, 0);

// Set the vehicle pose to lanelet 1211
pose_msg.pose.position.x = 5.0;
pose_msg.pose.position.x = 5.0;
pose_msg.pose.position.y = 29.0;

std::unique_ptr<geometry_msgs::msg::PoseStamped> pose_msg_ptr4 = std::make_unique<geometry_msgs::msg::PoseStamped>(pose_msg);
Expand All @@ -357,7 +357,7 @@ namespace plan_delegator{
pd->maneuverPlanCallback(std::move(maneuver_plan_ptr2));

// Update vehicle pose again (lanelet 1211) so that internal data members related to lane changes and turn signals
pose_msg.pose.position.x = 5.0;
pose_msg.pose.position.x = 5.0;
pose_msg.pose.position.y = 30.0;

std::unique_ptr<geometry_msgs::msg::PoseStamped> pose_msg_ptr5 = std::make_unique<geometry_msgs::msg::PoseStamped>(pose_msg);
Expand All @@ -372,6 +372,129 @@ namespace plan_delegator{
ASSERT_EQ(pd->latest_turn_signal_command_.l, 0);
}

/**
* Total route length should be 100m
*
* |1203|1213|1223|
* | _ _ _ _ _|
* |1202|1212|1222|
* | _ _ _ _ _|
* |1201|1211|1221| num = lanelet id hardcoded for easier testing
* | _ _ _ _ _| | = lane lines
* |1200|1210|1220| - - - = Lanelet boundary
* | 12100 |
* ****************
* START_LINE
*/
TEST(TestPlanDelegator, TestUpdateManeuverParameters)
{
rclcpp::NodeOptions node_options;
auto pd = std::make_shared<plan_delegator::PlanDelegator>(node_options);
pd->configure();
pd->activate();

// Use Guidance Lib to create map without an obstacle
carma_wm::test::MapOptions options;
options.obstacle_ = carma_wm::test::MapOptions::Obstacle::NONE;
std::shared_ptr<carma_wm::CARMAWorldModel> cmw = carma_wm::test::getGuidanceTestMap(options);

// Introduce overlapping lanelet not on the route
lanelet::Lanelet start_lanelet = cmw->getMutableMap()->laneletLayer.get(1210);
lanelet::Lanelet start_lanelet_overlapping = carma_wm::test::getLanelet(12100, start_lanelet.leftBound(), start_lanelet.rightBound());
auto overlapping_llt_id = std::to_string(start_lanelet_overlapping.id());
cmw->getMutableMap()->add(start_lanelet_overlapping);
cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph
carma_wm::test::setRouteByIds({1210, 1213}, cmw);
// Set PlanDelegator's world model object
pd->wm_ = cmw;

// Verify that start end dist are correct and lanelet on the route is prioritized when picking lanelet
carma_planning_msgs::msg::Maneuver maneuver_1;
maneuver_1.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
maneuver_1.lane_following_maneuver.start_dist = 25.0;
maneuver_1.lane_following_maneuver.end_dist = 50.0;
maneuver_1.lane_following_maneuver.lane_ids.push_back("1211");
pd->length_to_front_bumper_ = 4.0; // 4.0 meter vehicle length
pd->updateManeuverParameters(maneuver_1);

EXPECT_NEAR(maneuver_1.lane_following_maneuver.start_dist, 21.0, 0.01);
EXPECT_NEAR(maneuver_1.lane_following_maneuver.end_dist, 46.0, 0.01);
EXPECT_EQ(maneuver_1.lane_following_maneuver.lane_ids.front(), "1210");

// Switch route with a different lanelet to verify route prioritization
cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph
carma_wm::test::setRouteByIds({start_lanelet_overlapping.id(), 1213}, cmw);
pd->wm_ = cmw;
maneuver_1.lane_following_maneuver.start_dist = 25.0;
maneuver_1.lane_following_maneuver.end_dist = 50.0;
maneuver_1.lane_following_maneuver.lane_ids.front() = "1211";
pd->updateManeuverParameters(maneuver_1);

EXPECT_NEAR(maneuver_1.lane_following_maneuver.start_dist, 21.0, 0.01);
EXPECT_NEAR(maneuver_1.lane_following_maneuver.end_dist, 46.0, 0.01);
EXPECT_EQ(maneuver_1.lane_following_maneuver.lane_ids.front(), overlapping_llt_id);

// lanechange
carma_planning_msgs::msg::Maneuver maneuver_2;
maneuver_2.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE;
maneuver_2.lane_change_maneuver.start_dist = 25.0;
maneuver_2.lane_change_maneuver.end_dist = 50.0;
maneuver_2.lane_change_maneuver.starting_lane_id = "1211";
maneuver_2.lane_change_maneuver.ending_lane_id = "1221";

cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph
carma_wm::test::setRouteByIds({1210, 1213}, cmw);
pd->wm_ = cmw;

pd->updateManeuverParameters(maneuver_2);
EXPECT_NEAR(maneuver_2.lane_change_maneuver.start_dist, 21.0, 0.01);
EXPECT_NEAR(maneuver_2.lane_change_maneuver.end_dist, 46.0, 0.01);
EXPECT_EQ(maneuver_2.lane_change_maneuver.starting_lane_id, "1210");

// Switch route with a different lanelet to verify route prioritization
cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph
carma_wm::test::setRouteByIds({start_lanelet_overlapping.id(), 1211, 1212, 1213}, cmw);
pd->wm_ = cmw;
maneuver_2.lane_change_maneuver.start_dist = 25.0; // reset
maneuver_2.lane_change_maneuver.end_dist = 50.0;
maneuver_2.lane_change_maneuver.starting_lane_id = "1211";
maneuver_2.lane_change_maneuver.ending_lane_id = "1221";

pd->updateManeuverParameters(maneuver_2);
EXPECT_NEAR(maneuver_2.lane_change_maneuver.start_dist, 21.0, 0.01);
EXPECT_NEAR(maneuver_2.lane_change_maneuver.end_dist, 46.0, 0.01);
EXPECT_EQ(maneuver_2.lane_change_maneuver.starting_lane_id, overlapping_llt_id);

// Verify that the non-route lanelet is being picked if no suitable lanelet is on the route
cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph
carma_wm::test::setRouteByIds({1201, 1203}, cmw);
pd->wm_ = cmw;

// Lane Follow
maneuver_1.lane_following_maneuver.start_dist = 0.0; // Reset values
maneuver_1.lane_following_maneuver.end_dist = 25.0;
maneuver_1.lane_following_maneuver.lane_ids.front() = "1201";

pd->updateManeuverParameters(maneuver_1);

EXPECT_NEAR(maneuver_1.lane_following_maneuver.start_dist, -4.0, 0.01);
EXPECT_NEAR(maneuver_1.lane_following_maneuver.end_dist, 21.0, 0.01);
EXPECT_EQ(maneuver_1.lane_following_maneuver.lane_ids.front(), "1200");

// Lanechange
cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph
carma_wm::test::setRouteByIds({1221, 1213}, cmw);
pd->wm_ = cmw;
maneuver_2.lane_change_maneuver.start_dist = 0.0; // Reset values
maneuver_2.lane_change_maneuver.end_dist = 25.0;
maneuver_2.lane_change_maneuver.starting_lane_id = "1221";
maneuver_2.lane_change_maneuver.ending_lane_id = "1211";

pd->updateManeuverParameters(maneuver_2);
EXPECT_NEAR(maneuver_2.lane_change_maneuver.start_dist, -4.0, 0.01);
EXPECT_NEAR(maneuver_2.lane_change_maneuver.end_dist, 21.0, 0.01);
EXPECT_EQ(maneuver_2.lane_change_maneuver.starting_lane_id, "1220");
}
} // namespace plan_delegator

/*!
Expand Down

0 comments on commit f80dfc7

Please sign in to comment.