Skip to content

Commit

Permalink
- Modified findVelocitySignChange method to transform cusp into robot… (
Browse files Browse the repository at this point in the history
ros-navigation#3036)

* - Modified findVelocitySignChange method to transform cusp into robot frame before returning distance_to_cusp

* - Previous commit had incorrect usage of method transformPose based on different nav2 branch.
- Now a placeholder pose, input pose and desired frame id is passed.

* - Previous commit had incorrect usage of method transformPose based on different nav2 branch.
- Now a placeholder pose, input pose and desired frame id is passed.

Signed-off-by: Steven Brills <[email protected]>

* Failed lint check due to stray blank line.  Removed the blank line

* - Modified findVelocitySignChange function to take the transformed plan in robot's frame
- Removed need to pass pose to the findVelocitySignChange function

* - Modified the test file to reflect change in new parameters that findVelocitySignChange expects.

Signed-off-by: Steven Brills <[email protected]>

* - Corrected call to transformGlobalPoseWrapper method of BasicAPIRPP object.

Signed-off-by: Steven Brills <[email protected]>

* - transformGlobalPlanWrapper call bug fix

Signed-off-by: Steven Brills <[email protected]>

* - Updated tests now require frame_id and time_stamp for conversion since transformed plan is to be used.

Signed-off-by: Steven Brills <[email protected]>

* - Missing ; in test method

Signed-off-by: Steven Brills <[email protected]>

* -Modified findVelocitySignChange tests in test_regulated_pp to use transformed_plan instead

* -Modified findVelocitySignChange tests in test_regulated_pp to use transformed_plan instead

Signed-off-by: Steven Brills <[email protected]>

Co-authored-by: Steven Brills <[email protected]>
  • Loading branch information
stevenbrills and Steven Brills authored Jun 23, 2022
1 parent bccf6d6 commit 99bec08
Show file tree
Hide file tree
Showing 3 changed files with 29 additions and 21 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -249,7 +249,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
* @param pose Pose input to determine the cusp position
* @return robot distance from the cusp
*/
double findVelocitySignChange(const geometry_msgs::msg::PoseStamped & pose);
double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan);

/**
* Get the greatest extent of the costmap in meters from the center.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -283,7 +283,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
// Check for reverse driving
if (allow_reversing_) {
// Cusp check
double dist_to_cusp = findVelocitySignChange(pose);
double dist_to_cusp = findVelocitySignChange(transformed_plan);

// if the lookahead distance is further than the cusp, use the cusp distance instead
if (dist_to_cusp < lookahead_dist) {
Expand Down Expand Up @@ -720,27 +720,29 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
}

double RegulatedPurePursuitController::findVelocitySignChange(
const geometry_msgs::msg::PoseStamped & pose)
const nav_msgs::msg::Path & transformed_plan)
{
// Iterating through the global path to determine the position of the cusp
for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size() - 1; ++pose_id) {
// Iterating through the transformed global path to determine the position of the cusp
for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) {
// We have two vectors for the dot product OA and AB. Determining the vectors.
double oa_x = global_plan_.poses[pose_id].pose.position.x -
global_plan_.poses[pose_id - 1].pose.position.x;
double oa_y = global_plan_.poses[pose_id].pose.position.y -
global_plan_.poses[pose_id - 1].pose.position.y;
double ab_x = global_plan_.poses[pose_id + 1].pose.position.x -
global_plan_.poses[pose_id].pose.position.x;
double ab_y = global_plan_.poses[pose_id + 1].pose.position.y -
global_plan_.poses[pose_id].pose.position.y;
double oa_x = transformed_plan.poses[pose_id].pose.position.x -
transformed_plan.poses[pose_id - 1].pose.position.x;
double oa_y = transformed_plan.poses[pose_id].pose.position.y -
transformed_plan.poses[pose_id - 1].pose.position.y;
double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x -
transformed_plan.poses[pose_id].pose.position.x;
double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y -
transformed_plan.poses[pose_id].pose.position.y;

/* Checking for the existance of cusp, in the path, using the dot product
and determine it's distance from the robot. If there is no cusp in the path,
then just determine the distance to the goal location. */
if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) {
auto x = global_plan_.poses[pose_id].pose.position.x - pose.pose.position.x;
auto y = global_plan_.poses[pose_id].pose.position.y - pose.pose.position.y;
return hypot(x, y); // returning the distance if there is a cusp
// returning the distance if there is a cusp
// The transformed path is in the robots frame, so robot is at the origin
return hypot(
transformed_plan.poses[pose_id].pose.position.x,
transformed_plan.poses[pose_id].pose.position.y);
}
}

Expand Down
16 changes: 11 additions & 5 deletions nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,9 +103,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
}

double findVelocitySignChangeWrapper(
const geometry_msgs::msg::PoseStamped & pose)
const nav_msgs::msg::Path & transformed_plan)
{
return findVelocitySignChange(pose);
return findVelocitySignChange(transformed_plan);
}

nav_msgs::msg::Path transformGlobalPlanWrapper(
Expand Down Expand Up @@ -170,26 +170,32 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg)
TEST(RegulatedPurePursuitTest, findVelocitySignChange)
{
auto ctrl = std::make_shared<BasicAPIRPP>();
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPPfindVelocitySignChange");
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "smb";
auto time = node->get_clock()->now();
pose.header.stamp = time;
pose.pose.position.x = 1.0;
pose.pose.position.y = 0.0;

nav_msgs::msg::Path path;
path.poses.resize(3);
path.header.frame_id = "smb";
path.header.stamp = pose.header.stamp;
path.poses[0].pose.position.x = 1.0;
path.poses[0].pose.position.y = 1.0;
path.poses[1].pose.position.x = 2.0;
path.poses[1].pose.position.y = 2.0;
path.poses[2].pose.position.x = -1.0;
path.poses[2].pose.position.y = -1.0;
ctrl->setPlan(path);
auto rtn = ctrl->findVelocitySignChangeWrapper(pose);
EXPECT_EQ(rtn, sqrt(5.0));
auto rtn = ctrl->findVelocitySignChangeWrapper(path);
EXPECT_EQ(rtn, sqrt(8.0));

path.poses[2].pose.position.x = 3.0;
path.poses[2].pose.position.y = 3.0;
ctrl->setPlan(path);
rtn = ctrl->findVelocitySignChangeWrapper(pose);
rtn = ctrl->findVelocitySignChangeWrapper(path);
EXPECT_EQ(rtn, std::numeric_limits<double>::max());
}

Expand Down

0 comments on commit 99bec08

Please sign in to comment.