diff --git a/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp b/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp index 67fae2e..ac3fd43 100644 --- a/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp +++ b/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp @@ -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 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 tf_; std::string plugin_name_; @@ -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> global_pub_; }; } // namespace nav2_pure_pursuit_controller -#endif // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ \ No newline at end of file +#endif // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 4c2191a..ce0405c 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -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; @@ -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("received_global_plan", 1); } void PurePursuitController::cleanup() @@ -90,7 +89,6 @@ void PurePursuitController::cleanup() logger_, "Cleaning up controller: %s of type pure_pursuit_controller::PurePursuitController", plugin_name_.c_str()); - global_pub_.reset(); } void PurePursuitController::activate() @@ -98,8 +96,7 @@ 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() @@ -107,11 +104,10 @@ 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; @@ -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( @@ -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 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) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller)