Add temporal obstacle layer (#217)

* Add temporal obstacle layer

* add remove obstacles

* add bt nodes

mep3_behavior_tree/assets/strategies/big/purple_strategy.xml

@@ -1,14 +1,14 @@
 <root main_tree_to_execute="BehaviorTree">
     <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
-        <Sequence>
-            <DefaultTeamColor color="purple" />
-            <WaitMatchStart state="1" />
-            <!-- Robot setup (pre-match state) -->
-            <WaitMatchStart state="2" />
-            <!-- Robot strategy (match state) -->
-            <Wait duration="10000000.0" name="Wait forever as big robot" />
-        </Sequence>
+        <SequenceStar>
+            <AddObstacle label="initial" polygon="0.0;0.0|0.2;0.0|0.0;0.2" />
+            <AddObstacle label="initial" polygon="0.0;0.0|-0.2;0.0|0.0;-0.2" />
+            <Navigate goal="-0.8;0.0;0" />
+            <RemoveObstacle label="initial" />
+
+            <Wait duration="100000.0" name="Wait one big purple main strategy" />
+        </SequenceStar>
     </BehaviorTree>
     <!-- ////////// -->
-</root>
+</root>

mep3_behavior_tree/include/mep3_behavior_tree/add_obstacle_action.hpp

@@ -0,0 +1,78 @@
+// Copyright 2022 Memristor Robotics
+//
+// 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 MEP3_BEHAVIOR_TREE__ADD_OBSTACLE_ACTION_HPP_
+#define MEP3_BEHAVIOR_TREE__ADD_OBSTACLE_ACTION_HPP_
+
+#include <iostream>
+#include <string>
+
+#include "mep3_behavior_tree/pose_2d.hpp"
+#include "mep3_msgs/msg/temporal_obstacle.hpp"
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/action_node.h"
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/point.hpp"
+
+namespace mep3_behavior_tree
+{
+  class AddObstacleAction : public BT::AsyncActionNode
+  {
+  public:
+    AddObstacleAction(const std::string &name, const BT::NodeConfiguration &config_)
+        : BT::AsyncActionNode(name, config_)
+    {
+      node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+      publisher_ = node_->create_publisher<mep3_msgs::msg::TemporalObstacle>(
+          "add_obstacle", rclcpp::QoS(1).reliable());
+    }
+
+    AddObstacleAction() = delete;
+
+    BT::NodeStatus tick() override;
+
+    static BT::PortsList providedPorts()
+    {
+      return {
+          BT::InputPort<std::string>("label"),
+          BT::InputPort<std::vector<geometry_msgs::msg::Point>>("polygon")
+      };
+    }
+
+  private:
+    rclcpp::Node::SharedPtr node_;
+    rclcpp::Publisher<mep3_msgs::msg::TemporalObstacle>::SharedPtr publisher_;
+  };
+
+  BT::NodeStatus AddObstacleAction::tick()
+  {
+    std::string label;
+    std::vector<geometry_msgs::msg::Point> polygon;
+    getInput("label", label);
+    getInput<std::vector<geometry_msgs::msg::Point>>("polygon", polygon);
+
+    mep3_msgs::msg::TemporalObstacle msg;
+    msg.label = label;
+    msg.polygon = polygon;
+
+    publisher_->publish(msg);
+
+    return BT::NodeStatus::SUCCESS;
+  }
+
+} // namespace mep3_behavior_tree
+
+#endif // MEP3_BEHAVIOR_TREE__ADD_OBSTACLE_ACTION_HPP_

mep3_behavior_tree/include/mep3_behavior_tree/pose_2d.hpp

@@ -20,6 +20,7 @@
 
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
+#include "geometry_msgs/msg/point.hpp"
 
 namespace BT
 {
@@ -68,6 +69,28 @@ inline std::vector<Pose2D> convertFromString(StringView str)
   return output;
 }
 
+template<>
+inline std::vector<geometry_msgs::msg::Point> convertFromString(StringView str)
+{
+  std::vector<geometry_msgs::msg::Point> output;
+  auto pointsParts = splitString(str, '|');
+  for (auto &pointPart : pointsParts) {
+    auto parts = splitString(pointPart, ';');
+    if (parts.size() != 2 && parts.size() != 3) {
+      throw RuntimeError("invalid input)");
+    } else {
+      geometry_msgs::msg::Point point;
+      point.x = convertFromString<double>(parts[0]);
+      point.y = convertFromString<double>(parts[1]);
+      if (parts.size() == 3) {
+        point.z = convertFromString<double>(parts[2]);
+      }
+      output.push_back(point);
+    }
+  }
+  return output;
+}
+
 Pose2D& operator+=(Pose2D& lhs, const Pose2D& rhs)
 {
   lhs.x += rhs.x;

mep3_behavior_tree/include/mep3_behavior_tree/remove_obstacle_action.hpp

@@ -0,0 +1,71 @@
+// Copyright 2022 Memristor Robotics
+//
+// 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 MEP3_BEHAVIOR_TREE__REMOVE_OBSTACLE_ACTION_HPP_
+#define MEP3_BEHAVIOR_TREE__REMOVE_OBSTACLE_ACTION_HPP_
+
+#include <iostream>
+#include <string>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/action_node.h"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+namespace mep3_behavior_tree
+{
+  class RemoveObstacleAction : public BT::AsyncActionNode
+  {
+  public:
+    RemoveObstacleAction(const std::string &name, const BT::NodeConfiguration &config_)
+        : BT::AsyncActionNode(name, config_)
+    {
+      node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+      publisher_ = node_->create_publisher<std_msgs::msg::String>(
+          "remove_obstacle", rclcpp::QoS(1).reliable());
+    }
+
+    RemoveObstacleAction() = delete;
+
+    BT::NodeStatus tick() override;
+
+    static BT::PortsList providedPorts()
+    {
+      return {
+          BT::InputPort<std::string>("label"),
+      };
+    }
+
+  private:
+    rclcpp::Node::SharedPtr node_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  };
+
+  BT::NodeStatus RemoveObstacleAction::tick()
+  {
+    std::string label;
+    getInput("label", label);
+
+    std_msgs::msg::String msg;
+    msg.data = label;
+
+    publisher_->publish(msg);
+
+    return BT::NodeStatus::SUCCESS;
+  }
+
+} // namespace mep3_behavior_tree
+
+#endif // MEP3_BEHAVIOR_TREE__REMOVE_OBSTACLE_ACTION_HPP_

mep3_behavior_tree/src/mep3_behavior_tree.cpp

@@ -43,6 +43,8 @@
 #include "mep3_behavior_tree/set_shared_blackboard_action.hpp"
 #include "mep3_behavior_tree/blackboard_control_flow.hpp"
 #include "mep3_behavior_tree/navigate_through_action.hpp"
+#include "mep3_behavior_tree/add_obstacle_action.hpp"
+#include "mep3_behavior_tree/remove_obstacle_action.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 using KeyValueT = diagnostic_msgs::msg::KeyValue;
@@ -204,6 +206,12 @@ int main(int argc, char **argv)
   factory.registerNodeType<mep3_behavior_tree::NavigateThroughAction>(
     "NavigateThrough"
   );
+  factory.registerNodeType<mep3_behavior_tree::AddObstacleAction>(
+    "AddObstacle"
+  );
+  factory.registerNodeType<mep3_behavior_tree::RemoveObstacleAction>(
+    "RemoveObstacle"
+  );
 
   BT::Tree tree_main = factory.createTreeFromFile(tree_file_path, blackboard);
   BT::StdCoutLogger logger_cout(tree_main);

mep3_msgs/CMakeLists.txt

@@ -18,19 +18,18 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
-
+find_package(geometry_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "action/DynamixelCommand.action"
   "action/MotionCommand.action"
   "action/ResistanceMeter.action"
-  "action/Scoreboard.msg"
+  "msgs/Scoreboard.msg"
   "action/VacuumPumpCommand.action"
+  "msgs/TemporalObstacle.msg"
+  DEPENDENCIES geometry_msgs
 )
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-endif()
+ament_export_dependencies(rosidl_default_runtime)
 
 ament_package()

mep3_msgs/msgs/TemporalObstacle.msg

@@ -0,0 +1,2 @@
+string label
+geometry_msgs/Point[] polygon

mep3_navigation/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -40,6 +41,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(can_msgs REQUIRED)
 find_package(laser_filters REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
 
 include_directories(include)
 
@@ -121,6 +123,31 @@ install(TARGETS nav2_globally_updated_goal_condition_bt_node
   RUNTIME DESTINATION bin
 )
 
+# temporal_obstacle_layer
+add_library(
+  temporal_obstacle_layer
+  SHARED
+  src/temporal_obstacle_layer/temporal_obstacle_layer.cpp
+)
+ament_target_dependencies(
+  temporal_obstacle_layer
+  rclcpp
+  nav2_costmap_2d
+  pluginlib
+  mep3_msgs
+  diagnostic_msgs
+)
+target_include_directories(
+  temporal_obstacle_layer
+  PRIVATE
+  include
+)
+install(
+  TARGETS temporal_obstacle_layer
+  DESTINATION lib
+)
+pluginlib_export_plugin_description_file(nav2_costmap_2d temporal_obstacle_layer.xml)
+
 # Assets
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY params DESTINATION share/${PROJECT_NAME})

mep3_navigation/include/mep3_navigation/temporal_obstacle_layer/temporal_obstacle_layer.hpp

@@ -0,0 +1,65 @@
+// Copyright (c) 2020 Shrijit Singh
+// Copyright (c) 2020 Samsung Research America
+//
+// 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 MEP3_NAVIGATION__TEMPORAL_OBSTACLE_LAYER_HPP_
+#define MEP3_NAVIGATION__TEMPORAL_OBSTACLE_LAYER_HPP_
+
+#include <string>
+#include <vector>
+
+#include "nav2_costmap_2d/layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+#include "mep3_msgs/msg/temporal_obstacle.hpp"
+
+namespace mep3_navigation {
+class TemporalObstacleLayer : public nav2_costmap_2d::Layer {
+public:
+  TemporalObstacleLayer();
+
+  virtual void onInitialize();
+  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
+                            double *min_x, double *min_y, double *max_x,
+                            double *max_y);
+  virtual void updateCosts(nav2_costmap_2d::Costmap2D &master_grid, int min_i,
+                           int min_j, int max_i, int max_j);
+
+  virtual void reset() { return; }
+
+  virtual void onFootprintChanged();
+
+  virtual bool isClearable() { return false; }
+
+private:
+  rclcpp::Subscription<mep3_msgs::msg::TemporalObstacle>::SharedPtr
+      add_obstacle_subscriber_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr
+      remove_obstacle_subscriber_;
+
+  void on_new_obstacle(const mep3_msgs::msg::TemporalObstacle::SharedPtr msg);
+  void on_remove_obstacle(const std_msgs::msg::String::SharedPtr msg);
+
+  std::string add_obstacle_topic_;
+  std::string remove_obstacle_topic_;
+
+  bool need_update_;
+  std::vector<mep3_msgs::msg::TemporalObstacle::SharedPtr> obstacles_;
+};
+
+} // namespace mep3_navigation
+
+#endif // MEP3_NAVIGATION__TEMPORAL_OBSTACLE_LAYER_HPP_

mep3_navigation/params/nav2_params_big.yaml

@@ -186,7 +186,7 @@ global_costmap:
       robot_radius: 0.2
       resolution: 0.01
       track_unknown_space: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer", "temporal_obstacle_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
@@ -205,6 +205,10 @@ global_costmap:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 4.0
         inflation_radius: 1.0
+      temporal_obstacle_layer:
+        plugin: "mep3_navigation::TemporalObstacleLayer"
+        add_obstacle_topic: /big/add_obstacle
+        remove_obstacle_topic: /big/remove_obstacle
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters: