Make Navigation Precise

robosoft-ai · Oct 15, 2024 · 4d2dae6 · 4d2dae6
1 parent a2da057
commit 4d2dae6
Show file tree

Hide file tree

Showing 17 changed files with 4,358 additions and 8 deletions.
diff --git a/...nners/backward_global_planner/include/backward_global_planner/backward_global_planner.hpp b/...nners/backward_global_planner/include/backward_global_planner/backward_global_planner.hpp
@@ -74,6 +74,7 @@ class BackwardGlobalPlanner : public nav2_core::GlobalPlanner
     const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal);
 
 private:
+  void updateParameters();
   // rclcpp::Node::SharedPtr nh_;
   rclcpp_lifecycle::LifecycleNode::SharedPtr nh_;
 

diff --git a/..._planners/backward_global_planner/src/backward_global_planner/backward_global_planner.cpp b/..._planners/backward_global_planner/src/backward_global_planner/backward_global_planner.cpp
@@ -48,7 +48,7 @@ namespace backward_global_planner
 */
 BackwardGlobalPlanner::BackwardGlobalPlanner()
 {
-  skip_straight_motion_distance_ = 0.2;
+  skip_straight_motion_distance_ = 0.014;
   puresSpinningRadStep_ = 1000;  // rads
 }
 
@@ -67,6 +67,8 @@ void BackwardGlobalPlanner::configure(
   name_ = name;
   tf_ = tf;
   transform_tolerance_ = 0.1;
+  skip_straight_motion_distance_ = 0.014;
+  puresSpinningRadStep_ = 1000;  // rads
 
   // RCLCPP_INFO_NAMED(nh_->get_logger(), "Backwards", "BackwardGlobalPlanner initialize");
   costmap_ros_ = costmap_ros;
@@ -78,8 +80,23 @@ void BackwardGlobalPlanner::configure(
     nh_->create_publisher<visualization_msgs::msg::MarkerArray>("backward_planner/markers", 1);
 
   declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
+  declareOrSet(nh_, name_ + ".pure_spinning_rad_step", puresSpinningRadStep_);
+  declareOrSet(nh_, name_ + ".skip_straight_motion_distance", skip_straight_motion_distance_);
+
+}
+
+void BackwardGlobalPlanner::updateParameters()
+{ 
+  nh_->get_parameter(name_ + ".pure_spinning_rad_step", puresSpinningRadStep_);
+  nh_->get_parameter(name_ + ".skip_straight_motion_distance", skip_straight_motion_distance_);
+  nh_->get_parameter(name_ + ".transform_tolerance", transform_tolerance_);
+
+  RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner.pure_spinning_rad_step: " << puresSpinningRadStep_);
+  RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner.skip_straight_motion_distance: " << skip_straight_motion_distance_);
+  RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner.transform_tolerance: " << transform_tolerance_);
 }
 
+
 /**
 ******************************************************************************************************************
 * cleanup()
@@ -95,6 +112,7 @@ void BackwardGlobalPlanner::cleanup() { this->cleanMarkers(); }
 void BackwardGlobalPlanner::activate()
 {
   RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner] activating planner");
+  this->updateParameters();
   planPub_->on_activate();
   markersPub_->on_activate();
 }
@@ -228,6 +246,7 @@ void BackwardGlobalPlanner::createDefaultBackwardPath(
 nav_msgs::msg::Path BackwardGlobalPlanner::createPlan(
   const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal)
 {
+  this->updateParameters();
   RCLCPP_INFO_STREAM(
     nh_->get_logger(), "[BackwardGlobalPlanner] goal frame id: "
                          << goal.header.frame_id << " pose: " << goal.pose.position);

diff --git a/...planners/forward_global_planner/include/forward_global_planner/forward_global_planner.hpp b/...planners/forward_global_planner/include/forward_global_planner/forward_global_planner.hpp
@@ -78,6 +78,7 @@ class ForwardGlobalPlanner : public nav2_core::GlobalPlanner
     const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal);
 
 private:
+  void updateParameters();
   // rclcpp::Node::SharedPtr nh_;
   rclcpp_lifecycle::LifecycleNode::SharedPtr nh_;
 

diff --git a/...tom_planners/forward_global_planner/src/forward_global_planner/forward_global_planner.cpp b/...tom_planners/forward_global_planner/src/forward_global_planner/forward_global_planner.cpp
@@ -42,7 +42,7 @@ namespace forward_global_planner
 ForwardGlobalPlanner::ForwardGlobalPlanner()
 //   : nh_("~/ForwardGlobalPlanner")
 {
-  skip_straight_motion_distance_ = 0.2;  // meters
+  skip_straight_motion_distance_ = 0.01; // 0.2;  // meters
   puresSpinningRadStep_ = 1000;          // rads
 }
 
@@ -60,14 +60,33 @@ void ForwardGlobalPlanner::configure(
 
   RCLCPP_INFO(nh_->get_logger(), "[Forward Global Planner] initializing");
   planPub_ = nh_->create_publisher<nav_msgs::msg::Path>("global_plan", rclcpp::QoS(1));
-  skip_straight_motion_distance_ = 0.2;  // meters
+  skip_straight_motion_distance_ = 0.2; //0.2  // meters
   puresSpinningRadStep_ = 1000;          // rads
   transform_tolerance_ = 0.1;
+
+  declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
+  declareOrSet(nh_, name_ + ".pure_spinning_rad_step", puresSpinningRadStep_);
+  declareOrSet(nh_, name_ + ".skip_straight_motion_distance", skip_straight_motion_distance_);
+}
+void ForwardGlobalPlanner::updateParameters()
+{ 
+  nh_->get_parameter(name_ + ".pure_spinning_rad_step", puresSpinningRadStep_);
+  nh_->get_parameter(name_ + ".skip_straight_motion_distance", skip_straight_motion_distance_);
+  nh_->get_parameter(name_ + ".transform_tolerance", transform_tolerance_);
+
+  RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardGlobalPlanner.pure_spinning_rad_step: " << puresSpinningRadStep_);
+  RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardGlobalPlanner.skip_straight_motion_distance: " << skip_straight_motion_distance_);
+  RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardGlobalPlanner.transform_tolerance: " << transform_tolerance_);
 }
 
 void ForwardGlobalPlanner::cleanup() {}
 
-void ForwardGlobalPlanner::activate() { planPub_->on_activate(); }
+void ForwardGlobalPlanner::activate() 
+{ 
+  RCLCPP_INFO_STREAM(nh_->get_logger(), "activating global planner ForwardGlobalPlanner");
+  this->updateParameters();
+  planPub_->on_activate(); 
+  }
 
 void ForwardGlobalPlanner::deactivate()
 {
@@ -79,6 +98,9 @@ void ForwardGlobalPlanner::deactivate()
 nav_msgs::msg::Path ForwardGlobalPlanner::createPlan(
   const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal)
 {
+
+  this->updateParameters();
+
   RCLCPP_INFO(nh_->get_logger(), "[Forward Global Planner] planning");
 
   rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
@@ -108,6 +130,11 @@ nav_msgs::msg::Path ForwardGlobalPlanner::createPlan(
 
   double length = sqrt(dx * dx + dy * dy);
 
+  RCLCPP_INFO_STREAM(
+    nh_->get_logger(),
+    "[Forward Global Planner] current plan length: " << length);
+
+
   geometry_msgs::msg::PoseStamped prevState;
   if (length > skip_straight_motion_distance_)
   {

diff --git a/...custom_planners/forward_local_planner/src/forward_local_planner/forward_local_planner.cpp b/...custom_planners/forward_local_planner/src/forward_local_planner/forward_local_planner.cpp
@@ -40,7 +40,7 @@ namespace forward_local_planner
 * ForwardLocalPlanner()
 ******************************************************************************************************************
 */
-ForwardLocalPlanner::ForwardLocalPlanner() : transform_tolerance_(0.1), waitingTimeout_(2s) {}
+ForwardLocalPlanner::ForwardLocalPlanner() : transform_tolerance_(0.05), waitingTimeout_(2s) {}
 
 ForwardLocalPlanner::~ForwardLocalPlanner() {}
 
@@ -117,7 +117,7 @@ void ForwardLocalPlanner::configure(
 }
 
 void ForwardLocalPlanner::updateParameters()
-{
+{ 
   nh_->get_parameter(name_ + ".k_rho", k_rho_);
   nh_->get_parameter(name_ + ".k_alpha", k_alpha_);
   nh_->get_parameter(name_ + ".k_betta", k_betta_);
@@ -367,6 +367,8 @@ geometry_msgs::msg::TwistStamped ForwardLocalPlanner::computeVelocityCommands(
   RCLCPP_DEBUG(
     nh_->get_logger(), "[ForwardLocalPlanner] ----- COMPUTE VELOCITY COMMAND LOCAL PLANNER ---");
 
+  RCLCPP_INFO_STREAM(
+  nh_->get_logger(), "[ForwardLocalPlanner] plan_.size:" << (int)plan_.size());
   bool ok = false;
   while (!ok)
   {
@@ -382,6 +384,8 @@ geometry_msgs::msg::TwistStamped ForwardLocalPlanner::computeVelocityCommands(
       double dx = p.x - currentPose.pose.position.x;
       double dy = p.y - currentPose.pose.position.y;
       double dist = sqrt(dx * dx + dy * dy);
+      RCLCPP_INFO_STREAM(
+      nh_->get_logger(), "[ForwardLocalPlanner] dist:" << dist);
 
       double pangle = tf2::getYaw(q);
       double angle = tf2::getYaw(currentPose.pose.orientation);