Skip to content

[RPP] Remove optimization check on carrot, incorrect optimization #2209

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

Merged
SteveMacenski merged 1 commit into from
Feb 26, 2021
Merged

[RPP] Remove optimization check on carrot, incorrect optimization #2209

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
16 changes: 3 additions & 13 deletions nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -338,20 +338,10 @@ bool RegulatedPurePursuitController::isCollisionImminent(
const double & angular_vel)
{
// Note(stevemacenski): This may be a bit unusual, but the robot_pose is in
// odom frame and the carrot_pose is in robot base frame. We need to collision
// check in odom frame, so all values will be relative to robot base pose.
// But we can still use the carrot pose in odom to find various quantities.
// odom frame and the carrot_pose is in robot base frame.

geometry_msgs::msg::PoseStamped carrot_in_odom;
if (!transformPose(costmap_ros_->getGlobalFrameID(), carrot_pose, carrot_in_odom))
{
RCLCPP_ERROR(logger_, "Unable to get carrot pose in odom frame, failing collision check!");
return true;
}

// check current point and carrot point are OK, most likely ones to be in collision
if (inCollision(carrot_in_odom.pose.position.x, carrot_in_odom.pose.position.y) ||
inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y))
// check current point is OK
if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y))
{
return true;
}
Expand Down