diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index de5745a05c3..9b11cf905f5 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -209,7 +209,8 @@ NavigateThroughPosesNavigator::onLoop() res = blackboard->get( tracking_feedback_blackboard_id_, tracking_feedback); - feedback_msg->tracking_error = tracking_feedback.tracking_error; + feedback_msg->position_tracking_error = tracking_feedback.position_tracking_error; + feedback_msg->heading_tracking_error = tracking_feedback.heading_tracking_error; bt_action_server_->publishFeedback(feedback_msg); } @@ -290,10 +291,13 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr blackboard->set("number_recoveries", 0); // NOLINT previous_path_ = nav_msgs::msg::Path(); - // Update the goal pose on the blackboard + // Update the goal pose and path on the blackboard blackboard->set( goals_blackboard_id_, std::move(goals_array)); + blackboard->set( + path_blackboard_id_, + nav_msgs::msg::Path()); // Reset the waypoint states vector in the blackboard std::vector waypoint_statuses(goals_array.goals.size()); diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index fe385acf0ee..211e5b56793 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -189,7 +189,8 @@ NavigateToPoseNavigator::onLoop() res = blackboard->get( tracking_feedback_blackboard_id_, tracking_feedback); - feedback_msg->tracking_error = tracking_feedback.tracking_error; + feedback_msg->position_tracking_error = tracking_feedback.position_tracking_error; + feedback_msg->heading_tracking_error = tracking_feedback.heading_tracking_error; bt_action_server_->publishFeedback(feedback_msg); } @@ -266,8 +267,9 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) blackboard->set("number_recoveries", 0); // NOLINT previous_path_ = nav_msgs::msg::Path(); - // Update the goal pose on the blackboard + // Update the goal pose and path on the blackboard blackboard->set(goal_blackboard_id_, goal_pose); + blackboard->set(path_blackboard_id_, nav_msgs::msg::Path()); return true; } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2545457df2b..681627a73ef 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -731,15 +731,32 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); } - - // + // Calculate closest point and position error from path const auto path_search_result = nav2_util::distance_from_path( current_path_, robot_pose_in_path_frame.pose, start_index_, params_->search_window); + // Calculate heading error + double heading_tracking_error = 0.0; + if (path_search_result.closest_segment_index < + current_path_.poses.size() - 1) + { + const auto & path_segment_start = + current_path_.poses[path_search_result.closest_segment_index].pose; + const auto & path_segment_end = + current_path_.poses[path_search_result.closest_segment_index + 1].pose; + double path_yaw = std::atan2( + path_segment_end.position.y - path_segment_start.position.y, + path_segment_end.position.x - path_segment_start.position.x); + double robot_yaw = tf2::getYaw(robot_pose_in_path_frame.pose.orientation); + heading_tracking_error = angles::shortest_angular_distance( + robot_yaw, path_yaw); + } + // Create tracking error message auto tracking_feedback_msg = std::make_unique(); tracking_feedback_msg->header = pose.header; - tracking_feedback_msg->tracking_error = path_search_result.distance; + tracking_feedback_msg->position_tracking_error = path_search_result.distance; + tracking_feedback_msg->heading_tracking_error = heading_tracking_error; tracking_feedback_msg->current_path_index = path_search_result.closest_segment_index; tracking_feedback_msg->robot_pose = pose; tracking_feedback_msg->distance_to_goal = current_distance_to_goal; diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index ccf66dd7a5d..a2c86bebe91 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -25,5 +25,6 @@ string error_msg --- # feedback definition # Real-time tracking error indicating which side of the path the robot is on -# If the tracking_error is positive, the robot is to the left of the path +# If the position_tracking_error is positive, the robot is to the left of the path +# If the heading_tracking_error is positive, the robot heading is rotated to the right of the path direction nav2_msgs/TrackingFeedback tracking_feedback diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 97f63a2cdf7..f5ccaed4e87 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -23,6 +23,7 @@ builtin_interfaces/Duration navigation_time builtin_interfaces/Duration estimated_time_remaining int16 number_of_recoveries float32 distance_remaining -float32 tracking_error +float32 position_tracking_error +float32 heading_tracking_error int16 number_of_poses_remaining WaypointStatus[] waypoint_statuses diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index ee886672115..7673ee3c191 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -21,4 +21,5 @@ builtin_interfaces/Duration navigation_time builtin_interfaces/Duration estimated_time_remaining int16 number_of_recoveries float32 distance_remaining -float32 tracking_error +float32 position_tracking_error +float32 heading_tracking_error diff --git a/nav2_msgs/msg/TrackingFeedback.msg b/nav2_msgs/msg/TrackingFeedback.msg index a4b6a554d33..b5a762563d1 100644 --- a/nav2_msgs/msg/TrackingFeedback.msg +++ b/nav2_msgs/msg/TrackingFeedback.msg @@ -1,9 +1,14 @@ # Real-time tracking error for Nav2 controller std_msgs/Header header -# The sign of the tracking error indicates which side of the path the robot is on +# The sign of the position tracking error indicates which side of the path the robot is on # Positive sign indicates the robot is to the left of the path, negative to the right -float32 tracking_error +float32 position_tracking_error +# The sign of the heading tracking error indicates the angular difference between +# the robot's heading and the path. +# Positive sign means the robot heading is rotated to the right of the path direction, +# negative sign means it is rotated to the left. +float32 heading_tracking_error uint32 current_path_index geometry_msgs/PoseStamped robot_pose float32 distance_to_goal diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index b4e800555df..a0f18df1665 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -1083,9 +1083,6 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) syncTabsWithAccumulatedPoses(); // Sync tabs with new pose } else { acummulated_poses_ = nav_msgs::msg::Goals(); - // Reset goal index for single goal navigation - goal_index_ = 0; - store_poses_.goals.push_back(pose); updateWpNavigationMarkers(); std::cout << "Start navigation" << std::endl; startNavigation(pose); @@ -1629,8 +1626,10 @@ inline std::string Nav2Panel::toLabel(T & msg) toString(rclcpp::Duration(msg.estimated_time_remaining).seconds(), 0) + " s" "Distance remaining:" + toString(msg.distance_remaining, 2) + " m" - "Tracking error:" + - toString(msg.tracking_error, 2) + " m" + "Position error:" + + toString(msg.position_tracking_error, 2) + " m" + "Heading error:" + + toString(msg.heading_tracking_error, 2) + " rad" "Time taken:" + toString(rclcpp::Duration(msg.navigation_time).seconds(), 0) + " s" "Recoveries:" + diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py index ca165e392a7..7886039ef64 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -116,11 +116,17 @@ def main() -> None: ) print( - 'Tracking error: ' - + '{:.2f}'.format(feedback.tracking_error) + 'Position error: ' + + '{:.2f}'.format(feedback.position_tracking_error) + ' meters.' ) + print( + 'Heading error: ' + + '{:.2f}'.format(feedback.heading_tracking_error) + + ' radians.' + ) + # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): navigator.cancelTask() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py index 8e86a9c41c8..7eb2ee7d848 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -96,11 +96,17 @@ def main() -> None: ) print( - 'Tracking error: ' - + '{:.2f}'.format(feedback.tracking_error) + 'Position error: ' + + '{:.2f}'.format(feedback.position_tracking_error) + ' meters.' ) + print( + 'Heading error: ' + + '{:.2f}'.format(feedback.heading_tracking_error) + + ' radians.' + ) + # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): navigator.cancelTask() diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index 5afa1599b1d..1e7ed1cd92f 100644 --- a/nav2_util/src/path_utils.cpp +++ b/nav2_util/src/path_utils.cpp @@ -45,9 +45,9 @@ PathSearchResult distance_from_path( if (start_index >= path.poses.size()) { throw std::runtime_error( - "Invalid operation: requested start index (" + std::to_string(start_index) + + "Requested start index (" + std::to_string(start_index) + ") is greater than or equal to path size (" + std::to_string(path.poses.size()) + - "). Application is not properly managing state."); + ")."); } double distance_traversed = 0.0;