Skip to content
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

Jump to bottom

Add current path to GoalChecker interface #4593

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Add current path to GoalChecker interface #4593

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
2 changes: 1 addition & 1 deletion nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
void reset() override;
bool isGoalReached(
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
const geometry_msgs::msg::Twist & velocity) override;
const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override;
bool getTolerances(
geometry_msgs::msg::Pose & pose_tolerance,
geometry_msgs::msg::Twist & vel_tolerance) override;
Expand Down
2 changes: 1 addition & 1 deletion nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ class StoppedGoalChecker : public SimpleGoalChecker
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
bool isGoalReached(
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
const geometry_msgs::msg::Twist & velocity) override;
const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override;
bool getTolerances(
geometry_msgs::msg::Pose & pose_tolerance,
geometry_msgs::msg::Twist & vel_tolerance) override;
Expand Down
2 changes: 1 addition & 1 deletion nav2_controller/plugins/simple_goal_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ void SimpleGoalChecker::reset()

bool SimpleGoalChecker::isGoalReached(
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
const geometry_msgs::msg::Twist &)
const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &)
{
if (check_xy_) {
double dx = query_pose.position.x - goal_pose.position.x,
Expand Down
4 changes: 2 additions & 2 deletions nav2_controller/plugins/stopped_goal_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,9 +82,9 @@ void StoppedGoalChecker::initialize(

bool StoppedGoalChecker::isGoalReached(
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
const geometry_msgs::msg::Twist & velocity)
const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path)
{
bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity);
bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, current_path);
if (!ret) {
return ret;
}
Expand Down
37 changes: 21 additions & 16 deletions nav2_controller/plugins/test/goal_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include "nav2_controller/plugins/simple_goal_checker.hpp"
#include "nav2_controller/plugins/stopped_goal_checker.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/lifecycle_node.hpp"

using nav2_controller::SimpleGoalChecker;
Expand All @@ -49,6 +50,7 @@ void checkMacro(
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav,
const nav_msgs::msg::Path & path,
bool expected_result)
{
gc.reset();
Expand All @@ -67,12 +69,12 @@ void checkMacro(
EXPECT_TRUE(
gc.isGoalReached(
nav_2d_utils::pose2DToPose(pose0),
nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v), path));
} else {
EXPECT_FALSE(
gc.isGoalReached(
nav_2d_utils::pose2DToPose(pose0),
nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v), path));
}
}

Expand All @@ -81,20 +83,22 @@ void sameResult(
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav,
const nav_msgs::msg::Path & path,
bool expected_result)
{
checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, expected_result);
checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, expected_result);
}

void trueFalse(
nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav)
double xv, double yv, double thetav,
const nav_msgs::msg::Path & path)
{
checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true);
checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false);
checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, true);
checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, false);
}
class TestLifecycleNode : public nav2_util::LifecycleNode
{
Expand Down Expand Up @@ -162,18 +166,19 @@ TEST(VelocityIterator, two_checks)
SimpleGoalChecker gc;
StoppedGoalChecker sgc;
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
nav_msgs::msg::Path path;

gc.initialize(x, "nav2_controller", costmap);
sgc.initialize(x, "nav2_controller", costmap);
sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true);
sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false);
sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false);
sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false);
sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true);
trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1);
sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true);
sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false);
sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false);
sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false);
sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true);
trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path);
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path);
}

TEST(StoppedGoalChecker, get_tol_and_dynamic_params)
Expand Down
2 changes: 1 addition & 1 deletion nav2_controller/src/controller_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -756,7 +756,7 @@ bool ControllerServer::isGoalReached()

return goal_checkers_[current_goal_checker_]->isGoalReached(
pose.pose, transformed_end_pose.pose,
velocity);
velocity, current_path_);
}

bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)
Expand Down
7 changes: 5 additions & 2 deletions nav2_core/include/nav2_core/goal_checker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/path.hpp"

#include "nav2_costmap_2d/costmap_2d_ros.hpp"

Expand Down Expand Up @@ -84,8 +85,10 @@ class GoalChecker
* @return True if goal is reached
*/
virtual bool isGoalReached(
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
const geometry_msgs::msg::Twist & velocity) = 0;
const geometry_msgs::msg::Pose & query_pose,
const geometry_msgs::msg::Pose & goal_pose,
const geometry_msgs::msg::Twist & velocity,
const nav_msgs::msg::Path & current_path) = 0;

/**
* @brief Get the maximum possible tolerances used for goal checking in the major types.
Expand Down
3 changes: 2 additions & 1 deletion nav2_mppi_controller/test/utils_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,8 @@ class TestGoalChecker : public nav2_core::GoalChecker
virtual bool isGoalReached(
const geometry_msgs::msg::Pose & /*query_pose*/,
const geometry_msgs::msg::Pose & /*goal_pose*/,
const geometry_msgs::msg::Twist & /*velocity*/) {return false;}
const geometry_msgs::msg::Twist & /*velocity*/,
const nav_msgs::msg::Path & /*current_path*/) {return false;}

virtual bool getTolerances(
geometry_msgs::msg::Pose & pose_tolerance,
Expand Down
Loading