Skip to content

adding the fixes for navigate through poses terminating pre-maturely #5203

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
wants to merge 1 commit into from
Closed

adding the fixes for navigate through poses terminating pre-maturely #5203

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<nav2_util::OdomSmoother> odom_smoother_;
Expand Down
1 change: 1 addition & 0 deletions nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<nav2_util::OdomSmoother> odom_smoother_;
Expand Down
27 changes: 24 additions & 3 deletions nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
}

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"));
Expand All @@ -52,6 +53,12 @@
// 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));

Check warning on line 57 in nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp#L57 

Added line #L57 was not covered by tests
}

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);
}
Expand Down Expand Up @@ -137,6 +144,15 @@
result->waypoint_statuses = std::move(waypoint_statuses);
}

inline double createYawFromQuat(const geometry_msgs::msg::Quaternion & orientation)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

tf2 Utils has a function that does this getYaw

{
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()
{
Expand Down Expand Up @@ -169,19 +185,24 @@
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 =
[&current_pose, &current_path]() {
[&current_pose, &current_path, &orient_angle_rad]() {
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::max();
for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Instead we could check for the distance not just on XY but XY,Yaw so it would be 3D.

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)
{
Comment on lines +203 to +205
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
if (curr_dist < curr_min_dist &&
abs(orient_diff) < orient_angle_rad)
{
if (curr_dist < curr_min_dist && abs(orient_diff) < orient_angle_rad) {

curr_min_dist = curr_dist;
closest_pose_idx = curr_idx;
}
Expand Down
26 changes: 23 additions & 3 deletions nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -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()
{
Expand All @@ -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 =
[&current_pose, &current_path]() {
[&current_pose, &current_path, &orient_angle_rad]() {
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::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;
}
Expand Down
1 change: 1 addition & 0 deletions nav2_controller/include/nav2_controller/controller_server.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
20 changes: 17 additions & 3 deletions nav2_controller/src/controller_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<nav2_costmap_2d::Costmap2DROS>(
"local_costmap", std::string{get_namespace()},
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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*/)
{
Expand Down Expand Up @@ -679,15 +689,19 @@ 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, &current_path]()
double orient_angle_rad = orient_angle_rad_;
auto find_closest_pose_idx = [&robot_pose_in_path_frame, &current_path,
&orient_angle_rad]()
{
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::max();
for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
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;
}
Expand Down
Loading