Mission planner bt

src/MissionPlanner_BT/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.5)
+project(MissionPlanner_BT)
+aux_source_directory(./src/ SRC_LIST)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+
+find_package(BTpp REQUIRED)
+
+include_directories(include ${BTpp_INCLUDE_DIRS})
+
+add_executable(${PROJECT_NAME} ${SRC_LIST})
+
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${BTpp_LIBRARIES})
+

src/MissionPlanner_BT/README.md

@@ -0,0 +1,5 @@
+#Mission Planner Behaivor Tree
+------
+Clone/Download and install the library [BT++](https://github.com/miccol/Behavior-Tree) system-wide
+
+

src/MissionPlanner_BT/include/CancelAction.h

@@ -0,0 +1,24 @@
+// 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 <behavior_tree.h>
+
+class CancelAction : public BT::ActionNode
+{
+public:
+  CancelAction(std::string name);
+  ~CancelAction();
+  BT::ReturnStatus Tick();
+  void Halt();
+};

src/MissionPlanner_BT/include/CancelCondition.h

@@ -0,0 +1,27 @@
+// 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 <behavior_tree.h>
+
+class CancelCondition : public BT::ConditionNode
+{
+public:
+  CancelCondition(std::string name);
+  ~CancelCondition();
+  void set_boolean_value(bool boolean_value);
+  BT::ReturnStatus Tick();
+
+private:
+  bool boolean_value_;
+};

src/MissionPlanner_BT/include/Nav2PoseAction.h

@@ -0,0 +1,24 @@
+// 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 <behavior_tree.h>
+
+class Nav2PoseAction : public BT::ActionNode
+{
+public:
+  Nav2PoseAction(std::string name);
+  ~Nav2PoseAction();
+  BT::ReturnStatus Tick();
+  void Halt();
+};

src/MissionPlanner_BT/include/Nav2PoseCondition.h

@@ -0,0 +1,27 @@
+// 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 <behavior_tree.h>
+
+class Nav2PoseCondition : public BT::ConditionNode
+{
+public:
+  Nav2PoseCondition(std::string name);
+  ~Nav2PoseCondition();
+  void set_boolean_value(bool boolean_value);
+  BT::ReturnStatus Tick();
+
+private:
+  bool boolean_value_;
+};

src/MissionPlanner_BT/include/PlannerAction.h

@@ -0,0 +1,24 @@
+// 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 <behavior_tree.h>
+
+class PlannerAction : public BT::ActionNode
+{
+public:
+  PlannerAction(std::string name);
+  ~PlannerAction();
+  BT::ReturnStatus Tick();
+  void Halt();
+};

src/MissionPlanner_BT/include/TakePicAction.h

@@ -0,0 +1,24 @@
+// 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 <behavior_tree.h>
+
+class TakePicAction : public BT::ActionNode
+{
+public:
+  TakePicAction(std::string name);
+  ~TakePicAction();
+  BT::ReturnStatus Tick();
+  void Halt();
+};

src/MissionPlanner_BT/include/TakePicCondition.h

@@ -0,0 +1,27 @@
+// 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 <behavior_tree.h>
+
+class TakePicCondition : public BT::ConditionNode
+{
+public:
+  TakePicCondition(std::string name);
+  ~TakePicCondition();
+  void set_boolean_value(bool boolean_value);
+  BT::ReturnStatus Tick();
+
+private:
+  bool boolean_value_;
+};

src/MissionPlanner_BT/src/CancelAction.cpp

@@ -0,0 +1,45 @@
+// 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 "CancelAction.h"
+
+CancelAction::CancelAction(std::string name) : ActionNode::ActionNode(name)
+{
+}
+
+BT::ReturnStatus CancelAction::Tick()
+{
+    if (is_halted())
+    {
+        return BT::HALTED;
+    }
+    int count = 0;
+    while (count != 10)
+    {
+        count++;
+        std::cout << "Canceling the mission... " << count << std::endl;
+        if (count == 2)
+        {
+            count = 0;
+            std::cout << "Mission is canceled!" << std::endl;
+            return BT::SUCCESS;
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+}
+
+void CancelAction::Halt()
+{
+    std::cout << "Cancel Action is halted!" << std::endl;
+}

src/MissionPlanner_BT/src/CancelCondition.cpp

@@ -0,0 +1,42 @@
+// 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 "CancelCondition.h"
+
+CancelCondition::CancelCondition(std::string name) : BT::ConditionNode::ConditionNode(name)
+{
+    type_ = BT::CONDITION_NODE;
+    boolean_value_ = true;
+}
+
+CancelCondition::~CancelCondition() {}
+
+BT::ReturnStatus CancelCondition::Tick()
+{
+    if (boolean_value_)
+    {
+        set_status(BT::SUCCESS);
+        return BT::SUCCESS;
+    }
+    else
+    {
+        set_status(BT::FAILURE);
+        return BT::FAILURE;
+    }
+}
+
+void CancelCondition::set_boolean_value(bool boolean_value)
+{
+    boolean_value_ = boolean_value;
+}

src/MissionPlanner_BT/src/Nav2PoseAction.cpp

@@ -0,0 +1,48 @@
+// 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 "Nav2PoseAction.h"
+
+Nav2PoseAction::Nav2PoseAction(std::string name) : ActionNode::ActionNode(name)
+{
+}
+
+BT::ReturnStatus Nav2PoseAction::Tick()
+{
+    if (is_halted())
+    {
+        return BT::HALTED;
+    }
+    int count = 0;
+    while (count != 10)
+    {
+        count++;
+        std::cout << "Navigating to pose.. " << count << std::endl;
+        if (count == 5)
+        {
+            count = 0;
+            std::cout << "Navigating to pose has succeeded!" << std::endl;
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+            return BT::SUCCESS;
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+        //else
+        //return BT::RUNNING;
+    }
+}
+
+void Nav2PoseAction::Halt()
+{
+    std::cout << "Nav2Pose Action is halted!" << std::endl;
+}

src/MissionPlanner_BT/src/Nav2PoseCondition.cpp

@@ -0,0 +1,42 @@
+// 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 "Nav2PoseCondition.h"
+
+Nav2PoseCondition::Nav2PoseCondition(std::string name) : BT::ConditionNode::ConditionNode(name)
+{
+    type_ = BT::CONDITION_NODE;
+    boolean_value_ = true;
+}
+
+Nav2PoseCondition::~Nav2PoseCondition() {}
+
+BT::ReturnStatus Nav2PoseCondition::Tick()
+{
+    if (boolean_value_)
+    {
+        set_status(BT::SUCCESS);
+        return BT::SUCCESS;
+    }
+    else
+    {
+        set_status(BT::FAILURE);
+        return BT::FAILURE;
+    }
+}
+
+void Nav2PoseCondition::set_boolean_value(bool boolean_value)
+{
+    boolean_value_ = boolean_value;
+}