Merge branch 'fix/steering_event' into hotfix/candidate_3.3.3

usdot-fhwa-stol · Apr 4, 2021 · f9bd3aa · f9bd3aa
2 parents c7ad796 + e45cc9d
commit f9bd3aa
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 11 deletions.
diff --git a/route_following_plugin/src/route_following_plugin.cpp b/route_following_plugin/src/route_following_plugin.cpp
@@ -46,7 +46,7 @@ namespace route_following_plugin
 
         pnh_->param<double>("minimal_maneuver_duration", mvr_duration_, 16.0);
         pnh2_->param<double>("config_speed_limit",config_limit);
-        pnh_->param<double>("/guidance/route_end_jerk", jerk_, 1.0);
+        pnh_->param<double>("/guidance/route_end_jerk", jerk_, 0.05);
         wml_.reset(new carma_wm::WMListener());
         // set world model point form wm listener
         wm_ = wml_->getWorldModel();

diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp
@@ -129,12 +129,8 @@ namespace stop_and_wait_plugin
             return true;
         }
 
-        // Update state to correctly reflect current pos
-        auto curr_state = req.vehicle_state;
-        curr_state.X_pos_global = pose_msg_.pose.position.x;
-        curr_state.Y_pos_global = pose_msg_.pose.position.y;
 
-        std::vector<PointSpeedPair> points_and_target_speeds = maneuvers_to_points(maneuver_plan, current_downtrack, wm_, curr_state);
+        std::vector<PointSpeedPair> points_and_target_speeds = maneuvers_to_points(maneuver_plan, current_downtrack, wm_, req.vehicle_state);
 
         auto downsampled_points = 
             carma_utils::containers::downsample_vector(points_and_target_speeds,downsample_ratio_);
@@ -145,7 +141,7 @@ namespace stop_and_wait_plugin
         trajectory.header.stamp = ros::Time::now();
         trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
 
-        trajectory.trajectory_points = compose_trajectory_from_centerline(downsampled_points,curr_state);
+        trajectory.trajectory_points = compose_trajectory_from_centerline(downsampled_points,req.vehicle_state);
         ROS_DEBUG_STREAM("Trajectory points size:"<<trajectory.trajectory_points.size());
         trajectory.initial_longitudinal_velocity = req.vehicle_state.longitudinal_vel;
         resp.trajectory_plan = trajectory;
@@ -254,11 +250,16 @@ namespace stop_and_wait_plugin
 
                 lanelet::BasicLineString2d route_geometry = carma_wm::geometry::concatenate_lanelets(lanelets_to_add);
                 int nearest_pt_index = getNearestRouteIndex(route_geometry,state);
+                // route end point index
                 auto temp_state = state;
-                temp_state.X_pos_global = wm_->getRoute()->getEndPoint().basicPoint2d().x();
-                temp_state.Y_pos_global =  wm_->getRoute()->getEndPoint().basicPoint2d().y();
-                int nearest_end_pt_index = getNearestRouteIndex(route_geometry,temp_state);
-                lanelet::BasicLineString2d future_route_geometry(route_geometry.begin() + nearest_pt_index, route_geometry.begin()+ nearest_end_pt_index);
+
+                // maneuver end dist index
+                int ending_downtrack_pt_index = (int)(route_geometry.size() * (ending_downtrack / wm_->getRoute()->length2d()));
+                ROS_DEBUG_STREAM("ending_downtrack: " << ending_downtrack);
+                ROS_DEBUG_STREAM("ending_downtrack_pt_index" << ending_downtrack_pt_index);
+                ROS_DEBUG_STREAM("nearest_pt_index" << nearest_pt_index);
+
+                lanelet::BasicLineString2d future_route_geometry(route_geometry.begin() + nearest_pt_index, route_geometry.begin()+ ending_downtrack_pt_index);
 
                 int points_count = future_route_geometry.size();
                 delta_time = maneuver_time_/(points_count-1);