Merge pull request #1445 from usdot-fhwa-stol/fix/wz_plugin

Commit changes to fix/wz_plugin

wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin_config.h

@@ -28,6 +28,9 @@ struct WzStrategicPluginConfig
   //! A multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabilities
   double vehicle_decel_limit_multiplier = 0.75;
 
+  //! The length parameter of the participant vehicle used to help calculate the distance before stopping at a red traffic signal
+  double vehicle_length = 5.0;
+
   //! The minimum distance in meters that the vehicle can be at before requiring a transition to the APPROACH state
   double min_approach_distance = 30.0;
 

wz_strategic_plugin/src/main.cpp

@@ -50,6 +50,7 @@ int main(int argc, char** argv)
   pnh.param<std::string>("lane_following_plugin_name",       config.lane_following_plugin_name, config.lane_following_plugin_name);
   pnh.param<std::string>("stop_and_wait_plugin_name",        config.stop_and_wait_plugin_name, config.stop_and_wait_plugin_name);
   pnh.param<std::string>("intersection_transit_plugin_name", config.intersection_transit_plugin_name, config.intersection_transit_plugin_name);
+  pnh.param<double>("/vehicle_length", config.vehicle_length, config.vehicle_length);
   // clang-format on
 
   // Construct plugin

wz_strategic_plugin/src/wz_strategic_plugin.cpp

@@ -175,7 +175,7 @@ void WzStrategicPlugin::planWhenUNAVAILABLE(const cav_srvs::PlanManeuversRequest
 
   ROS_DEBUG("traffic_light_down_track %f", traffic_light_down_track);
 
-  double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack;
+  double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack - config_.vehicle_length;
 
   ROS_DEBUG("distance_remaining_to_traffic_light %f", distance_remaining_to_traffic_light);
 
@@ -244,7 +244,7 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest
   // In the case of higher accel limits this calculation should always overestimate the arrival time which should be
   // fine as that would hit the following red phase.
   double time_remaining_to_traffic_light =
-      (2.0 * distance_remaining_to_traffic_light) /
+      (2.0 * (distance_remaining_to_traffic_light - config_.vehicle_length)) /
       (intersection_speed_.get() + current_state.speed);  // Kinematic Equation: 2*d / (vf + vi) = t
 
   ROS_DEBUG_STREAM("time_remaining_to_traffic_light: " << time_remaining_to_traffic_light);
@@ -290,7 +290,7 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest
 
     // TODO do we need to check for anything before pushing onto the plan?
     resp.new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(
-        current_state.downtrack, traffic_light_down_track, current_state.speed, intersection_speed_.get(),
+        current_state.downtrack, traffic_light_down_track - config_.vehicle_length, current_state.speed, intersection_speed_.get(),
         current_state.stamp, light_arrival_time_at_freeflow,
         lanelet::utils::transform(crossed_lanelets, [](const auto& ll) { return ll.id(); })));
 
@@ -300,15 +300,15 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest
         light_arrival_time_at_freeflow + ros::Duration(intersection_length / intersection_speed_.get());
 
     resp.new_plan.maneuvers.push_back(composeIntersectionTransitMessage(
-        traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(),
+        traffic_light_down_track - config_.vehicle_length, intersection_end_downtrack_.get(), intersection_speed_.get(),
         intersection_speed_.get(), light_arrival_time_at_freeflow, intersection_exit_time, crossed_lanelets.back().id(),
         nearest_traffic_light->getControlledLanelets().back().id()));
   }
   else  // Red or Yellow light
   {
     ROS_DEBUG_STREAM("Planning stop and wait maneuver");
     resp.new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(
-        current_state.downtrack, traffic_light_down_track, current_state.speed, crossed_lanelets.front().id(),
+        current_state.downtrack, traffic_light_down_track - config_.vehicle_length, current_state.speed, crossed_lanelets.front().id(),
         crossed_lanelets.back().id(), current_state.stamp,
         req.header.stamp + ros::Duration(config_.min_maneuver_planning_period)));
   }

wz_strategic_plugin/test/test_strategic_plugin.cpp

@@ -54,12 +54,13 @@ TEST_F(WorkZoneTestFixture, planManeuverCb)
   ROS_DEBUG("Out of range test ");
   req.header.stamp = ros::Time(0);
   req.veh_x = 1.85;
-  req.veh_y = 5.0; // Out of range of light which should kick in at 8.36567466667
+  req.veh_y = 1.0; // Out of range of light which should kick in at 8.36567466667
   req.veh_downtrack = req.veh_y;
   req.veh_logitudinal_velocity = 11.176;
   req.veh_lane_id = "1200";
 
   wzp.planManeuverCb(req, resp);
+  ROS_ERROR_STREAM("resp.newplanmaneuvers size: "<<resp.new_plan.maneuvers.size());
 
   ASSERT_TRUE(resp.new_plan.maneuvers.empty());
 
@@ -79,9 +80,9 @@ TEST_F(WorkZoneTestFixture, planManeuverCb)
   ASSERT_NEAR(0.0, resp.new_plan.maneuvers[0].stop_and_wait_maneuver.start_time.toSec(), 0.00001);
   ASSERT_NEAR(11.176, resp.new_plan.maneuvers[0].stop_and_wait_maneuver.start_speed, 0.00001);
   ASSERT_NEAR(config.min_maneuver_planning_period, resp.new_plan.maneuvers[0].stop_and_wait_maneuver.end_time.toSec(), 0.001);
-  ASSERT_NEAR(50.0, resp.new_plan.maneuvers[0].stop_and_wait_maneuver.end_dist, 0.0001);
+  ASSERT_NEAR(45.0, resp.new_plan.maneuvers[0].stop_and_wait_maneuver.end_dist, 0.0001);
   ASSERT_TRUE(resp.new_plan.maneuvers[0].stop_and_wait_maneuver.starting_lane_id.compare("1200") == 0);
-  ASSERT_TRUE(resp.new_plan.maneuvers[0].stop_and_wait_maneuver.ending_lane_id.compare("1201") == 0);
+  ASSERT_TRUE(resp.new_plan.maneuvers[0].stop_and_wait_maneuver.ending_lane_id.compare("1202") == 0);
 
   ROS_DEBUG("Waiting test ");
   req = cav_srvs::PlanManeuversRequest();
@@ -126,7 +127,7 @@ TEST_F(WorkZoneTestFixture, planManeuverCb)
   ASSERT_NEAR(75.0, resp.new_plan.maneuvers[0].intersection_transit_straight_maneuver.end_dist, 0.0001);
   ASSERT_NEAR(11.176, resp.new_plan.maneuvers[0].intersection_transit_straight_maneuver.end_speed, 0.00001);
   ASSERT_TRUE(resp.new_plan.maneuvers[0].intersection_transit_straight_maneuver.starting_lane_id.compare("1201") == 0);
-  ASSERT_TRUE(resp.new_plan.maneuvers[0].intersection_transit_straight_maneuver.ending_lane_id.compare("1202") == 0);
+  ASSERT_TRUE(resp.new_plan.maneuvers[0].intersection_transit_straight_maneuver.ending_lane_id.compare("1203") == 0);
 
   ROS_DEBUG("Exit test ");
   req = cav_srvs::PlanManeuversRequest();
@@ -161,22 +162,22 @@ TEST_F(WorkZoneTestFixture, planManeuverCb)
   ASSERT_EQ(10.0, resp.new_plan.maneuvers[0].lane_following_maneuver.start_dist);
   ASSERT_NEAR(25.0, resp.new_plan.maneuvers[0].lane_following_maneuver.start_time.toSec(), 0.00001);
   ASSERT_NEAR(11.176, resp.new_plan.maneuvers[0].lane_following_maneuver.start_speed, 0.00001);
-  ASSERT_NEAR((ros::Time(25.0) + ros::Duration(3.57909806729)).toSec(), resp.new_plan.maneuvers[0].lane_following_maneuver.end_time.toSec(), 0.25);
-  ASSERT_NEAR(50.0, resp.new_plan.maneuvers[0].lane_following_maneuver.end_dist, 0.0001);
+  ASSERT_NEAR((ros::Time(25.0) + ros::Duration(3.13171080888)).toSec(), resp.new_plan.maneuvers[0].lane_following_maneuver.end_time.toSec(), 0.25);
+  ASSERT_NEAR(45.0, resp.new_plan.maneuvers[0].lane_following_maneuver.end_dist, 0.0001);
   ASSERT_NEAR(11.176, resp.new_plan.maneuvers[0].lane_following_maneuver.end_speed, 0.00001);
-  ASSERT_EQ(2, resp.new_plan.maneuvers[0].lane_following_maneuver.lane_ids.size());
+  ASSERT_EQ(3, resp.new_plan.maneuvers[0].lane_following_maneuver.lane_ids.size());
   ASSERT_TRUE(resp.new_plan.maneuvers[0].lane_following_maneuver.lane_ids[0].compare("1200") == 0);
   ASSERT_TRUE(resp.new_plan.maneuvers[0].lane_following_maneuver.lane_ids[1].compare("1201") == 0);
 
 
   ASSERT_EQ(cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT, resp.new_plan.maneuvers[1].type);
-  ASSERT_EQ(50.0, resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.start_dist );
-  ASSERT_NEAR((ros::Time(25.0) + ros::Duration(3.57909806729)).toSec(), resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.start_time.toSec(), 0.00001);
+  ASSERT_EQ(45.0, resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.start_dist );
+  ASSERT_NEAR((ros::Time(25.0) + ros::Duration(3.13171080888)).toSec(), resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.start_time.toSec(), 0.25);
   ASSERT_NEAR(11.176, resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.start_speed, 0.00001);
-  ASSERT_NEAR((ros::Time(25.0) + ros::Duration(2.23693629205 + 3.57909806729)).toSec(), resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.end_time.toSec(), 0.25);
+  ASSERT_NEAR((ros::Time(25.0) + ros::Duration(2.23693629205 + 3.13171080888)).toSec(), resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.end_time.toSec(), 0.25);
   ASSERT_NEAR(75.0, resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.end_dist, 0.0001);
   ASSERT_NEAR(11.176, resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.end_speed, 0.00001);
-  ASSERT_TRUE(resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.starting_lane_id.compare("1201") == 0);
+  ASSERT_TRUE(resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.starting_lane_id.compare("1202") == 0);
   ASSERT_TRUE(resp.new_plan.maneuvers[1].intersection_transit_straight_maneuver.ending_lane_id.compare("1202") == 0);
 }
 }  // namespace wz_strategic_plugin