diff --git a/Dockerfile b/Dockerfile
index 2406b671c9..5d73cbe514 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -33,9 +33,9 @@
 # Stage 1 - Acquire the CARMA source as well as any extra packages
 # /////////////////////////////////////////////////////////////////////////////
 
-FROM 63c52089ebf6 AS base-image
+FROM usdotfhwastoldev/autoware.ai:develop AS base-image
 
-FROM 63c52089ebf6 AS source-code
+FROM base-image AS source-code
 
 RUN mkdir ~/src
 COPY --chown=carma . /home/carma/src/carma-platform/
@@ -46,7 +46,7 @@ RUN ~/src/carma-platform/docker/checkout.bash
 # /////////////////////////////////////////////////////////////////////////////
 
 
-FROM 63c52089ebf6 AS install
+FROM base-image AS install
 ARG ROS1_PACKAGES=""
 ENV ROS1_PACKAGES=${ROS1_PACKAGES}
 ARG ROS2_PACKAGES=""
@@ -63,7 +63,7 @@ RUN ~/carma_ws/src/carma-platform/docker/install.sh
 # /////////////////////////////////////////////////////////////////////////////
 
 
-FROM 63c52089ebf6
+FROM base-image
 
 ARG BUILD_DATE="NULL"
 ARG VCS_REF="NULL"
diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp
index aa1bbdcc3e..7174a825c0 100644
--- a/basic_autonomy/src/basic_autonomy.cpp
+++ b/basic_autonomy/src/basic_autonomy.cpp
@@ -1305,14 +1305,14 @@ namespace basic_autonomy
             const carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory>& yield_client,
             int yield_plugin_service_call_timeout)
         {
-            RCLCPP_ERROR(node_handler->get_logger(), "Activate Object Avoidance");
+            RCLCPP_DEBUG(node_handler->get_logger(), "Activate Object Avoidance");
 
             if (!yield_client || !yield_client->service_is_ready())
             {
                 throw std::runtime_error("Yield Client is not set or unavailable after configuration state of lifecycle");
             }
 
-            RCLCPP_ERROR(node_handler->get_logger(), "Yield Client is valid");
+            RCLCPP_DEBUG(node_handler->get_logger(), "Yield Client is valid");
 
             auto yield_srv = std::make_shared<carma_planning_msgs::srv::PlanTrajectory::Request>();
             yield_srv->initial_trajectory_plan = resp->trajectory_plan;
@@ -1330,11 +1330,11 @@ namespace basic_autonomy
                 return resp;
             }
 
-            RCLCPP_ERROR(node_handler->get_logger(), "Received Traj from Yield");
+            RCLCPP_DEBUG(node_handler->get_logger(), "Received Traj from Yield");
             carma_planning_msgs::msg::TrajectoryPlan yield_plan = yield_resp.get()->trajectory_plan;
             if (is_valid_yield_plan(node_handler, yield_plan))
             {
-                RCLCPP_ERROR(node_handler->get_logger(), "Yield trajectory validated");
+                RCLCPP_DEBUG(node_handler->get_logger(), "Yield trajectory validated");
                 resp->trajectory_plan = yield_plan;
             }
             else
diff --git a/carma/launch/environment.launch.py b/carma/launch/environment.launch.py
index 3d8799db7a..7a70919c5f 100644
--- a/carma/launch/environment.launch.py
+++ b/carma/launch/environment.launch.py
@@ -287,7 +287,7 @@ def generate_launch_description():
                     ("incoming_psm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_psm" ] ),
                     ("incoming_bsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm" ] ),
                     ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
-                    #("external_objects", "fused_external_objects") TODO: directly listening to external objects
+                    ("external_objects", "fused_external_objects")
                 ],
                 parameters=[
                     motion_computation_param_file,
@@ -529,6 +529,6 @@ def generate_launch_description():
         carma_external_objects_container,
         lanelet2_map_loader_container,
         lanelet2_map_visualization_container,
-        #carma_cooperative_perception_container,
+        carma_cooperative_perception_container,
         subsystem_controller
     ])
diff --git a/carma/rviz/carma_default.rviz b/carma/rviz/carma_default.rviz
index 4927f3ec8b..5e036976b9 100644
--- a/carma/rviz/carma_default.rviz
+++ b/carma/rviz/carma_default.rviz
@@ -10,7 +10,7 @@ Panels:
         - /MobilityPath Collision Warning1
         - /TCM Visualizer1
       Splitter Ratio: 0.5446686148643494
-    Tree Height: 441
+    Tree Height: 214
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -26,10 +26,9 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: Points Map
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -59,154 +58,82 @@ Visualization Manager:
       Enabled: true
       Frame Timeout: 15
       Frames:
-        207:
-          Value: true
-        219:
-          Value: true
-        220:
-          Value: true
-        221:
-          Value: true
-        222:
-          Value: true
-        223:
-          Value: true
-        224:
-          Value: true
-        225:
-          Value: true
         All Enabled: true
         base_link:
           Value: true
-        camera:
-          Value: true
-        carma_1:
-          Value: true
-        carma_1/camera/rgb/front:
-          Value: true
-        carma_1/gnss/gnss1:
-          Value: true
-        carma_1/gnss/novatel_gnss:
+        camera_sr:
           Value: true
-        carma_1/imu/novatel_imu:
+        concat_velodyne:
           Value: true
-        carma_1/lidar/lidar:
+        front_esr:
           Value: true
-        carma_1/radar/radar_fc:
+        front_left_srr:
           Value: true
-        carma_1/radar/radar_fl:
-          Value: true
-        carma_1/radar/radar_fr:
-          Value: true
-        carma_1/radar/radar_rl:
-          Value: true
-        carma_1/radar/radar_rr:
-          Value: true
-        carma_1/semantic_lidar/front:
+        front_right_srr:
           Value: true
         gps:
           Value: true
-        host_vehicle:
-          Value: true
-        lidar/front:
-          Value: true
         map:
           Value: true
         mobileye:
           Value: true
-        mobility:
-          Value: true
         ned_heading:
           Value: true
-        novatel_imu:
-          Value: true
-        radar_fc:
+        novatel:
           Value: true
-        radar_fl:
+        novatel_imu:
           Value: true
-        radar_fr:
+        rear_esr:
           Value: true
-        radar_rl:
+        rear_left_srr:
           Value: true
-        radar_rr:
+        rear_right_srr:
           Value: true
         vehicle_front:
           Value: true
-        velodyne:
+        velodyne_1:
           Value: true
-        world:
+        velodyne_2:
           Value: true
+      Marker Alpha: 1
       Marker Scale: 5
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        world:
-          map:
-            207:
-              carma_1:
-                base_link:
-                  gps:
-                    ned_heading:
-                      {}
-                  host_vehicle:
-                    {}
-                  mobileye:
-                    {}
-                  novatel_imu:
-                    {}
-                  radar_fc:
-                    {}
-                  radar_fl:
-                    {}
-                  radar_fr:
-                    {}
-                  radar_rl:
-                    {}
-                  radar_rr:
-                    {}
-                  vehicle_front:
-                    {}
-            219:
-              {}
-            220:
-              {}
-            221:
+        map:
+          base_link:
+            camera_sr:
               {}
-            222:
+            concat_velodyne:
               {}
-            223:
+            front_esr:
               {}
-            224:
+            front_left_srr:
               {}
-            225:
+            front_right_srr:
               {}
-            carma_1/camera/rgb/front:
-              camera:
+            gps:
+              ned_heading:
                 {}
-            carma_1/gnss/novatel_gnss:
-              {}
-            carma_1/imu/novatel_imu:
+            mobileye:
               {}
-            carma_1/lidar/lidar:
-              velodyne:
-                {}
-            carma_1/radar/radar_fc:
+            novatel:
               {}
-            carma_1/radar/radar_fl:
+            novatel_imu:
               {}
-            carma_1/radar/radar_fr:
+            rear_esr:
               {}
-            carma_1/radar/radar_rl:
+            rear_left_srr:
               {}
-            carma_1/radar/radar_rr:
+            rear_right_srr:
               {}
-            carma_1/semantic_lidar/front:
+            vehicle_front:
               {}
-            lidar/front:
+            velodyne_1:
               {}
-            mobility:
+            velodyne_2:
               {}
       Update Interval: 0
       Value: true
@@ -225,9 +152,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Points Map
       Position Transformer: XYZ
       Queue Size: 10
@@ -307,9 +232,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Points Raw
       Position Transformer: XYZ
       Queue Size: 10
@@ -369,11 +292,27 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        gps:
+        camera_sr:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        concat_velodyne:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_esr:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_left_srr:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_srr:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        host_vehicle:
+        gps:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -385,35 +324,35 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        novatel_imu:
+        novatel:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        radar_fc:
+        novatel_imu:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        radar_fl:
+        rear_esr:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        radar_fr:
+        rear_left_srr:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        radar_rl:
+        rear_right_srr:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        radar_rr:
+        vehicle_front:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        vehicle_front:
+        velodyne_1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        velodyne:
+        velodyne_2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -438,9 +377,7 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Points Cluster
       Position Transformer: XYZ
       Queue Size: 10
@@ -662,6 +599,7 @@ Visualization Manager:
       Head Length: 0.30000001192092896
       Head Radius: 0.10000000149011612
       Name: Pose
+      Queue Size: 10
       Shaft Length: 1
       Shaft Radius: 0.05000000074505806
       Shape: Axes
@@ -685,7 +623,7 @@ Visualization Manager:
       Marker Topic: /guidance/trajectory_visualizer
       Name: MarkerArray
       Namespaces:
-        trajectory_visualizer: true
+        {}
       Queue Size: 10
       Value: true
     - Class: rviz/Marker
@@ -693,7 +631,7 @@ Visualization Manager:
       Marker Topic: /guidance/route_marker
       Name: RouteMarker
       Namespaces:
-        {}
+        route_visualizer: true
       Queue Size: 100
       Value: true
     - Class: rviz/MarkerArray
@@ -755,7 +693,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -0.02500048652291298
+      Angle: 1.5899995565414429
       Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -765,23 +703,22 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 8.807906150817871
+      Scale: 8.796120643615723
       Target Frame: base_link
-      Value: TopDownOrtho (rviz)
-      X: 19.913219451904297
-      Y: 3.1033883094787598
+      X: 0.6240193843841553
+      Y: 5.644039630889893
     Saved: ~
 Window Geometry:
   Camera:
     collapsed: false
   Displays:
     collapsed: true
-  Height: 581
+  Height: 288
   Hide Left Dock: true
   Hide Right Dock: true
   Image:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -790,6 +727,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 924
-  X: 996
-  Y: 27
+  Width: 683
+  X: 72
+  Y: 245
diff --git a/docker/checkout.bash b/docker/checkout.bash
index c97e31c691..85627fdeea 100755
--- a/docker/checkout.bash
+++ b/docker/checkout.bash
@@ -37,3 +37,73 @@ done
 
 cd ${dir}/src
 
+
+# clone carma repos
+
+if [[ "$BRANCH" = "develop" ]]; then
+      git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch  $BRANCH
+      git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch $BRANCH
+      git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch $BRANCH
+else
+      git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch develop
+      git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch develop
+      git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch develop
+fi
+
+# Get humble branch of message filters which supports template Node arguments (foxy version supports rclcpp::Node only)
+git clone https://github.com/usdot-fhwa-stol/carma-message-filters.git --branch develop
+
+git clone https://github.com/usdot-fhwa-stol/multiple_object_tracking --branch develop
+
+# add astuff messages
+# NOTE: The ibeo_msgs package is ignored because on build the cmake files in that package run a sed command
+#       which can make them incompatible with a new ros version after a source switch
+git clone https://github.com/astuff/astuff_sensor_msgs
+
+cd astuff_sensor_msgs
+git checkout 41d5ef0c33fb27eb3c9ba808b51332bcce186a83
+
+# Disable ibeo_msgs
+cd ibeo_msgs
+echo "" > COLCON_IGNORE
+cd ../astuff_sensor_msgs
+echo "" > COLCON_IGNORE
+
+cd ../
+
+# Clone the foxy branch of ros2_tracing in order to enable certain analyses of CARMA Platform
+# made possible through collected trace data, such as analyzing ROS 2 callback durations.
+git clone -b foxy https://github.com/ros2/ros2_tracing
+
+#rosbridge_suite is a ROS meta-package including all the rosbridge packages.
+# NOTE: clone -b flag is used instead of --branch to avoid hook rewriting it
+git clone -b ros2 https://github.com/usdot-fhwa-stol/rosbridge_suite
+
+# The feature/integrate-carma branch of rosbag2 includes improvements that were not possible to backport into the foxy branch
+# of rosbag2. These rosbag2 packages will replace the originally built foxy rosbag2 packages.
+# NOTE: Additional information regarding the rosbag2 improvements on this branch are included in the forked repository's README.
+git clone -b carma-develop https://github.com/usdot-fhwa-stol/rosbag2
+
+# Novatel OEM7 Driver
+# NOTE: This is required since otherwise this image will not contain the novatel_oem7_msgs package, and a missing ROS 2 message package
+#       can cause ROS 2 rosbag logging to fail in Foxy.
+# Related GitHub discussion for fix that was not backported to Foxy: https://github.com/ros2/rosbag2/pull/858
+git clone https://github.com/novatel/novatel_oem7_driver.git ${dir}/src/novatel_oem7_driver -b ros2-dev
+# Checkout verified commit
+cd ${dir}/src/novatel_oem7_driver
+git checkout 3055e220bb9715b59c3ef53ab0aba05a495d9d5
+# Ignore novatel_oem7_driver package; only novatel_oem7_msgs is required
+cd ${dir}/src/novatel_oem7_driver/src/novatel_oem7_driver
+echo "" > COLCON_IGNORE
+cd ${dir}/src
+
+
+cd ${dir}/src
+
+# git clone --branch master --depth 1 https://github.com/nitroshare/qhttpengine.git
+git clone -b master --depth 1 https://github.com/etherealjoy/qhttpengine.git
+git clone -b develop --depth 1 https://github.com/usdot-fhwa-OPS/V2X-Hub.git
+cd V2X-Hub
+git config core.sparsecheckout true
+git sparse-checkout init
+git sparse-checkout set ext/ccserver
diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp
index 414b376995..2ff6c6ee00 100644
--- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp
+++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp
@@ -116,7 +116,7 @@ void InLaneCruisingPlugin::plan_trajectory_callback(
   }
   else
   {
-    RCLCPP_ERROR(nh_->get_logger(), "Ignored Object Avoidance");
+    RCLCPP_DEBUG(nh_->get_logger(), "Ignored Object Avoidance");
   }
 
   if (config_.publish_debug) { // Publish the debug message if in debug logging mode
diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp
index 3cf10975f5..83f84c7ef7 100644
--- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp
+++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp
@@ -249,7 +249,7 @@ namespace light_controlled_intersection_tactical_plugin
         }
         else
         {
-            RCLCPP_ERROR(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Ignored Object Avoidance");
+            RCLCPP_DEBUG(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Ignored Object Avoidance");
         }
 
         resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
diff --git a/motion_computation/config/parameters.yaml b/motion_computation/config/parameters.yaml
index fe48f7a689..d3a2fe079d 100644
--- a/motion_computation/config/parameters.yaml
+++ b/motion_computation/config/parameters.yaml
@@ -1,8 +1,8 @@
 # Time between predicted states in seconds
-prediction_time_step: 0.2
+prediction_time_step: 0.1
 
 # Period of prediction in seconds
-prediction_period: 6.0
+prediction_period: 3.0
 
 # CV Model X-Axis Acceleration Noise
 cv_x_accel_noise: 9.0
diff --git a/route_following_plugin/src/route_following_plugin.cpp b/route_following_plugin/src/route_following_plugin.cpp
index 514f42d35c..ebd8edbf0c 100644
--- a/route_following_plugin/src/route_following_plugin.cpp
+++ b/route_following_plugin/src/route_following_plugin.cpp
@@ -32,8 +32,8 @@ namespace route_following_plugin
 namespace {
 /**
  * \brief Anonymous function to extract maneuver end speed which can not be optained with GET_MANEUVER_PROPERY calls due to it missing in stop and wait plugin
- */
-double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) {
+ */ 
+double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) { 
     switch(mvr.type) {
         case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING:
             return mvr.lane_following_maneuver.end_speed;
@@ -54,7 +54,7 @@ double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) {
 
 /**
  * \brief Anonymous function to set the lanelet ids for all maneuver types except lane following
- */
+ */ 
 void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id start_id, lanelet::Id end_id) {
 
     switch(mvr.type) {
@@ -114,7 +114,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
     get_parameter<double>("vehicle_lateral_accel_limit", config_.lateral_accel_limit_);
     get_parameter<double>("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier_);
     get_parameter<double>("min_maneuver_length", config_.min_maneuver_length_);
-
+    
     RCLCPP_INFO_STREAM(rclcpp::get_logger("route_following_plugin"), "RouteFollowingPlugin Config: " << config_);
 
     // Setup subscribers
@@ -122,7 +122,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
                                                               std::bind(&RouteFollowingPlugin::twist_cb,this,std_ph::_1));
     current_maneuver_plan_sub_ = create_subscription<carma_planning_msgs::msg::ManeuverPlan>("maneuver_plan", 50,
                                                               std::bind(&RouteFollowingPlugin::current_maneuver_plan_cb,this,std_ph::_1));
-
+    
     // set world model point form wm listener
     wml_ = get_world_model_listener();
 
@@ -142,7 +142,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
     });
 
     initializeBumperTransformLookup();
-
+    
     // Return success if everthing initialized successfully
     return CallbackReturn::SUCCESS;
   }
@@ -156,27 +156,27 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         // Return success if everthing initialized successfully
         return CallbackReturn::SUCCESS;
     }
-
+    
     void RouteFollowingPlugin::twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg)
     {
         current_speed_ = msg->twist.linear.x;
     }
-
+    
     void RouteFollowingPlugin::current_maneuver_plan_cb(carma_planning_msgs::msg::ManeuverPlan::UniquePtr msg) {
         current_maneuver_plan_ = std::move(msg);
     }
 
     std::vector<carma_planning_msgs::msg::Maneuver> RouteFollowingPlugin::routeCb(const lanelet::routing::LaneletPath &route_shortest_path)
     {
-
-
+        
+        
         for (const auto& ll:route_shortest_path)
         {
             shortest_path_set_.insert(ll.id());
         }
         std::vector<carma_planning_msgs::msg::Maneuver> maneuvers;
         //This function calculates the maneuver plan every time the route is set
-        RCLCPP_ERROR_STREAM(get_logger(),"New route created");
+        RCLCPP_DEBUG_STREAM(get_logger(),"New route created");
 
         //Go through entire route - identify lane changes and fill in the spaces with lane following
         auto nearest_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc_, 10); //Return 10 nearest lanelets
@@ -198,22 +198,22 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         //Find lane changes in path - up to the second to last lanelet in path (till lane change is possible)
         for (shortest_path_index = 0; shortest_path_index < route_shortest_path.size() - 1; ++shortest_path_index)
         {
-            RCLCPP_ERROR_STREAM(get_logger(),"current shortest_path_index:" << shortest_path_index);
+            RCLCPP_DEBUG_STREAM(get_logger(),"current shortest_path_index:" << shortest_path_index);
 
             auto following_lanelets = wm_->getRoute()->followingRelations(route_shortest_path[shortest_path_index]);
-            RCLCPP_ERROR_STREAM(get_logger(),"following_lanelets.size():" << following_lanelets.size());
+            RCLCPP_DEBUG_STREAM(get_logger(),"following_lanelets.size():" << following_lanelets.size());
 
             double target_speed_in_lanelet = findSpeedLimit(route_shortest_path[shortest_path_index]);
 
             //update start distance and start speed from previous maneuver if it exists
             start_dist = (maneuvers.empty()) ? wm_->routeTrackPos(route_shortest_path[shortest_path_index].centerline2d().front()).downtrack : GET_MANEUVER_PROPERTY(maneuvers.back(), end_dist); // TODO_REFAC if there is no initial maneuver start distance and start speed should be derived from current state. Current state ought to be provided in planning request
             start_speed = (maneuvers.empty()) ? 0.0 : getManeuverEndSpeed(maneuvers.back());
-            RCLCPP_ERROR_STREAM(get_logger(),"start_dist:" << start_dist << ", start_speed:" << start_speed);
+            RCLCPP_DEBUG_STREAM(get_logger(),"start_dist:" << start_dist << ", start_speed:" << start_speed);
 
             end_dist = wm_->routeTrackPos(route_shortest_path[shortest_path_index].centerline2d().back()).downtrack;
-            RCLCPP_ERROR_STREAM(get_logger(),"end_dist:" << end_dist);
+            RCLCPP_DEBUG_STREAM(get_logger(),"end_dist:" << end_dist);
             end_dist = std::min(end_dist, route_length);
-            RCLCPP_ERROR_STREAM(get_logger(),"min end_dist:" << end_dist);
+            RCLCPP_DEBUG_STREAM(get_logger(),"min end_dist:" << end_dist);
 
             if (std::fabs(start_dist - end_dist) < 0.1) //TODO: edge case that was not recreatable. Sometimes start and end dist was same which crashes inlanecruising
             {
@@ -226,10 +226,10 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
 
             if (isLaneChangeNeeded(following_lanelets, route_shortest_path[shortest_path_index + 1].id()))
             {
-                RCLCPP_ERROR_STREAM(get_logger(),"LaneChangeNeeded");
-
+                RCLCPP_DEBUG_STREAM(get_logger(),"LaneChangeNeeded");
+    
                 // Determine the Lane Change Status
-                RCLCPP_ERROR_STREAM(get_logger(),"Recording lanechange start_dist <<" << start_dist  << ", from llt id:" << route_shortest_path[shortest_path_index].id() << " to llt id: " <<
+                RCLCPP_DEBUG_STREAM(get_logger(),"Recording lanechange start_dist <<" << start_dist  << ", from llt id:" << route_shortest_path[shortest_path_index].id() << " to llt id: " << 
                     route_shortest_path[shortest_path_index+ 1].id());
 
                 maneuvers.push_back(composeLaneChangeManeuverMessage(start_dist, end_dist, start_speed, target_speed_in_lanelet, route_shortest_path[shortest_path_index].id(), route_shortest_path[shortest_path_index + 1].id()));
@@ -238,7 +238,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
             }
             else
             {
-                RCLCPP_ERROR_STREAM(get_logger(),"Lanechange NOT Needed ");
+                RCLCPP_DEBUG_STREAM(get_logger(),"Lanechange NOT Needed ");
                 maneuvers.push_back(composeLaneFollowingManeuverMessage(start_dist, end_dist, start_speed, target_speed_in_lanelet, { route_shortest_path[shortest_path_index].id() } ));
             }
         }
@@ -255,15 +255,15 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
 
             // Add stop and wait maneuver based on deceleration target and entry speed
             maneuvers = addStopAndWaitAtRouteEnd( maneuvers, route_length, stopping_entry_speed, stopping_accel_limit, config_.lateral_accel_limit_, config_.min_maneuver_length_ );
-
+        
         }
         ////------------------
-        RCLCPP_ERROR_STREAM(get_logger(),"Maneuver plan along route successfully generated");
+        RCLCPP_DEBUG_STREAM(get_logger(),"Maneuver plan along route successfully generated");
         return maneuvers;
     }
 
     std::vector<carma_planning_msgs::msg::Maneuver> RouteFollowingPlugin::addStopAndWaitAtRouteEnd (
-        const std::vector<carma_planning_msgs::msg::Maneuver>& input_maneuvers,
+        const std::vector<carma_planning_msgs::msg::Maneuver>& input_maneuvers, 
         double route_end_downtrack, double stopping_entry_speed, double stopping_logitudinal_accel,
         double lateral_accel_limit, double min_maneuver_length
     ) const
@@ -276,20 +276,20 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
          * 3. If end downtrack - stop distance is > last maneuver end distance then fill delta with lane follow followed by stop and wait
          * 4. If end downtrack - stop distance is < last maneuver end distance then reduce the existing maneuver to allow for stopping maneuver to be planned
          * 5. Add maneuver to list
-         */
+         */ 
         std::vector<carma_planning_msgs::msg::Maneuver> maneuvers = input_maneuvers; // Output maneuvers which will be modified
 
 
         // Compute stopping distance where v_f = 0
-        // (v_f^2 - v_i^2) / (2*a) = d
+        // (v_f^2 - v_i^2) / (2*a) = d 
         double stopping_distance = 0.5 * (stopping_entry_speed * stopping_entry_speed) / stopping_logitudinal_accel;
 
         // Compute required starting downtrack for maneuver
         double required_start_downtrack = std::max(0.0, route_end_downtrack - stopping_distance);
 
         // Loop to drop any maneuvers which fully overlap our stopping maneuver while accounting for minimum length maneuver buffers
-        while ( !maneuvers.empty() && maneuverWithBufferStartsAfterDowntrack(maneuvers.back(), required_start_downtrack, lateral_accel_limit, config_.min_maneuver_length_) ) {
-
+        while ( !maneuvers.empty() && maneuverWithBufferStartsAfterDowntrack(maneuvers.back(), required_start_downtrack, lateral_accel_limit, config_.min_maneuver_length_) ) { 
+            
             RCLCPP_WARN_STREAM(get_logger(),"Dropping maneuver with id: " <<  GET_MANEUVER_PROPERTY(maneuvers.back(), parameters.maneuver_id) );
 
             if (maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE) {
@@ -302,21 +302,21 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
 
         }
 
-        double last_maneuver_end_downtrack =  required_start_downtrack; // Set default starting location for stop and wait maneuver
+        double last_maneuver_end_downtrack =  required_start_downtrack; // Set default starting location for stop and wait maneuver 
 
         if ( !maneuvers.empty() ) { // If there are existing maneuvers we need to make sure stop and wait does not overwrite them
-
+            
             last_maneuver_end_downtrack = GET_MANEUVER_PROPERTY(maneuvers.back(), end_dist);
-
+            
             // If our stopping maneuver does not intersect with existing maneuvers
-            if ( required_start_downtrack >=  last_maneuver_end_downtrack ) {
-
+            if ( required_start_downtrack >=  last_maneuver_end_downtrack ) {  
+                
                 // If the delta is under minimum_maneuver_length we can just extend the stopping maneuver
                 // Otherwise add a new lane follow maneuver
                 if (required_start_downtrack - last_maneuver_end_downtrack > min_maneuver_length) {
-
+                 
                     // Identify the lanelets which will be crossed by this lane follow maneuver
-                    std::vector<lanelet::ConstLanelet> crossed_lanelets = wm_->getLaneletsBetween(last_maneuver_end_downtrack, required_start_downtrack, true, false);
+                    std::vector<lanelet::ConstLanelet> crossed_lanelets = wm_->getLaneletsBetween(last_maneuver_end_downtrack, required_start_downtrack, true, false); 
 
                     if (crossed_lanelets.empty()) {
                         throw std::invalid_argument("The new lane follow maneuver does not cross any lanelets going from: " + std::to_string(last_maneuver_end_downtrack) + " to: " + std::to_string(required_start_downtrack));
@@ -324,16 +324,16 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
 
                     // Create the lane follow maneuver
                     maneuvers.push_back(composeLaneFollowingManeuverMessage(last_maneuver_end_downtrack, required_start_downtrack, stopping_entry_speed, stopping_entry_speed, lanelet::utils::transform(crossed_lanelets, [](auto ll) { return ll.id(); })));
-
+                    
                     // Update last maneuver end downtrack so the stop and wait maneuver can be properly formulated
-                    last_maneuver_end_downtrack = required_start_downtrack;
+                    last_maneuver_end_downtrack = required_start_downtrack; 
                 } else {
-                    RCLCPP_ERROR_STREAM(get_logger(),"Stop and wait maneuver being extended to nearest maneuver which is closer than the minimum maneuver length");
+                    RCLCPP_DEBUG_STREAM(get_logger(),"Stop and wait maneuver being extended to nearest maneuver which is closer than the minimum maneuver length");
                 }
 
             } else { // If our stopping maneuver intersects with existing maneuvers
 
-
+                                    
                 SET_MANEUVER_PROPERTY(maneuvers.back(), end_dist, required_start_downtrack);
 
                 // Identify the lanelets which will be crossed by this updated maneuver
@@ -356,9 +356,9 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
 
                 last_maneuver_end_downtrack = required_start_downtrack;
 
-            }
+            }   
         }
-
+        
         // Identify the lanelets which will be crossed by this stop and wait maneuver
         std::vector<lanelet::ConstLanelet> crossed_lanelets = wm_->getLaneletsBetween(last_maneuver_end_downtrack, route_end_downtrack, true, false);
 
@@ -379,24 +379,24 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
     }
 
     bool RouteFollowingPlugin::maneuverWithBufferStartsAfterDowntrack(const carma_planning_msgs::msg::Maneuver& maneuver, double downtrack, double lateral_accel, double min_maneuver_length) const {
-
+       
         if (maneuver.type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE) {
-
+                
             // Compute the time it takes to move laterally to the next lane
             double lane_change_time = sqrt(0.5 * MAX_LANE_WIDTH / lateral_accel);
-
+            
             // Compute logitudinal distance covered in lane change time
             double min_lane_change_distance = std::max(
-                min_maneuver_length,
+                min_maneuver_length, 
                 lane_change_time * (GET_MANEUVER_PROPERTY(maneuver, start_speed) + getManeuverEndSpeed(maneuver)) / 2.0 // dist = v_avg * t
             );
 
             return GET_MANEUVER_PROPERTY(maneuver, start_dist) + min_lane_change_distance > downtrack;
 
-        } else {
-
-            return GET_MANEUVER_PROPERTY(maneuver, start_dist) + min_maneuver_length > downtrack;
-
+        } else { 
+            
+            return GET_MANEUVER_PROPERTY(maneuver, start_dist) + min_maneuver_length > downtrack; 
+        
         }
     }
 
@@ -404,15 +404,15 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         {
          return true;
         }
-
+    
         std::string RouteFollowingPlugin::get_version_id()
         {
                 return "v1.0";
         }
 
     void  RouteFollowingPlugin::plan_maneuvers_callback(
-      std::shared_ptr<rmw_request_id_t> srv_header,
-      carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req,
+      std::shared_ptr<rmw_request_id_t> srv_header, 
+      carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, 
       carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp)
       {
 
@@ -423,18 +423,18 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         }
 
         double current_downtrack;
-
+        
         if (!req->prior_plan.maneuvers.empty())
         {
             current_downtrack = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_dist);
-            RCLCPP_ERROR_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end_dist:"<< current_downtrack);
+            RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end_dist:"<< current_downtrack);            
         }
         else
         {
             current_downtrack = req->veh_downtrack;
-            RCLCPP_ERROR_STREAM(get_logger(),"Detected NO prior plan! Using req.veh_downtrack: "<< current_downtrack);
+            RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using req.veh_downtrack: "<< current_downtrack);
         }
-
+        
         //Return the set of maneuvers which intersect with min_plan_duration
         size_t i = 0;
         double planned_time = 0.0;
@@ -443,11 +443,11 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
 
         while (planned_time < config_.min_plan_duration_ && i < latest_maneuver_plan_.size())
         {
-            RCLCPP_ERROR_STREAM(get_logger(),"Checking maneuver id " << i);
+            RCLCPP_DEBUG_STREAM(get_logger(),"Checking maneuver id " << i);
             //Ignore plans for distance already covered
             if (GET_MANEUVER_PROPERTY(latest_maneuver_plan_[i], end_dist) <= current_downtrack)
             {
-                RCLCPP_ERROR_STREAM(get_logger(),"Skipping maneuver id " << i);
+                RCLCPP_DEBUG_STREAM(get_logger(),"Skipping maneuver id " << i);
 
                 ++i;
                 continue;
@@ -472,13 +472,13 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         if (!req->prior_plan.maneuvers.empty())
         {
             updateTimeProgress(new_maneuvers, GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time));
-           RCLCPP_ERROR_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end time:"<< std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time)).seconds()));
-           RCLCPP_ERROR_STREAM(get_logger(),"Where plan_completion_time was:"<< std::to_string(rclcpp::Time(req->prior_plan.planning_completion_time).seconds()));
+           RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end time:"<< std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time)).seconds()));    
+           RCLCPP_DEBUG_STREAM(get_logger(),"Where plan_completion_time was:"<< std::to_string(rclcpp::Time(req->prior_plan.planning_completion_time).seconds()));            
         }
         else
         {
             updateTimeProgress(new_maneuvers, rclcpp::Time(req->header.stamp));
-           RCLCPP_ERROR_STREAM(get_logger(),"Detected NO prior plan! Using this->now():"<< std::to_string(this->now().seconds()));
+           RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using this->now():"<< std::to_string(this->now().seconds()));   
         }
 
         //update starting speed of first maneuver
@@ -505,24 +505,24 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
                 default:
                     throw std::invalid_argument("Invalid maneuver type, cannot update starting speed for maneuver");
             }
-
+            
             updateStartingSpeed(new_maneuvers.front(), start_speed);
-            RCLCPP_ERROR_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end speed:"<< start_speed);
+            RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end speed:"<< start_speed);    
         }
-        else
+        else 
         {
             updateStartingSpeed(new_maneuvers.front(), req->veh_logitudinal_velocity);
-            RCLCPP_ERROR_STREAM(get_logger(),"Detected NO prior plan! Using req->veh_logitudinal_velocity:"<< req->veh_logitudinal_velocity);
+            RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using req->veh_logitudinal_velocity:"<< req->veh_logitudinal_velocity);    
         }
         //update plan
         resp->new_plan = req->prior_plan;
-        RCLCPP_ERROR_STREAM(get_logger(),"Updating maneuvers before returning... Prior plan size:" << req->prior_plan.maneuvers.size());
+        RCLCPP_DEBUG_STREAM(get_logger(),"Updating maneuvers before returning... Prior plan size:" << req->prior_plan.maneuvers.size());
         for (const auto& mvr : new_maneuvers)
         {
             resp->new_plan.maneuvers.push_back(mvr);
         }
 
-        RCLCPP_ERROR_STREAM(get_logger(),"Returning total of maneuver size: " << resp->new_plan.maneuvers.size());
+        RCLCPP_DEBUG_STREAM(get_logger(),"Returning total of maneuver size: " << resp->new_plan.maneuvers.size());
         resp->new_plan.planning_completion_time = this->now();
 
         return;
@@ -530,10 +530,10 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
 
     void RouteFollowingPlugin::bumper_pose_cb()
     {
-        RCLCPP_ERROR_STREAM(get_logger(),"Entering pose_cb");
-
-        RCLCPP_ERROR_STREAM(get_logger(),"Looking up front bumper pose...");
+        RCLCPP_DEBUG_STREAM(get_logger(),"Entering pose_cb");
 
+        RCLCPP_DEBUG_STREAM(get_logger(),"Looking up front bumper pose...");
+        
         try
         {
             tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(1, 0)); //save to local copy of transform 1 sec timeout
@@ -547,23 +547,23 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         geometry_msgs::msg::Pose front_bumper_pose;
         front_bumper_pose.position.x = frontbumper_transform_.getOrigin().getX();
         front_bumper_pose.position.y = frontbumper_transform_.getOrigin().getY();
-
+        
         if (!wm_->getRoute())
             return;
 
         lanelet::BasicPoint2d current_loc(front_bumper_pose.position.x, front_bumper_pose.position.y);
         current_loc_ = current_loc;
         double current_progress = wm_->routeTrackPos(current_loc).downtrack;
-
-        RCLCPP_ERROR_STREAM(get_logger(),"pose_cb : current_progress" << current_progress);
-
+        
+        RCLCPP_DEBUG_STREAM(get_logger(),"pose_cb : current_progress" << current_progress);
+        
         // Check if we need to return to route shortest path.
         // Step 1. Check if another plugin aside from RFP has been in control
         if (current_maneuver_plan_  != nullptr &&
             GET_MANEUVER_PROPERTY(current_maneuver_plan_->maneuvers[0], parameters.planning_strategic_plugin) \
             != planning_strategic_plugin_) {
             // If another plugin may have brought us off shortest path, check our current lanelet
-            auto llts = wm_->getLaneletsFromPoint(current_loc, 10);
+            auto llts = wm_->getLaneletsFromPoint(current_loc, 10);                                          
             // Remove any candidate lanelets not on the route
             llts.erase(std::remove_if(llts.begin(), llts.end(),
                 [&](auto lanelet) -> bool { return !wm_->getRoute()->contains(lanelet); }),
@@ -584,23 +584,13 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
                 throw std::domain_error("Vehicle not on route, unable to compute shortest path.");
             }
 
-            auto current_lanelet = llts[0];
-            // get the lanelet that is on the route in case overlapping ones found
-            for (const auto& check_llt : llts)
-            {
-                auto route = wm_->getRoute()->shortestPath();
-                if (std::find(route.begin(), route.end(), check_llt) != route.end())
-                {
-                    current_lanelet = check_llt;
-                    break;
-                }
-            }
-            RCLCPP_ERROR_STREAM(get_logger(), "Vehicle is currently in lanelet " << current_lanelet.id());
+            const auto& current_lanelet = llts[0];
+            RCLCPP_DEBUG_STREAM(get_logger(), "Vehicle is currently in lanelet " << current_lanelet.id());
 
             // if the current lanelet is not on the shortest path
             if (shortest_path_set_.find(current_lanelet.id()) == shortest_path_set_.end())
             {
-                RCLCPP_ERROR_STREAM(get_logger(), "Generating a new shortest path since the vehicle is not on the shortest path");
+                RCLCPP_DEBUG_STREAM(get_logger(), "Generating a new shortest path since the vehicle is not on the shortest path");
                 returnToShortestPath(current_lanelet);
             }
 
@@ -608,7 +598,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
             for(size_t i = 0; i < latest_maneuver_plan_.size(); ++i){
                 if((GET_MANEUVER_PROPERTY(latest_maneuver_plan_[i], start_dist) <= current_progress) && (current_progress <= GET_MANEUVER_PROPERTY(latest_maneuver_plan_[i], end_dist))){
                     if(latest_maneuver_plan_[i].type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE){
-                        RCLCPP_ERROR_STREAM(get_logger(), "Generating a new shortest path since vehicle is positioned in a lane change but the previous maneuver plan was not generated by route_following_plugin");
+                        RCLCPP_DEBUG_STREAM(get_logger(), "Generating a new shortest path since vehicle is positioned in a lane change but the previous maneuver plan was not generated by route_following_plugin");
                         returnToShortestPath(current_lanelet);
                     }
                 }
@@ -628,7 +618,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         double maneuver_start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
         double maneuver_end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);
 
-        RCLCPP_ERROR_STREAM(get_logger(),"maneuver_end_dist: " << maneuver_end_dist << ", maneuver_start_dist: " << maneuver_start_dist << ", cur_plus_target: " << cur_plus_target);
+        RCLCPP_DEBUG_STREAM(get_logger(),"maneuver_end_dist: " << maneuver_end_dist << ", maneuver_start_dist: " << maneuver_start_dist << ", cur_plus_target: " << cur_plus_target);
 
         duration = rclcpp::Duration((maneuver_end_dist - maneuver_start_dist) / (0.5 * cur_plus_target) * 1e9);
 
@@ -754,9 +744,9 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         for (const auto& id : maneuver_msg.lane_following_maneuver.lane_ids)
             ss << " " << id;
 
-        RCLCPP_ERROR_STREAM(get_logger(),"Creating lane follow id: " << maneuver_msg.lane_following_maneuver.parameters.maneuver_id
+        RCLCPP_DEBUG_STREAM(get_logger(),"Creating lane follow id: " << maneuver_msg.lane_following_maneuver.parameters.maneuver_id 
                         << " start dist: " << start_dist << " end dist: " << end_dist << "lane_ids: " << ss.str());
-
+        
         return maneuver_msg;
     }
 
@@ -780,14 +770,14 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         //       If maneuvers were not generated only on route updates we would want to preserve the ids across plans
         maneuver_msg.lane_change_maneuver.parameters.maneuver_id = getNewManeuverId();
 
-        RCLCPP_ERROR_STREAM(get_logger(),"Creating lane change id: "  << maneuver_msg.lane_change_maneuver.parameters.maneuver_id << "start dist: " << start_dist << " end dist: " << end_dist << " Starting llt: " << starting_lane_id << " Ending llt: " << ending_lane_id);
+        RCLCPP_DEBUG_STREAM(get_logger(),"Creating lane change id: "  << maneuver_msg.lane_change_maneuver.parameters.maneuver_id << "start dist: " << start_dist << " end dist: " << end_dist << " Starting llt: " << starting_lane_id << " Ending llt: " << ending_lane_id);
 
         return maneuver_msg;
     }
 
     std::string RouteFollowingPlugin::getNewManeuverId() const {
         static auto gen = boost::uuids::random_generator(); // Initialize uuid generator
-
+        
         return boost::lexical_cast<std::string>(gen()); // generate uuid and convert to string
     }
 
@@ -810,12 +800,12 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         // Set the meta-data for the StopAndWait Maneuver to define the buffer in the route end point stopping location
         maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.route_end_point_buffer_);
         maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
-
+        
         // NOTE: The maneuver id is set here because maneuvers are regenerated once per route, so it is acceptable to regenerate them on route updates.
         //       If maneuvers were not generated only on route updates we would want to preserve the ids across plans
         maneuver_msg.stop_and_wait_maneuver.parameters.maneuver_id = getNewManeuverId();
 
-        RCLCPP_ERROR_STREAM(get_logger(),"Creating stop and wait maneuver id:" << maneuver_msg.stop_and_wait_maneuver.parameters.maneuver_id << "start dist: " << start_dist << " end dist: " << end_dist << " start_speed: " << start_speed << " Starting llt: " << starting_lane_id << " Ending llt: " << ending_lane_id);
+        RCLCPP_DEBUG_STREAM(get_logger(),"Creating stop and wait maneuver id:" << maneuver_msg.stop_and_wait_maneuver.parameters.maneuver_id << "start dist: " << start_dist << " end dist: " << end_dist << " start_speed: " << start_speed << " Starting llt: " << starting_lane_id << " Ending llt: " << ending_lane_id);
 
         return maneuver_msg;
     }
@@ -846,7 +836,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         }
     }
 
-    void RouteFollowingPlugin::initializeBumperTransformLookup()
+    void RouteFollowingPlugin::initializeBumperTransformLookup() 
     {
         tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_));
         tf2_buffer_.setUsingDedicatedThread(true);
@@ -855,9 +845,9 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
     void RouteFollowingPlugin::returnToShortestPath(const lanelet::ConstLanelet &current_lanelet)
     {
         auto original_shortestpath = wm_->getRoute()->shortestPath();
-        RCLCPP_ERROR_STREAM(get_logger(),"The vehicle has left the shortest path");
+        RCLCPP_DEBUG_STREAM(get_logger(),"The vehicle has left the shortest path");
         auto routing_graph = wm_->getMapRoutingGraph();
-
+        
         // Obtain the lanelet following the current lanelet in this same lane, this lanelet must exist since the new shortest path cannot begin with a lane change
         auto current_lane_following_lanelets = routing_graph->following(current_lanelet);
         lanelet::ConstLanelet current_lane_following_lanelet;
@@ -879,13 +869,13 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
                 {
                     auto following_lanelets = routing_graph->following(adjacent);
                     const auto& target_following_lanelet = following_lanelets[0];
-                    RCLCPP_ERROR_STREAM(get_logger(),"The target_following_lanelet id is: " << target_following_lanelet.id());
+                    RCLCPP_DEBUG_STREAM(get_logger(),"The target_following_lanelet id is: " << target_following_lanelet.id());
                     lanelet::ConstLanelets interm;
                     interm.push_back(static_cast<lanelet::ConstLanelet>(current_lane_following_lanelet));
                     interm.push_back(static_cast<lanelet::ConstLanelet>(target_following_lanelet));
                     // a new shortest path, via the the lanelets in 'interm' is calculated and used an alternative shortest path
                     auto new_shortestpath = routing_graph->shortestPathVia(current_lanelet, interm, original_shortestpath.back());
-                    RCLCPP_ERROR_STREAM(get_logger(),"a new shortestpath is generated to return to original shortestpath");
+                    RCLCPP_DEBUG_STREAM(get_logger(),"a new shortestpath is generated to return to original shortestpath");
                     // routeCb is called to update latest_maneuver_plan_
                     if (new_shortestpath) this->latest_maneuver_plan_ = routeCb(new_shortestpath.get());
                     break;
@@ -896,12 +886,12 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
                     lanelet::ConstLanelets new_interm;
                     new_interm.push_back(static_cast<lanelet::ConstLanelet>(current_lane_following_lanelet));
                     auto new_shortestpath = routing_graph->shortestPathVia(current_lanelet, new_interm, original_shortestpath.back());
-                    RCLCPP_ERROR_STREAM(get_logger(),"Cannot return to the original shortestpath from adjacent lanes, so a new shortestpath is generated");
+                    RCLCPP_DEBUG_STREAM(get_logger(),"Cannot return to the original shortestpath from adjacent lanes, so a new shortestpath is generated");
                     // routeCb is called to update latest_maneuver_plan_
                     if (new_shortestpath) this->latest_maneuver_plan_ = routeCb(new_shortestpath.get());
                 }
             }
-
+            
         }
         else
         {
@@ -909,7 +899,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id
         }
 
     }
-
+              
 }
 
 #include "rclcpp_components/register_node_macro.hpp"
diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp
index c78e59e68d..9cc9a55cf7 100644
--- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp
+++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp
@@ -154,7 +154,7 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R
   }
   else
   {
-    RCLCPP_ERROR(rclcpp::get_logger("stop_and_wait_plugin"), "Ignored Object Avoidance");
+    RCLCPP_DEBUG(rclcpp::get_logger("stop_and_wait_plugin"), "Ignored Object Avoidance");
   }
 
   std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now();  // Planning complete
diff --git a/yield_plugin/config/parameters.yaml b/yield_plugin/config/parameters.yaml
index 58b8645219..fe7e5b5264 100644
--- a/yield_plugin/config/parameters.yaml
+++ b/yield_plugin/config/parameters.yaml
@@ -2,7 +2,7 @@
 # Adjustment factor for safe and comfortable acceleration/deceleration
 # Value type: Desired
 # No unit
-acceleration_adjustment_factor: 2.0
+acceleration_adjustment_factor: 4.0
 # Time horizon for collision detection for vehicles on the route
 # NOTE: not applied for pedestrians or other non-vehicle obstacles on the road
 # Units: s
@@ -11,7 +11,7 @@ on_route_vehicle_collision_horizon: 10.0
 # Minimum speed for moving obstacle
 # Units: m/s
 # Value type: Desired
-min_obstacle_speed: 3.0
+min_obstacle_speed: 0.5
 # Minimum object avoidance planning time
 # Units: s
 # Value type: Desired
@@ -24,6 +24,18 @@ yield_max_deceleration: 3.0
 # Units: meters
 # Value type: Desired
 x_gap: 10.0
+# Host vehicle length
+# Units: m
+# Value type: Measured
+vehicle_length: 5.0
+# Host vehicle width
+# Units: m
+# Value type: Measured
+vehicle_width: 2.5
+# Host vehicle height
+# Units: m
+# Value type: Measured
+vehicle_height: 3.0
 # Maximum speed value to consider the ego vehicle stopped
 # Units: m/s
 # Value type: Desired
diff --git a/yield_plugin/include/yield_plugin/yield_plugin.hpp b/yield_plugin/include/yield_plugin/yield_plugin.hpp
index 5b85542ae9..9979a219f8 100644
--- a/yield_plugin/include/yield_plugin/yield_plugin.hpp
+++ b/yield_plugin/include/yield_plugin/yield_plugin.hpp
@@ -53,7 +53,7 @@ using LaneChangeStatusCB = std::function<void(const carma_planning_msgs::msg::La
 
 /**
  * \brief Convenience class for pairing 2d points with speeds
- */
+ */ 
 struct PointSpeedPair
 {
   lanelet::BasicPoint2d point;
@@ -62,19 +62,19 @@ struct PointSpeedPair
 
 /**
  * \brief Class containing primary business logic for the In-Lane Cruising Plugin
- *
- */
+ * 
+ */ 
 class YieldPlugin
 {
 public:
   /**
    * \brief Constructor
-   *
+   * 
    * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data
    * \param config The configuration to be used for this object
    * \param mobility_response_publisher Callback which will publish the mobility response
    * \param lc_status_publisher Callback which will publish the cooperative lane change status
-   */
+   */ 
   YieldPlugin(std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh, carma_wm::WorldModelConstPtr wm, YieldPluginConfig config,
                        MobilityResponseCB mobility_response_publisher,
                        LaneChangeStatusCB lc_status_publisher);
@@ -84,17 +84,17 @@ class YieldPlugin
    * \param srv_header header
    * \param req The service request
    * \param resp The service response
-   *
-   */
+   * 
+   */ 
   void plan_trajectory_callback(
-    carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
+    carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, 
     carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp);
 
   /**
    * \brief trajectory is modified to safely avoid obstacles on the road
    * \param original_tp original trajectory plan without object avoidance
    * \param current_speed_ current speed of the vehicle
-   * \param
+   * \param 
    * \return modified trajectory plan
    */
   carma_planning_msgs::msg::TrajectoryPlan update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double initial_velocity);
@@ -124,20 +124,20 @@ class YieldPlugin
 
   /**
    * \brief calculates distance between trajectory points in a plan
-   * \param trajectory_plan input trajectory plan
+   * \param trajectory_plan input trajectory plan 
    * \return vector of relative distances between trajectory points
-   */
-  std::vector<double> get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const;
+   */                     
+  std::vector<double> get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const;  
 
   /**
    * \brief callback for mobility request
-   * \param msg mobility request message
+   * \param msg mobility request message 
    */
   void mobilityrequest_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg);
 
   /**
    * \brief callback for bsm message
-   * \param msg mobility bsm message
+   * \param msg mobility bsm message 
    */
   void bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg);
 
@@ -148,11 +148,11 @@ class YieldPlugin
    * \return vector of 2d points in map frame
    */
   std::vector<lanelet::BasicPoint2d> convert_eceftrajectory_to_mappoints(const carma_v2x_msgs::msg::Trajectory& ecef_trajectory) const;
-
+  
   /**
    * \brief convert a point in ecef frame (in cm) into map frame (in meters)
    * \param ecef_point carma point ecef frame in cm
-   * \param map_in_earth translate frame
+   * \param map_in_earth translate frame 
    * \return 2d point in map frame
    */
   lanelet::BasicPoint2d ecef_to_map_point(const carma_v2x_msgs::msg::LocationECEF& ecef_point) const;
@@ -165,7 +165,7 @@ class YieldPlugin
    * \return filled mobility response
    */
   carma_v2x_msgs::msg::MobilityResponse compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const;
-
+  
   /**
    * \brief generate a Jerk Minimizing Trajectory(JMT) with the provided start and end conditions
    * \param original_tp original trajectory plan
@@ -174,10 +174,10 @@ class YieldPlugin
    * \param initial_velocity start velocity
    * \param goal_velocity end velocity
    * \param planning_time time duration of the planning
-   * \return updated JMT trajectory
+   * \return updated JMT trajectory 
    */
   carma_planning_msgs::msg::TrajectoryPlan generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time);
-
+  
   /**
    * \brief update trajectory for yielding to an incoming cooperative behavior
    * \param original_tp original trajectory plan
@@ -193,14 +193,14 @@ class YieldPlugin
    * \return vector of pairs of 2d intersection points and index of the point in trajectory array
    */
   std::vector<std::pair<int, lanelet::BasicPoint2d>> detect_trajectories_intersection(std::vector<lanelet::BasicPoint2d> self_trajectory, std::vector<lanelet::BasicPoint2d> incoming_trajectory) const;
-
+  
 
   /**
    * \brief set values for member variables related to cooperative behavior
    * \param req_trajectory requested trajectory
    * \param req_speed speed of requested cooperative behavior
    * \param req_planning_time planning time for the requested cooperative behavior
-   * \param req_timestamp the mobility request time stamp
+   * \param req_timestamp the mobility request time stamp 
    */
   void set_incoming_request_info(std::vector <lanelet::BasicPoint2d> req_trajectory, double req_speed, double req_planning_time, double req_timestamp);
 
@@ -219,29 +219,24 @@ class YieldPlugin
   /**
    * \brief Setter for map projection string to define lat/lon -> map conversion
    * \param georeference The proj string defining the projection.
-   */
+   */ 
   void set_georeference_string(const std::string& georeference);
 
   /**
    * \brief Setter for external objects with predictions in the environment
    * \param object_list The object list.
-   */
+   */ 
   void set_external_objects(const std::vector<carma_perception_msgs::msg::ExternalObject>& object_list);
 
   /**
    * \brief Return collision time given two trajectories
    * \param trajectory1 trajectory of the ego vehicle
-   * \param trajectory2 trajectory of the obstacle
+   * \param trajectory2 trajectory of the obstacle 
    * \param collision_radius a distance to check between two trajectory points at a same timestamp that is considered a collision
    * \return time_of_collision if collision detected, otherwise, std::nullopt
-   */
+   */ 
   std::optional<rclcpp::Time> detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector<carma_perception_msgs::msg::PredictedState>& trajectory2, double collision_radius);
 
-
-  // TODO
-  std::optional<std::pair<carma_perception_msgs::msg::ExternalObject, double>> get_earliest_collision_object_and_time(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects);
-  double get_object_velocity_along_trajectory(const geometry_msgs::msg::Twist& object_velocity_in_map_frame, const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double collision_timestamp_in_seconds);
-
 private:
 
   carma_wm::WorldModelConstPtr wm_;
@@ -284,4 +279,4 @@ class YieldPlugin
   }
 
 };
-}  // namespace yield_plugin
+};  // namespace yield_plugin
diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp
index 07f5485d7c..f63e9de099 100644
--- a/yield_plugin/src/yield_plugin.cpp
+++ b/yield_plugin/src/yield_plugin.cpp
@@ -373,7 +373,7 @@ namespace yield_plugin
     for(size_t i = 1; i < original_tp.trajectory_points.size(); i++ )
     {
       double traj_target_time = i * planning_time / original_tp.trajectory_points.size();
-      double dt_dist = polynomial_calc(values, traj_target_time); // TODO WHY NOT USED?
+      double dt_dist = polynomial_calc(values, traj_target_time);
       double dv = polynomial_calc_d(values, traj_target_time);
 
       if (dv >= original_max_speed)
@@ -417,11 +417,9 @@ namespace yield_plugin
     jmt_trajectory.header = original_tp.header;
     jmt_trajectory.trajectory_id = original_tp.trajectory_id;
     jmt_trajectory.trajectory_points = jmt_trajectory_points;
-    jmt_trajectory.initial_longitudinal_velocity = initial_velocity;
     return jmt_trajectory;
   }
 
-
   std::optional<rclcpp::Time> YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector<carma_perception_msgs::msg::PredictedState>& trajectory2, double collision_radius)
   {
     // Iterate through each pair of consecutive points in the trajectories
@@ -519,10 +517,7 @@ namespace yield_plugin
         }
         else
         {
-
-          RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected at timestamp " << std::to_string(p2a_t) << ", x: " << x1 << ", y: " << y1 <<
-            ", within actual downtrack distance: " << object_downtrack - vehicle_downtrack <<
-            ", and collision distance: " << distance);
+          RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected at timestamp " << std::to_string(p2a_t) << ", x: " << x1 << ", y: " << y1 << ", within distance: " << distance);
           return rclcpp::Time(p2a.header.stamp);
         }
       }
@@ -532,19 +527,35 @@ namespace yield_plugin
     return std::nullopt;
   }
 
-  std::optional<std::pair<carma_perception_msgs::msg::ExternalObject, double>> YieldPlugin::get_earliest_collision_object_and_time(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects)
+  carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double initial_velocity)
   {
-    std::optional<carma_perception_msgs::msg::ExternalObject> earliest_collision_obj = std::nullopt;
-    std::set<int> checked_external_object_ids;
+    if (original_tp.trajectory_points.size() < 2)
+    {
+      RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged...");
+      return original_tp;
+    }
+
+    if (original_tp.trajectory_points.empty())
+    {
+      RCLCPP_ERROR(nh_->get_logger(), "Yield plugin received empty trajectory plan in update_traj_for_object");
+      throw std::invalid_argument("Yield plugin received empty trajectory plan in update_traj_for_object");
+    }
+
     rclcpp::Time plan_start_time = original_tp.trajectory_points[0].target_time;
+    carma_planning_msgs::msg::TrajectoryPlan update_tpp_vector;
+    geometry_msgs::msg::Twist current_velocity;
+    current_velocity.linear.x = initial_velocity;
 
+    std::optional<carma_perception_msgs::msg::ExternalObject> earliest_collision_obj = std::nullopt;
+    std::set<int> checked_external_object_ids;
 
+    std::vector<carma_perception_msgs::msg::PredictedState> new_list;
     RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size());
 
     if (wm_->getRoute() == nullptr)
     {
-      RCLCPP_WARN(nh_->get_logger(), "Yield plugin was not able to analyze collision since route is not available! Please check if route is set");
-      return std::nullopt;
+      RCLCPP_WARN(nh_->get_logger(), "No route available!");
+      return original_tp; //route not available
     }
 
     // save route Ids for faster access
@@ -564,16 +575,12 @@ namespace yield_plugin
       // do not process outdated objects
       if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp) > plan_start_time)
       {
-        std::vector<carma_perception_msgs::msg::PredictedState> new_list;
         carma_perception_msgs::msg::PredictedState curr_state;
         // artificially include current position as one of the predicted states
         curr_state.header.stamp = curr_obstacle.header.stamp;
         curr_state.predicted_position.position.x = curr_obstacle.pose.pose.position.x;
         curr_state.predicted_position.position.y = curr_obstacle.pose.pose.position.y;
-        // NOTE: predicted_velocity is not used for collision calculation, but timestamps
         curr_state.predicted_velocity.linear.x = curr_obstacle.velocity.twist.linear.x;
-        curr_state.predicted_velocity.linear.y = curr_obstacle.velocity.twist.linear.y;
-        RCLCPP_ERROR_STREAM(nh_->get_logger(), "Object: " << curr_obstacle.id <<", type: " << static_cast<int>(curr_obstacle.object_type) << ", speed: " << curr_obstacle.velocity.twist.linear.x);
         new_list.push_back(curr_state);
         new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend());
 
@@ -593,144 +600,92 @@ namespace yield_plugin
       }
     }
 
-    if (earliest_collision_obj == std::nullopt)
-      return std::nullopt;
-
-    RCLCPP_ERROR_STREAM(nh_->get_logger(),"earliest object x: " << earliest_collision_obj.value().velocity.twist.linear.x << ", y: " << earliest_collision_obj.value().velocity.twist.linear.y);
-
-    return std::make_pair(earliest_collision_obj.value(), earliest_collision_obj_time);
-  }
-
-  double YieldPlugin::get_object_velocity_along_trajectory(const geometry_msgs::msg::Twist& object_velocity_in_map_frame, const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double collision_timestamp_in_seconds)
-  {
-    RCLCPP_ERROR_STREAM(nh_->get_logger(), "collision_timestamp_in_seconds: " << std::to_string(collision_timestamp_in_seconds) <<
-      ", original_tp.trajectory_points.back().target_time: " << std::to_string(rclcpp::Time(original_tp.trajectory_points.back().target_time).seconds()));
+    lanelet::BasicPoint2d point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y);
+    double vehicle_downtrack = wm_->routeTrackPos(point).downtrack;
 
-    for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; ++i)
-    {
-      auto p1a = original_tp.trajectory_points.at(i);
-      auto p1b = original_tp.trajectory_points.at(i + 1);
-      double p1b_t = rclcpp::Time(p1b.target_time).seconds();
+    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack);
 
-      if (p1b_t >= collision_timestamp_in_seconds)
-      {
-        // TODO GEOMETRY
-        auto dx = p1b.x - p1a.x;
-        auto dy = p1b.y - p1a.y;
-        tf2::Vector3 trajectory_direction(dx, dy, 0);
-        tf2::Vector3 object_direction(object_velocity_in_map_frame.linear.x, object_velocity_in_map_frame.linear.y, 0);
-
-        RCLCPP_ERROR_STREAM(nh_->get_logger(), "collision_timestamp_in_seconds: " << std::to_string(collision_timestamp_in_seconds)
-          << ", p1b_t: " << std::to_string(p1b_t)
-          << ", dx: " << dx << ", dy: " << dy << ", "
-          << ", object_velocity_in_map_frame.x: " << object_velocity_in_map_frame.linear.x
-          << ", object_velocity_in_map_frame.y: " << object_velocity_in_map_frame.linear.y);
-        auto return_value = static_cast<double>(tf2::tf2Dot(object_direction, trajectory_direction)) / trajectory_direction.length();
-        return return_value;
-      }
-    }
-    // TODO error
-    return 0.0;
-  }
+    // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route,
+    // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon.
 
-  carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double initial_velocity)
-  {
-    if (original_tp.trajectory_points.size() < 2)
+    if(earliest_collision_obj.has_value())
     {
-      RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged...");
-      return original_tp;
-    }
+      RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!");
 
-    if (original_tp.trajectory_points.empty())
-    {
-      RCLCPP_ERROR(nh_->get_logger(), "Yield plugin received empty trajectory plan in update_traj_for_object");
-      throw std::invalid_argument("Yield plugin received empty trajectory plan in update_traj_for_object");
-    }
+      lanelet::BasicPoint2d point_o(earliest_collision_obj.value().pose.pose.position.x, earliest_collision_obj.value().pose.pose.position.y);
+      double object_downtrack = wm_->routeTrackPos(point_o).downtrack;
 
-    rclcpp::Time plan_start_time = original_tp.trajectory_points[0].target_time;
-    carma_planning_msgs::msg::TrajectoryPlan update_tpp_vector;
+      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack);
 
-    // GET EARLIEST COLLISION OBJECT TODO
-    std::optional<std::pair<carma_perception_msgs::msg::ExternalObject, double>> earliest_collision_obj_pair = get_earliest_collision_object_and_time(original_tp, external_objects);
+      double dist_to_object = object_downtrack - vehicle_downtrack;
+      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"dist_to_object: " << dist_to_object);
 
-    if (!earliest_collision_obj_pair.has_value())
-    {
-      RCLCPP_DEBUG(nh_->get_logger(),"No collision detection, so trajectory not modified.");
-      return original_tp;
-    }
+      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object speed: " << earliest_collision_obj.value().velocity.twist.linear.x);
 
-    carma_perception_msgs::msg::ExternalObject earliest_collision_obj = earliest_collision_obj_pair.value().first;
-    double earliest_collision_time = earliest_collision_obj_pair.value().second;
+      // Distance from the original trajectory point to the lead vehicle/object
+      double dist_x = earliest_collision_obj.value().pose.pose.position.x - original_tp.trajectory_points[0].x;
+      double dist_y = earliest_collision_obj.value().pose.pose.position.y - original_tp.trajectory_points[0].y;
+      double x_lead = sqrt(dist_x*dist_x + dist_y*dist_y);
 
-    // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route,
-    // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon.
+      // roadway object position
+      double gap_time = (x_lead - config_.x_gap)/initial_velocity;
 
-    lanelet::BasicPoint2d point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y);
-    double vehicle_downtrack = wm_->routeTrackPos(point).downtrack;
+      double goal_velocity = earliest_collision_obj.value().velocity.twist.linear.x;
+      // determine the safety inter-vehicle gap based on speed
+      double safety_gap = std::max(goal_velocity * gap_time, config_.x_gap);
+      if (config_.enable_adjustable_gap)
+      {
+        // check if a digital_gap is available
+        double digital_gap = check_traj_for_digital_min_gap(original_tp);
+        RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap);
+        // if a digital gap is available, it is replaced as safety gap
+        safety_gap = std::max(safety_gap, digital_gap);
+      }
+      // safety gap is implemented
+      double goal_pos = x_lead - safety_gap;
 
-    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack);
+      if (goal_velocity <= config_.min_obstacle_speed){
+        RCLCPP_WARN(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0");
+        goal_velocity = 0.0;
+      }
 
-    RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!");
+      double initial_time = 0;
+      double initial_pos = 0.0; //relative initial position (first trajectory point)
 
-    lanelet::BasicPoint2d point_o(earliest_collision_obj.pose.pose.position.x, earliest_collision_obj.pose.pose.position.y);
-    double object_downtrack = wm_->routeTrackPos(point_o).downtrack;
+      double initial_accel = 0;
+      double goal_accel = 0;
 
-    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack);
+      double delta_v_max = fabs(earliest_collision_obj.value().velocity.twist.linear.x - max_trajectory_speed(original_tp.trajectory_points));
+      // reference time, is the maximum time available to perform object avoidance (length of a trajectory)
+      double t_ref = (rclcpp::Time(original_tp.trajectory_points[original_tp.trajectory_points.size() - 1].target_time).seconds() - plan_start_time.seconds());
+      // time required for comfortable deceleration
+      double t_ph = config_.acceleration_adjustment_factor * delta_v_max / config_.yield_max_deceleration;
 
-    double x_lead = std::max(0.0, object_downtrack - vehicle_downtrack);
-    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"x_lead: " << x_lead);
+      // planning time for object avoidance
+      double tp = 0;
 
-    double goal_velocity = get_object_velocity_along_trajectory(earliest_collision_obj.velocity.twist, original_tp, earliest_collision_time);
-    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object's speed along trajectory at collision: " << goal_velocity);
+      if(t_ph > config_.tpmin && t_ref > t_ph)
+      {
+        tp = t_ph;
+      }
+      else if(t_ph < config_.tpmin)
+      {
+        tp = config_.tpmin;
+      }
+      else
+      {
+        tp = t_ref;
+      }
 
-    // roadway object position
-    double gap_time = std::max(0.0, x_lead - config_.x_gap)/initial_velocity;
+      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << tp);
 
-    if (goal_velocity <= config_.min_obstacle_speed){
-      RCLCPP_WARN_STREAM(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0 from: " << goal_velocity);
-      goal_velocity = 0.0;
-    }
+      update_tpp_vector = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, tp);
 
-    // determine the safety inter-vehicle gap based on speed
-    double safety_gap = std::max(goal_velocity * gap_time, config_.x_gap);
-    if (config_.enable_adjustable_gap)
-    {
-      // check if a digital_gap is available
-      double digital_gap = check_traj_for_digital_min_gap(original_tp);
-      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap);
-      // if a digital gap is available, it is replaced as safety gap
-      safety_gap = std::max(safety_gap, digital_gap);
-    }
-    // safety gap is implemented
-    double goal_pos = std::max(0.0, x_lead - safety_gap);
-    double initial_pos = 0.0; //relative initial position (first trajectory point)
-    double delta_v_max = fabs(goal_velocity - max_trajectory_speed(original_tp.trajectory_points));
-    // reference time, is the maximum time available to perform object avoidance (length of a trajectory)
-    double t_ref = (rclcpp::Time(original_tp.trajectory_points[original_tp.trajectory_points.size() - 1].target_time).seconds() - plan_start_time.seconds());
-    // time required for comfortable deceleration
-    double t_ph = config_.acceleration_adjustment_factor * delta_v_max / config_.yield_max_deceleration;
-
-    // planning time for object avoidance
-    double tp = 0;
-
-    if(t_ph > config_.tpmin && t_ref > t_ph)
-    {
-      tp = t_ph;
-    }
-    else if(t_ph < config_.tpmin)
-    {
-      tp = config_.tpmin;
+      return update_tpp_vector;
     }
-    else
-    {
-      tp = t_ref;
-    }
-
-    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << tp);
-
-    update_tpp_vector = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, tp);
 
-    return update_tpp_vector;
+    RCLCPP_DEBUG(nh_->get_logger(),"No collision detection, so trajectory not modified.");
+    return original_tp;
   }