Merge pull request #27 from DIT-ROBOTICS/develop

Develop

Author: Rabbit025879

Navigation2/nav2_behavior_tree/plugins/decorator/stop_controller.cpp

@@ -34,7 +34,7 @@ namespace nav2_behavior_tree
             cmd_vel.angular.z = 0.0;
             cmd_vel_pub->publish(cmd_vel);
             RCLCPP_INFO(node_->get_logger(), "running in stop_controller");
-            return BT::NodeStatus::RUNNING;
+            return BT::NodeStatus::FAILURE;
         }
         return child_node_->executeTick();
     }

Navigation2/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml

@@ -1,33 +1,35 @@
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
-    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
-      <StopController stop="false">
-        <PipelineSequence name="NavigateWithReplanning">
-          <RateController hz="5">
-            <RecoveryNode number_of_retries="1" name="ComputePathToPose">
-              <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+      <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+        <StopController>
+          <PipelineSequence name="NavigateWithReplanning">
+            <RateController hz="5">
+              <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+                <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+                <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+              </RecoveryNode>
+            </RateController>
+            <RecoveryNode number_of_retries="1" name="FollowPath">
+              <FollowPath path="{path}" controller_id="FollowPath"/>
+              <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
             </RecoveryNode>
-          </RateController>
-          <RecoveryNode number_of_retries="1" name="FollowPath">
-            <FollowPath path="{path}" controller_id="FollowPath"/>
-            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
-          </RecoveryNode>
-        </PipelineSequence>
-      </StopController>
-      <ReactiveFallback name="RecoveryFallback">
-        <GoalUpdated/>
-        <RoundRobin name="RecoveryActions">
-          <!-- <Sequence name="ClearingActions">
-            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
-          </Sequence> -->
-          <Wait wait_duration="3"/>
-          <Shrink shrink_to="0.05"/>
-          <Wait wait_duration="1"/>
-          <Escape run_dist="5" time_allowance="5" is_recovery="1"/>
-        </RoundRobin>
-      </ReactiveFallback>
-    </RecoveryNode>
+          </PipelineSequence>
+        </StopController>
+        <StopController>
+          <ReactiveFallback name="RecoveryFallback">
+            <GoalUpdated/>
+            <RoundRobin name="RecoveryActions">
+              <!-- <Sequence name="ClearingActions">
+                <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+                <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+              </Sequence> -->
+              <Wait wait_duration="3"/>
+              <Shrink shrink_to="0.05"/>
+              <Wait wait_duration="1"/>
+              <Escape run_dist="5" time_allowance="5" is_recovery="1"/>
+            </RoundRobin>
+          </ReactiveFallback>
+        </StopController>
+      </RecoveryNode>
   </BehaviorTree>
 </root>

custom_bts/custom_nav_thru_poses/src/ControllerSelector.cpp

@@ -19,19 +19,19 @@ class ControllerSelector : public rclcpp::Node {
             controller_selector_pub_ = this->create_publisher<std_msgs::msg::String>("/controller_type", rclcpp::QoS(10).reliable().transient_local());
             controller_selector_timer_ = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&ControllerSelector::timer_callback, this));
 
-            declare_parameter("Path_controller", rclcpp::ParameterValue("FollowPath"));
-            declare_parameter("Dock_controller", rclcpp::ParameterValue("DockMission"));
+            declare_parameter("Fast_controller", rclcpp::ParameterValue("FollowPath"));
+            declare_parameter("Slow_controller", rclcpp::ParameterValue("Cautious"));
 
-            this->get_parameter("Path_controller", path_controller_);
-            this->get_parameter("Dock_controller", dock_controller_);
+            this->get_parameter("Fast_controller", fast_controller_);
+            this->get_parameter("Slow_controller", slow_controller_);
         }   
 
     private:
         void feedback_callback(nav2_msgs::action::NavigateThroughPoses::Feedback feedback) {
             if(feedback.number_of_poses_remaining <= 1) {
-                controller_type_ = dock_controller_;
+                controller_type_ = slow_controller_;
             } else {
-                controller_type_ = path_controller_;
+                controller_type_ = fast_controller_;
             }
 
             if(controller_type_prev_ != controller_type_)    RCLCPP_INFO(this->get_logger(), "Controller type has switch to '%s'", controller_type_.c_str());
@@ -60,7 +60,7 @@ class ControllerSelector : public rclcpp::Node {
         // Hardcoded controller selection
         std::string controller_type_;
         std::string controller_type_prev_ = "None";
-        std::string path_controller_, dock_controller_;
+        std::string fast_controller_, slow_controller_;
 };
 
 int main(int argc, char * argv[]) {

custom_controller/include/custom_controller/custom_controller.hpp

@@ -123,7 +123,7 @@ class CustomController : public nav2_core::Controller{
         int check_index_;
         int current_index_;
         bool isObstacleExist_;
-        bool keep_palnning_;
+        bool keep_planning_;
 
 };
 

custom_controller/src/custom_controller.cpp

@@ -89,6 +89,7 @@ void CustomController::configure(
     declare_parameter_if_not_declared(node, plugin_name_ + ".look_ahead_distance", rclcpp::ParameterValue(1.0));
     declare_parameter_if_not_declared(node, plugin_name_ + ".costmap_tolerance", rclcpp::ParameterValue(60));
     declare_parameter_if_not_declared(node, plugin_name_ + ".speed_decade", rclcpp::ParameterValue(0.7));
+    declare_parameter_if_not_declared(node, plugin_name_ + ".keep_planning", rclcpp::ParameterValue(true));
 
     //declare_parameter_if_not_declared(node, plugin_name_ + ".keepPlan", rclcpp::ParameterValue(ture));
     // Get parameters from the config file
@@ -102,9 +103,9 @@ void CustomController::configure(
     node->get_parameter(plugin_name_ + ".angular_kp", angular_kp_);
     node->get_parameter(plugin_name_ + ".linear_kp", linear_kp_);
     node->get_parameter(plugin_name_ + ".look_ahead_distance", look_ahead_distance_);
-    node->get_parameter(plugin_name_ + ".keep_palnning", keep_palnning_);
     node->get_parameter(plugin_name_ + ".costmap_tolerance", costmap_tolerance_);
     node->get_parameter(plugin_name_ + ".speed_decade", speed_decade_);
+    node->get_parameter(plugin_name_ + ".keep_planning", keep_planning_);
     double transform_tolerance;
 
     node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance);
@@ -140,34 +141,40 @@ void CustomController::deactivate(){
 // Get the global plan with transformed poses
 void CustomController::setPlan(const nav_msgs::msg::Path & path)
 {   
-    if(cur_goal_pose_.x_ != path.poses.back().pose.position.x || cur_goal_pose_.y_ != path.poses.back().pose.position.y) update_plan_ = true;
+    if(cur_goal_pose_.x_ != path.poses.back().pose.position.x || cur_goal_pose_.y_ != path.poses.back().pose.position.y 
+        || cur_goal_pose_.theta_ != tf2::getYaw(path.poses.back().pose.orientation)) update_plan_ = true;
+    
     if(!update_plan_){
+        RCLCPP_INFO(logger_, "cur_goal_pose_ is [%lf] [%lf] [%lf]", cur_goal_pose_.x_, cur_goal_pose_.y_, cur_goal_pose_.theta_);
+        RCLCPP_INFO(logger_, "path.poses.back().pose.position is [%lf] [%lf] [%lf]", path.poses.back().pose.position.x, path.poses.back().pose.position.y, tf2::getYaw(path.poses.back().pose.orientation));
+        RCLCPP_INFO(logger_, "The global plan is not updated");
         return;
     }
 
     if(!global_plan_.poses.empty()){
         global_plan_.poses.clear();
     }   
     global_plan_ = path;
-    RCLCPP_INFO(logger_, "Received a new plan");
+    // RCLCPP_INFO(logger_, "Received a new plan");
     cur_goal_pose_.x_ = global_plan_.poses.back().pose.position.x;
     cur_goal_pose_.y_ = global_plan_.poses.back().pose.position.y;
+    cur_goal_pose_.theta_ = tf2::getYaw(global_plan_.poses.back().pose.orientation);
 
     auto msg = std::make_unique<nav_msgs::msg::Path>(global_plan_);
     global_plan_.header.stamp = path.header.stamp;
     global_plan_.header.frame_id = path.header.frame_id;  // or "odom"
     global_path_pub_->publish(std::move(msg));
 
     //RCLCPP_INFO(logger_, "global_plan_.orientation x y z w = [%lf] [%lf] [%lf] [%lf]", global_plan_.poses.back().pose.orientation.x, global_plan_.poses.back().pose.orientation.y, global_plan_.poses.back().pose.orientation.z, global_plan_.poses.back().pose.orientation.w);
-    tf2::Quaternion q;
-    tf2::fromMsg(global_plan_.poses.back().pose.orientation, q);
-    tf2::Matrix3x3 qt(q);
-    double pitch, row, yaw;
-    qt.getRPY(pitch, row, yaw);
-    //RCLCPP_INFO(logger_, "yaw is = [%lf]", yaw);
-    final_goal_angle_ = yaw;
-
-    update_plan_ = keep_palnning_;
+    // tf2::Quaternion q;
+    // tf2::fromMsg(global_plan_.poses.back().pose.orientation, q);
+    // tf2::Matrix3x3 qt(q);
+    // double pitch, row, yaw;
+    // qt.getRPY(pitch, row, yaw);
+    // RCLCPP_INFO(logger_, "yaw is = [%lf]", yaw);
+    final_goal_angle_ = tf2::getYaw(global_plan_.poses.back().pose.orientation);
+
+    update_plan_ = keep_planning_;
     // update_plan_ = true;
     isObstacleExist_ = false;
     //print the distance between the points
@@ -288,35 +295,18 @@ RobotState CustomController::getLookAheadPoint(
 
 double CustomController::getGoalAngle(double cur_angle, double goal_angle) {
     double ang_diff_ = goal_angle - cur_angle;
-    double angular_max_vel_ = 2.0;
-    double angle_vel_ = 0.0;
-    double angular_kp_ = 4.0;
-    if(cur_angle >= 0 && goal_angle >= 0){
-            if(ang_diff_ >= 0) angle_vel_ = std::min((ang_diff_ * angular_kp_), angular_max_vel_);
-            else angle_vel_ = std::max((ang_diff_ * angular_kp_), -angular_max_vel_);
-        }
-        else if(cur_angle < 0 && goal_angle < 0){
-           if(ang_diff_ >= 0) angle_vel_ = std::min((ang_diff_ * angular_kp_), angular_max_vel_);
-            else angle_vel_ = std::max((ang_diff_ * angular_kp_), -angular_max_vel_);
-        }
-        else if(cur_angle < 0 && goal_angle >= 0){
-            if((fabs(cur_angle) + goal_angle) >= M_PI) angle_vel_ = std::max((-ang_diff_ * angular_kp_), -angular_max_vel_);
-            else angle_vel_ = std::min((ang_diff_ * angular_kp_), angular_max_vel_); 
-        }
-        else{
-            if((cur_angle + fabs(goal_angle)) <= M_PI) angle_vel_ = std::max((ang_diff_ * angular_kp_), -angular_max_vel_);
-            else angle_vel_ = std::min((-ang_diff_ * angular_kp_), angular_max_vel_);
-        }
+
+    if (ang_diff_ > M_PI) {
+        ang_diff_ -= 2.0 * M_PI;
+    } else if (ang_diff_ < -M_PI) {
+        ang_diff_ += 2.0 * M_PI;
+    }
+
+    double angle_vel_ = std::clamp(ang_diff_ * angular_kp_, -max_angular_vel_, max_angular_vel_);
     return angle_vel_;
-    // if(goal_angle - cur_angle >= 3.1415926) {
-    //     return (goal_angle - cur_angle - 3.1415926 * 2);
-    // } else if(goal_angle - cur_angle < -3.1415926) {
-    //     return (goal_angle - cur_angle + 3.1415926 * 2);
-    // } else {
-    //     return goal_angle;
-    // }
 }
 
+
 int CustomController::getIndex(RobotState cur_pose, std::vector<RobotState> &path, double look_ahead_distance){
     if (path.empty()) {
         RCLCPP_INFO(logger_, "[%s] Path is empty", plugin_name_.c_str());
@@ -439,7 +429,7 @@ geometry_msgs::msg::TwistStamped CustomController::computeVelocityCommands(
     cmd_vel.header.frame_id = pose.header.frame_id;
     cmd_vel.header.stamp = clock_->now();
 
-    if(!goal_checker->isGoalReached(pose.pose, global_plan_.poses.back().pose, velocity)){
+    // if(!goal_checker->isGoalReached(pose.pose, global_plan_.poses.back().pose, velocity)){
 
 
         local_goal_ = getLookAheadPoint(cur_pose_, vector_global_path_, look_ahead_distance_);
@@ -503,26 +493,26 @@ geometry_msgs::msg::TwistStamped CustomController::computeVelocityCommands(
         }
 
         return cmd_vel;
-    }
-    else if(fabs(final_goal_angle_ - cur_pose_.theta_) > 0.01){
-        cmd_vel.twist.linear.x = 0.0;
-        cmd_vel.twist.linear.y = 0.0;
-        cmd_vel.twist.angular.z = getGoalAngle(cur_pose_.theta_, final_goal_angle_);
-        return cmd_vel;
-    }
-    else{
-        RCLCPP_INFO(logger_, "[%s] Goal reached", plugin_name_.c_str());
-        cmd_vel.twist.linear.x = 0.0;
-        cmd_vel.twist.linear.y = 0.0;
-        cmd_vel.twist.angular.z = 0.0;
-        update_plan_ = true;
-        return cmd_vel;
-    }
+    // }
+    // else if(fabs(final_goal_angle_ - cur_pose_.theta_) > 0.01){
+    //     cmd_vel.twist.linear.x = 0.0;
+    //     cmd_vel.twist.linear.y = 0.0;
+    //     cmd_vel.twist.angular.z = getGoalAngle(cur_pose_.theta_, final_goal_angle_);
+    //     return cmd_vel;
+    // }
+    // else{
+    //     RCLCPP_INFO(logger_, "[%s] Goal reached", plugin_name_.c_str());
+    //     cmd_vel.twist.linear.x = 0.0;
+    //     cmd_vel.twist.linear.y = 0.0;
+    //     cmd_vel.twist.angular.z = 0.0;
+    //     update_plan_ = true;
+    //     return cmd_vel;
+    // }
 }
 //test
 void CustomController::setSpeedLimit(
-  const double & speed_limit,
-  const bool & percentage)
+  const double & /*speed_limit*/,
+  const bool & /*percentage*/)
 {
     return;
 //   if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {

custom_layer/object_layer/include/object_layer/object_layer.hpp

@@ -37,12 +37,13 @@ namespace Object_costmap_plugin {
             // data processes
             void columnPoseArrayCallback(const geometry_msgs::msg::PoseArray::SharedPtr object_poseArray);
             void boardPoseArrayCallback(const geometry_msgs::msg::PoseArray::SharedPtr object_poseArray);
+            void checkClear();        
         private:
             std::deque<geometry_msgs::msg::PoseStamped> columnList;
             std::deque<geometry_msgs::msg::PoseStamped> boardList;
             rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr column_poseArray_sub;
             rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr board_poseArray_sub;
-
+            int clearTimer;
             double column_inscribed_radius, board_inscribed_radius;
             double column_inflation_radius, board_inflation_radius;
             double cost_scaling_factor;

custom_layer/object_layer/plugin/object_layer.cpp

@@ -31,13 +31,13 @@ namespace Object_costmap_plugin {
         node->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor);
 
         column_poseArray_sub = node->create_subscription<geometry_msgs::msg::PoseArray>(
-            "/column_pose_array", 100, std::bind(&ObjectLayer::columnPoseArrayCallback, this, std::placeholders::_1));
+            "/detected/global_center_poses/column", 100, std::bind(&ObjectLayer::columnPoseArrayCallback, this, std::placeholders::_1));
         board_poseArray_sub = node->create_subscription<geometry_msgs::msg::PoseArray>(
-            "/board_pose_array", 100, std::bind(&ObjectLayer::boardPoseArrayCallback, this, std::placeholders::_1));
+            "/detected/global_center_poses/platform", 100, std::bind(&ObjectLayer::boardPoseArrayCallback, this, std::placeholders::_1));
 
         columnList.clear();
         boardList.clear();
-        
+        clearTimer = 20;
     }
 
     void ObjectLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
@@ -64,6 +64,16 @@ namespace Object_costmap_plugin {
             ExpandPointWithRectangle(object.pose.position.x, object.pose.position.y, nav2_costmap_2d::LETHAL_OBSTACLE, board_inflation_radius, cost_scaling_factor, board_inscribed_radius, object);
         }
         updateWithMax(master_grid, 0, 0, getSizeInCellsX(), getSizeInCellsY());
+        checkClear();
+    }
+
+    void ObjectLayer::checkClear(){
+        if(clearTimer == 0){
+            boardList.clear();
+            columnList.clear();
+            clearTimer = 20;
+        }
+        clearTimer--;
     }
 
     bool ObjectLayer::isClearable(){
@@ -82,7 +92,7 @@ namespace Object_costmap_plugin {
     }
 
     void ObjectLayer::columnPoseArrayCallback(const geometry_msgs::msg::PoseArray::SharedPtr object_poseArray){
-        columnList.clear();
+        // columnList.clear();
         for(auto pose : object_poseArray->poses){
             geometry_msgs::msg::PoseStamped poseStamped;
             poseStamped.pose = pose;
@@ -91,7 +101,7 @@ namespace Object_costmap_plugin {
     }
 
     void ObjectLayer::boardPoseArrayCallback(const geometry_msgs::msg::PoseArray::SharedPtr object_poseArray){
-        boardList.clear();
+        // boardList.clear();
         for(auto pose : object_poseArray->poses){
             geometry_msgs::msg::PoseStamped poseStamped;
             poseStamped.pose = pose;
@@ -146,10 +156,13 @@ namespace Object_costmap_plugin {
         double cosy_cosp = 1.0 - 2.0 * (object.pose.orientation.y * object.pose.orientation.y +
                                         object.pose.orientation.z * object.pose.orientation.z);
         double angle = std::atan2(siny_cosp, cosy_cosp);
-        
+        // RCLCPP_WARN(
+        //     rclcpp::get_logger("ObjectLayer"), 
+        //     "my angle : %lf", angle);        
         // Precompute sine and cosine of the angle.
-        double cosAngle = std::cos(angle);
-        double sinAngle = std::sin(angle);
+        double receivedAngle = object.pose.orientation.x;
+        double cosAngle = std::cos(receivedAngle);
+        double sinAngle = std::sin(receivedAngle);
 
         unsigned int mx, my;
         double cost = 0.0;