@@ -609,18 +609,17 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
609
609
throw nav2_core::InvalidPath (" Path is empty." );
610
610
}
611
611
controllers_[current_controller_]->setPlan (path);
612
-
612
+
613
613
end_pose_ = path.poses .back ();
614
614
end_pose_.header .frame_id = path.header .frame_id ;
615
615
goal_checkers_[current_goal_checker_]->reset ();
616
-
616
+
617
617
RCLCPP_DEBUG (
618
618
get_logger (), " Path end point is (%.2f, %.2f)" ,
619
619
end_pose_.pose .position .x , end_pose_.pose .position .y );
620
-
620
+
621
621
start_index_ = 0 ;
622
622
current_path_ = path;
623
-
624
623
}
625
624
626
625
void ControllerServer::computeAndPublishVelocity ()
@@ -770,16 +769,18 @@ void ControllerServer::publishTrackingState()
770
769
return ;
771
770
}
772
771
773
- const double distance_to_goal = nav2_util::geometry_utils::euclidean_distance (robot_pose, end_pose_);
774
- const auto path_search_result = nav2_util::distance_from_path (current_path_, robot_pose.pose , start_index_, search_window_);
772
+ const double distance_to_goal = nav2_util::geometry_utils::euclidean_distance (robot_pose,
773
+ end_pose_);
774
+ const auto path_search_result = nav2_util::distance_from_path (current_path_, robot_pose.pose ,
775
+ start_index_, search_window_);
775
776
const size_t closest_idx = path_search_result.closest_segment_index ;
776
777
start_index_ = closest_idx;
777
778
778
- const auto & segment_start = current_path_.poses [closest_idx];
779
- const auto & segment_end = current_path_.poses [closest_idx + 1 ];
780
-
779
+ const auto & segment_start = current_path_.poses [closest_idx];
780
+ const auto & segment_end = current_path_.poses [closest_idx + 1 ];
781
+
781
782
double cross_product = nav2_util::geometry_utils::cross_product_2d (
782
- robot_pose.pose .position , segment_start.pose ,segment_end.pose );
783
+ robot_pose.pose .position , segment_start.pose , segment_end.pose );
783
784
784
785
nav2_msgs::msg::TrackingError tracking_error_msg;
785
786
tracking_error_msg.header .stamp = now ();
0 commit comments