diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 2226c5fc3d7..3d59fbf6f91 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -111,6 +111,7 @@ class NavigateThroughPosesNavigator std::string goals_blackboard_id_; std::string path_blackboard_id_; std::string waypoint_statuses_blackboard_id_; + double orient_angle_rad_; // Odometry smoother object std::shared_ptr odom_smoother_; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 447d02cc449..c147a2ab8ba 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -127,6 +127,7 @@ class NavigateToPoseNavigator std::string goal_blackboard_id_; std::string path_blackboard_id_; + double orient_angle_rad_; // Odometry smoother object std::shared_ptr odom_smoother_; diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index a28e1c59586..1450bb1e180 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -35,6 +35,7 @@ NavigateThroughPosesNavigator::configure( } goals_blackboard_id_ = node->get_parameter("goals_blackboard_id").as_string(); + orient_angle_rad_ = node->get_parameter("orient_angle_rad", orient_angle_rad_); if (!node->has_parameter("path_blackboard_id")) { node->declare_parameter("path_blackboard_id", std::string("path")); @@ -52,6 +53,12 @@ NavigateThroughPosesNavigator::configure( // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; + if (!node->has_parameter("orient_angle_rad")) { + node->declare_parameter("orient_angle_rad", rclcpp::ParameterValue(1.57)); + } + + node->get_parameter("orient_angle_rad", orient_angle_rad_); + if (!node->has_parameter(getName() + ".enable_groot_monitoring")) { node->declare_parameter(getName() + ".enable_groot_monitoring", false); } @@ -137,6 +144,15 @@ NavigateThroughPosesNavigator::goalCompleted( result->waypoint_statuses = std::move(waypoint_statuses); } +inline double createYawFromQuat(const geometry_msgs::msg::Quaternion & orientation) +{ + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + return yaw; +} + void NavigateThroughPosesNavigator::onLoop() { @@ -169,19 +185,24 @@ NavigateThroughPosesNavigator::onLoop() return; } - // Get current path points + // Get current path points and orientation angle nav_msgs::msg::Path current_path; + double orient_angle_rad = orient_angle_rad_; res = blackboard->get(path_blackboard_id_, current_path); if (res && current_path.poses.size() > 0u) { // Find the closest pose to current pose on global path auto find_closest_pose_idx = - [¤t_pose, ¤t_path]() { + [¤t_pose, ¤t_path, &orient_angle_rad]() { size_t closest_pose_idx = 0; double curr_min_dist = std::numeric_limits::max(); for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { double curr_dist = nav2_util::geometry_utils::euclidean_distance( current_pose, current_path.poses[curr_idx]); - if (curr_dist < curr_min_dist) { + double orient_diff = createYawFromQuat(current_pose.pose.orientation) - + createYawFromQuat(current_path.poses[curr_idx].pose.orientation); + if (curr_dist < curr_min_dist && + abs(orient_diff) < orient_angle_rad) + { curr_min_dist = curr_dist; closest_pose_idx = curr_idx; } diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index a421db4c3f6..c3e4b9d5c5b 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -51,6 +51,12 @@ NavigateToPoseNavigator::configure( rclcpp::SystemDefaultsQoS(), std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1)); + if (!node->has_parameter("orient_angle_rad")) { + node->declare_parameter("orient_angle_rad", rclcpp::ParameterValue(1.57)); + } + + node->get_parameter("orient_angle_rad", orient_angle_rad_); + if (!node->has_parameter(getName() + ".enable_groot_monitoring")) { node->declare_parameter(getName() + ".enable_groot_monitoring", false); } @@ -128,6 +134,15 @@ NavigateToPoseNavigator::goalCompleted( } } +inline double createYawFromQuat(const geometry_msgs::msg::Quaternion & orientation) +{ + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + return yaw; +} + void NavigateToPoseNavigator::onLoop() { @@ -147,19 +162,24 @@ NavigateToPoseNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); - // Get current path points + // Get current path points and orientation angle nav_msgs::msg::Path current_path; + double orient_angle_rad = orient_angle_rad_; auto res = blackboard->get(path_blackboard_id_, current_path); if (res && current_path.poses.size() > 0u) { // Find the closest pose to current pose on global path auto find_closest_pose_idx = - [¤t_pose, ¤t_path]() { + [¤t_pose, ¤t_path, &orient_angle_rad]() { size_t closest_pose_idx = 0; double curr_min_dist = std::numeric_limits::max(); for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { double curr_dist = nav2_util::geometry_utils::euclidean_distance( current_pose, current_path.poses[curr_idx]); - if (curr_dist < curr_min_dist) { + double orient_diff = createYawFromQuat(current_pose.pose.orientation) - + createYawFromQuat(current_path.poses[curr_idx].pose.orientation); + if (curr_dist < curr_min_dist && + abs(orient_diff) < orient_angle_rad) + { curr_min_dist = curr_dist; closest_pose_idx = curr_idx; } diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index ba55850848e..58d0ae0a4db 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -268,6 +268,7 @@ class ControllerServer : public nav2_util::LifecycleNode double min_x_velocity_threshold_; double min_y_velocity_threshold_; double min_theta_velocity_threshold_; + double orient_angle_rad_; double failure_tolerance_; bool use_realtime_priority_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2597c277996..8c3a5d78e6d 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -64,7 +64,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true)); declare_parameter("costmap_update_timeout", 0.30); // 300ms - + declare_parameter("orient_angle_rad", rclcpp::ParameterValue(1.57)); // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( "local_costmap", std::string{get_namespace()}, @@ -130,6 +130,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("failure_tolerance", failure_tolerance_); get_parameter("use_realtime_priority", use_realtime_priority_); get_parameter("publish_zero_velocity", publish_zero_velocity_); + get_parameter("orient_angle_rad", orient_angle_rad_); costmap_ros_->configure(); // Launch a thread to run the costmap node @@ -251,6 +252,15 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) return nav2_util::CallbackReturn::SUCCESS; } +inline double createYawFromQuat(const geometry_msgs::msg::Quaternion & orientation) +{ + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + return yaw; +} + nav2_util::CallbackReturn ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { @@ -679,7 +689,9 @@ void ControllerServer::computeAndPublishVelocity() feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y); nav_msgs::msg::Path & current_path = current_path_; - auto find_closest_pose_idx = [&robot_pose_in_path_frame, ¤t_path]() + double orient_angle_rad = orient_angle_rad_; + auto find_closest_pose_idx = [&robot_pose_in_path_frame, ¤t_path, + &orient_angle_rad]() { size_t closest_pose_idx = 0; double curr_min_dist = std::numeric_limits::max(); @@ -687,7 +699,9 @@ void ControllerServer::computeAndPublishVelocity() double curr_dist = nav2_util::geometry_utils::euclidean_distance(robot_pose_in_path_frame, current_path.poses[curr_idx]); - if (curr_dist < curr_min_dist) { + double orient_diff = createYawFromQuat(robot_pose_in_path_frame.pose.orientation) - + createYawFromQuat(current_path.poses[curr_idx].pose.orientation); + if (curr_dist < curr_min_dist && abs(orient_diff) < orient_angle_rad) { curr_min_dist = curr_dist; closest_pose_idx = curr_idx; }