Merge pull request #3 from logivations/feature_WMO-55690_detect_and_a…

…void_obstacles_on_w2mo_paths

WMO-55690 Detect and avoid obstacles on w2mo paths

doc/parameters/param_list.md

@@ -5,7 +5,7 @@
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | default_bt_xml_filename | N/A | path to the default behavior tree XML description |
-| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_compute_waypoints_to_pose_action_bt_node", "nav2_compute_path_along_waypoints_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_path_empty_condition_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries |
+| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_compute_waypoints_to_pose_action_bt_node", "nav2_compute_path_along_waypoints_action_bt_node", "nav2_find_free_goal_on_path_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_path_empty_condition_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries |
 | transform_tolerance | 0.1 | TF transform tolerance |
 | global_frame | "map" | Reference frame |
 | robot_base_frame | "base_link" | Robot base frame |

nav2_behavior_tree/CMakeLists.txt

@@ -63,6 +63,9 @@ list(APPEND plugin_libs nav2_compute_waypoints_to_pose_action_bt_node)
 add_library(nav2_compute_path_along_waypoints_action_bt_node SHARED plugins/action/compute_path_along_waypoints_action.cpp)
 list(APPEND plugin_libs nav2_compute_path_along_waypoints_action_bt_node)
 
+add_library(nav2_find_free_goal_on_path_action_bt_node SHARED plugins/action/find_free_goal_on_path_action.cpp)
+list(APPEND plugin_libs nav2_find_free_goal_on_path_action_bt_node)
+
 add_library(nav2_follow_path_action_bt_node SHARED plugins/action/follow_path_action.cpp)
 list(APPEND plugin_libs nav2_follow_path_action_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_along_waypoints_action.hpp

@@ -42,6 +42,7 @@ class ComputePathAlongWaypointsAction : public BtActionNode<nav2_msgs::action::C
       {
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathAlongWaypoints node"),
         BT::OutputPort<nav_msgs::msg::Path>("remaining_waypoints", "Remaining waypoints to be followed"),
+        BT::InputPort<nav_msgs::msg::Path>("skip_waypoints", "Waypoints to skip"),
         BT::InputPort<nav_msgs::msg::Path>("waypoints", "Waypoints to follow"),
       });
   }

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/find_free_goal_on_path_action.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FIND_FREE_GOAL_ON_PATH_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FIND_FREE_GOAL_ON_PATH_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/find_free_goal_on_path.hpp"
+#include "nav_msgs/msg/path.h"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+    class FindFreeGoalOnPathAction : public BtActionNode<nav2_msgs::action::FindFreeGoalOnPath>
+    {
+    public:
+        FindFreeGoalOnPathAction(
+                const std::string & xml_tag_name,
+                const std::string & action_name,
+                const BT::NodeConfiguration & conf);
+
+        void on_tick() override;
+
+        BT::NodeStatus on_success() override;
+
+        static BT::PortsList providedPorts()
+        {
+            return providedBasicPorts(
+                    {
+                            BT::InputPort<nav_msgs::msg::Path>("waypoints", "Waypoints to follow"),
+                            BT::OutputPort<nav_msgs::msg::Path>("skip_waypoints", "Waypoints to skip"),
+                            BT::OutputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
+                    });
+        }
+
+    private:
+        bool first_time_{true};
+    };
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FIND_FREE_GOAL_ON_PATH_ACTION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -38,10 +38,17 @@
 
     <Action ID="ComputePathAlongWaypoints">
       <input_port name="waypoints">Waypoints to follow</input_port>
+      <input_port name="skip_waypoints">Waypoints to skip</input_port>
       <output_port name="path">Path created by ComputePathAlongWaypoints node</output_port>
       <input_port name="planner_id"/>
     </Action>
 
+    <Action ID="FindFreeGoalOnPath">
+      <output_port name="waypoints">Waypoints to follow</output_port>
+      <output_port name="skip_waypoints">Waypoints to skip</output_port>
+      <input_port name="goal">Destination to plan to</input_port>
+    </Action>
+
     <Action ID="FollowPath">
       <input_port name="controller_id" default="FollowPath"/>
       <input_port name="path">Path to follow</input_port>

nav2_behavior_tree/plugins/action/compute_path_along_waypoints_action.cpp

@@ -31,6 +31,7 @@ ComputePathAlongWaypointsAction::ComputePathAlongWaypointsAction(
 void ComputePathAlongWaypointsAction::on_tick()
 {
   getInput("waypoints", goal_.waypoints);
+  getInput("skip_waypoints", goal_.skip_waypoints);
   getInput("planner_id", goal_.planner_id);
 }
 
@@ -43,7 +44,6 @@ BT::NodeStatus ComputePathAlongWaypointsAction::on_success()
   } else {
     config().blackboard->set("path_updated", true);
   }
-
   setOutput("remaining_waypoints", result_.result->remaining_waypoints);
 
   return BT::NodeStatus::SUCCESS;

nav2_behavior_tree/plugins/action/find_free_goal_on_path_action.cpp

@@ -0,0 +1,57 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/action/find_free_goal_on_path_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+    FindFreeGoalOnPathAction::FindFreeGoalOnPathAction(
+            const std::string & xml_tag_name,
+            const std::string & action_name,
+            const BT::NodeConfiguration & conf)
+            : BtActionNode<nav2_msgs::action::FindFreeGoalOnPath>(xml_tag_name, action_name, conf)
+    {
+    }
+
+    void FindFreeGoalOnPathAction::on_tick()
+    {
+        getInput("waypoints", goal_.waypoints);
+    }
+
+    BT::NodeStatus FindFreeGoalOnPathAction::on_success()
+    {
+        setOutput("goal", result_.result->pose);
+        setOutput("skip_waypoints", result_.result->skip_waypoints);
+        return BT::NodeStatus::SUCCESS;
+    }
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+        {
+                BT::NodeBuilder builder =
+        [](const std::string & name, const BT::NodeConfiguration & config)
+        {
+            return std::make_unique<nav2_behavior_tree::FindFreeGoalOnPathAction>(
+                    name, "find_free_goal_on_path", config);
+        };
+
+        factory.registerBuilder<nav2_behavior_tree::FindFreeGoalOnPathAction>(
+        "FindFreeGoalOnPath", builder);
+        }

nav2_bringup/bringup/params/forklift_params.yaml

@@ -117,6 +117,7 @@ bt_navigator:
     - nav2_compute_path_to_pose_python_action_bt_node
     - nav2_compute_waypoints_to_pose_action_bt_node
     - nav2_compute_path_along_waypoints_action_bt_node
+    - nav2_find_free_goal_on_path_action_bt_node
     - nav2_follow_path_action_bt_node
     - nav2_back_up_action_bt_node
     - nav2_spin_action_bt_node

nav2_bt_navigator/behavior_trees/navigate_with_replanning_v2.xml

@@ -15,19 +15,19 @@
                     This fails, if the path is too far away from current pose, 
                     or if there is an obstacle on the path in front of us.
               It automatically switches to the next waypoint when necessary, and puts the remaining waypoints to remaining_waypoints -->
-              <ComputePathAlongWaypoints waypoints="{waypoints}" path="{path}" remaining_waypoints="{waypoints}"/>
+              <ComputePathAlongWaypoints waypoints="{waypoints}" skip_waypoints="{skipwaypoints}" path="{path}" remaining_waypoints="{waypoints}"/>
 
-              <!-- if the path cannot be followed, try free navigation to the next reachable point on the path-->
-              <!--<SequenceStar name="AvoidObstacle">
+               <!--if the path cannot be followed, try free navigation to the next reachable point on the path-->
+              <SequenceStar name="AvoidObstacle">
 
-                finds the next goal on the waypoint path 
-                <FindFreeGoalOnPath waypoints="{waypoints}" goal="{subgoal}"/>
+                <!--finds the next goal on the waypoint path -->
+                <FindFreeGoalOnPath waypoints="{waypoints}" goal="{subgoal}" skip_waypoints="{skipwaypoints}"/>
 
-                computePathToPose is the HybridA* planner
-                <ComputePathToPose goal="{subgoal}" path="{path}" planner_id="HybridAStar"/> 
+                <!--ComputePathToPose is the SMACPlanner planner-->
+                <ComputePathToPose goal="{subgoal}" path="{path}" planner_id="SMACPlanner"/>
 
-                <VerifyLaneChange path="{path}"/>
-              </SequenceStar>            -->
+               <!-- <VerifyLaneChange path="{path}"/> -->
+              </SequenceStar>
               <Wait wait_duration="1"/>
             </ReactiveFallback>
           </RateController>

nav2_bt_navigator/src/bt_navigator.cpp

@@ -41,6 +41,7 @@ BtNavigator::BtNavigator()
     "nav2_compute_path_to_pose_python_action_bt_node",
     "nav2_compute_waypoints_to_pose_action_bt_node",
     "nav2_compute_path_along_waypoints_action_bt_node",
+    "nav2_find_free_goal_on_path_action_bt_node",
     "nav2_follow_path_action_bt_node",
     "nav2_back_up_action_bt_node",
     "nav2_spin_action_bt_node",

nav2_msgs/CMakeLists.txt

@@ -31,6 +31,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/ComputePathToPose.action"
   "action/ComputeWaypointsToPose.action"
   "action/ComputePathAlongWaypoints.action"
+  "action/FindFreeGoalOnPath.action"
   "action/FollowPath.action"
   "action/NavigateToPose.action"
   "action/Wait.action"

nav2_msgs/action/ComputePathAlongWaypoints.action

@@ -1,5 +1,6 @@
 #goal definition
 nav_msgs/Path waypoints
+nav_msgs/Path skip_waypoints
 string planner_id
 ---
 #result definition

nav2_msgs/action/FindFreeGoalOnPath.action

@@ -0,0 +1,9 @@
+#goal definition
+nav_msgs/Path waypoints
+---
+#result definition
+geometry_msgs/PoseStamped pose
+nav_msgs/Path skip_waypoints
+builtin_interfaces/Duration planning_time
+---
+#feedback