Skip to content

Controller server refactor #131

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

Open
mini-1235 wants to merge 1 commit into
base: rolling
Choose a base branch
from
+23 −162
Open

Controller server refactor #131

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
21 changes: 5 additions & 16 deletions ..._pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,21 +42,13 @@ class PurePursuitController : public nav2_core::Controller
geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * goal_checker) override;
nav2_core::GoalChecker * goal_checker,
const nav_msgs::msg::Path & transformed_global_plan,
const geometry_msgs::msg::PoseStamped & global_goal) override;

void setPlan(const nav_msgs::msg::Path & path) override;
void newPathReceived(const nav_msgs::msg::Path & raw_global_path) override;

protected:
nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose);

bool transformPose(
const std::shared_ptr<tf2_ros::Buffer> tf,
const std::string frame,
const geometry_msgs::msg::PoseStamped & in_pose,
geometry_msgs::msg::PoseStamped & out_pose,
const rclcpp::Duration & transform_tolerance
) const;

nav2::LifecycleNode::WeakPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::string plugin_name_;
Expand All @@ -68,11 +60,8 @@ class PurePursuitController : public nav2_core::Controller
double lookahead_dist_;
double max_angular_vel_;
rclcpp::Duration transform_tolerance_ {0, 0};

nav_msgs::msg::Path global_plan_;
std::shared_ptr<nav2::Publisher<nav_msgs::msg::Path>> global_pub_;
};

} // namespace nav2_pure_pursuit_controller

#endif // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
#endif // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
164 changes: 18 additions & 146 deletions nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
#include "nav2_core/planner_exceptions.hpp"
#include "nav2_pure_pursuit_controller/pure_pursuit_controller.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/path_utils.hpp"

using std::hypot;
using std::min;
Expand Down Expand Up @@ -80,8 +81,6 @@ void PurePursuitController::configure(
double transform_tolerance;
node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance);
transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance);

global_pub_ = node->create_publisher<nav_msgs::msg::Path>("received_global_plan", 1);
}

void PurePursuitController::cleanup()
Expand All @@ -90,28 +89,25 @@ void PurePursuitController::cleanup()
logger_,
"Cleaning up controller: %s of type pure_pursuit_controller::PurePursuitController",
plugin_name_.c_str());
global_pub_.reset();
}

void PurePursuitController::activate()
{
RCLCPP_INFO(
logger_,
"Activating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s",
plugin_name_.c_str(),plugin_name_.c_str());
global_pub_->on_activate();
plugin_name_.c_str(), plugin_name_.c_str());
}

void PurePursuitController::deactivate()
{
RCLCPP_INFO(
logger_,
"Dectivating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s",
plugin_name_.c_str(),plugin_name_.c_str());
global_pub_->on_deactivate();
plugin_name_.c_str(), plugin_name_.c_str());
}

void PurePursuitController::setSpeedLimit(const double& speed_limit, const bool& percentage)
void PurePursuitController::setSpeedLimit(const double & speed_limit, const bool & percentage)
{
(void) speed_limit;
(void) percentage;
Expand All @@ -120,12 +116,22 @@ void PurePursuitController::setSpeedLimit(const double& speed_limit, const bool&
geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * goal_checker)
nav2_core::GoalChecker * goal_checker,
const nav_msgs::msg::Path & transformed_global_plan,
const geometry_msgs::msg::PoseStamped & /*global_goal*/)
{
(void)velocity;
(void)goal_checker;

auto transformed_plan = transformGlobalPlan(pose);
// Transform the plan from costmap's global frame to robot base frame
nav_msgs::msg::Path transformed_plan;
if (!nav2_util::transformPathInTargetFrame(
transformed_global_plan, transformed_plan, *tf_,
costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance()))
{
throw nav2_core::ControllerTFError(
"Unable to transform plan pose into local frame");
}

// Find the first pose which is at a distance greater than the specified lookahed distance
auto goal_pose_it = std::find_if(
Expand Down Expand Up @@ -167,145 +173,11 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands(
return cmd_vel;
}

void PurePursuitController::setPlan(const nav_msgs::msg::Path & path)
{
global_pub_->publish(path);
global_plan_ = path;
}

nav_msgs::msg::Path
PurePursuitController::transformGlobalPlan(
const geometry_msgs::msg::PoseStamped & pose)
{
// Original mplementation taken fron nav2_dwb_controller

if (global_plan_.poses.empty()) {
throw nav2_core::PlannerException("Received plan with zero length");
}

// let's get the pose of the robot in the frame of the plan
geometry_msgs::msg::PoseStamped robot_pose;
if (!transformPose(
tf_, global_plan_.header.frame_id, pose,
robot_pose, transform_tolerance_))
{
throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame");
}

// We'll discard points on the plan that are outside the local costmap
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) *
costmap->getResolution() / 2.0;

// First find the closest pose on the path to the robot
auto transformation_begin =
min_by(
global_plan_.poses.begin(), global_plan_.poses.end(),
[&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
return euclidean_distance(robot_pose, ps);
});

// From the closest point, look for the first point that's further then dist_threshold from the
// robot. These points are definitely outside of the costmap so we won't transform them.
auto transformation_end = std::find_if(
transformation_begin, end(global_plan_.poses),
[&](const auto & global_plan_pose) {
return euclidean_distance(robot_pose, global_plan_pose) > dist_threshold;
});

// Helper function for the transform below. Transforms a PoseStamped from global frame to local
auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {
// We took a copy of the pose, let's lookup the transform at the current time
geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose;
stamped_pose.header.frame_id = global_plan_.header.frame_id;
stamped_pose.header.stamp = pose.header.stamp;
stamped_pose.pose = global_plan_pose.pose;
transformPose(
tf_, costmap_ros_->getBaseFrameID(),
stamped_pose, transformed_pose, transform_tolerance_);
return transformed_pose;
};

// Transform the near part of the global plan into the robot's frame of reference.
nav_msgs::msg::Path transformed_plan;
std::transform(
transformation_begin, transformation_end,
std::back_inserter(transformed_plan.poses),
transformGlobalPoseToLocal);
transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID();
transformed_plan.header.stamp = pose.header.stamp;

// Remove the portion of the global plan that we've already passed so we don't
// process it on the next iteration (this is called path pruning)
global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin);
global_pub_->publish(transformed_plan);

if (transformed_plan.poses.empty()) {
throw nav2_core::PlannerException("Resulting plan has 0 poses in it.");
}

return transformed_plan;
}

bool PurePursuitController::transformPose(
const std::shared_ptr<tf2_ros::Buffer> tf,
const std::string frame,
const geometry_msgs::msg::PoseStamped & in_pose,
geometry_msgs::msg::PoseStamped & out_pose,
const rclcpp::Duration & transform_tolerance
) const
void PurePursuitController::newPathReceived(const nav_msgs::msg::Path & /*raw_global_path*/)
{
// Implementation taken as is fron nav_2d_utils in nav2_dwb_controller

if (in_pose.header.frame_id == frame) {
out_pose = in_pose;
return true;
}

try {
tf->transform(in_pose, out_pose, frame);
return true;
} catch (tf2::ExtrapolationException & ex) {
auto transform = tf->lookupTransform(
frame,
in_pose.header.frame_id,
tf2::TimePointZero
);
if (
(rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) >
transform_tolerance)
{
RCLCPP_ERROR(
rclcpp::get_logger("tf_help"),
"Transform data too old when converting from %s to %s",
in_pose.header.frame_id.c_str(),
frame.c_str()
);
RCLCPP_ERROR(
rclcpp::get_logger("tf_help"),
"Data time: %ds %uns, Transform time: %ds %uns",
in_pose.header.stamp.sec,
in_pose.header.stamp.nanosec,
transform.header.stamp.sec,
transform.header.stamp.nanosec
);
return false;
} else {
tf2::doTransform(in_pose, out_pose, transform);
return true;
}
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(
rclcpp::get_logger("tf_help"),
"Exception in transformPose: %s",
ex.what()
);
return false;
}
return false;
}

} // namespace nav2_pure_pursuit_controller

// Register this controller as a nav2_core plugin
PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller)
PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller)