diff --git a/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml b/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml index 7ab5a2cd8..65a1dd805 100644 --- a/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml +++ b/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml @@ -1,14 +1,14 @@ - - - - - - - - + + + + + + + + - \ No newline at end of file + diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/add_obstacle_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/add_obstacle_action.hpp new file mode 100644 index 000000000..c500259a9 --- /dev/null +++ b/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 +#include + +#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("node"); + publisher_ = node_->create_publisher( + "add_obstacle", rclcpp::QoS(1).reliable()); + } + + AddObstacleAction() = delete; + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("label"), + BT::InputPort>("polygon") + }; + } + + private: + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr publisher_; + }; + + BT::NodeStatus AddObstacleAction::tick() + { + std::string label; + std::vector polygon; + getInput("label", label); + getInput>("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_ diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/pose_2d.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/pose_2d.hpp index 5d7f65475..998f366d3 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/pose_2d.hpp +++ b/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 convertFromString(StringView str) return output; } +template<> +inline std::vector convertFromString(StringView str) +{ + std::vector 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(parts[0]); + point.y = convertFromString(parts[1]); + if (parts.size() == 3) { + point.z = convertFromString(parts[2]); + } + output.push_back(point); + } + } + return output; +} + Pose2D& operator+=(Pose2D& lhs, const Pose2D& rhs) { lhs.x += rhs.x; diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/remove_obstacle_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/remove_obstacle_action.hpp new file mode 100644 index 000000000..64da344fe --- /dev/null +++ b/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 +#include + +#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("node"); + publisher_ = node_->create_publisher( + "remove_obstacle", rclcpp::QoS(1).reliable()); + } + + RemoveObstacleAction() = delete; + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("label"), + }; + } + + private: + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::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_ diff --git a/mep3_behavior_tree/src/mep3_behavior_tree.cpp b/mep3_behavior_tree/src/mep3_behavior_tree.cpp index 16b8e704b..9cf01436a 100644 --- a/mep3_behavior_tree/src/mep3_behavior_tree.cpp +++ b/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( "NavigateThrough" ); + factory.registerNodeType( + "AddObstacle" + ); + factory.registerNodeType( + "RemoveObstacle" + ); BT::Tree tree_main = factory.createTreeFromFile(tree_file_path, blackboard); BT::StdCoutLogger logger_cout(tree_main); diff --git a/mep3_msgs/CMakeLists.txt b/mep3_msgs/CMakeLists.txt index bda9b6cf5..8f664dd7a 100644 --- a/mep3_msgs/CMakeLists.txt +++ b/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() diff --git a/mep3_msgs/action/Scoreboard.msg b/mep3_msgs/msgs/Scoreboard.msg similarity index 100% rename from mep3_msgs/action/Scoreboard.msg rename to mep3_msgs/msgs/Scoreboard.msg diff --git a/mep3_msgs/msgs/TemporalObstacle.msg b/mep3_msgs/msgs/TemporalObstacle.msg new file mode 100644 index 000000000..300312c80 --- /dev/null +++ b/mep3_msgs/msgs/TemporalObstacle.msg @@ -0,0 +1,2 @@ +string label +geometry_msgs/Point[] polygon diff --git a/mep3_navigation/CMakeLists.txt b/mep3_navigation/CMakeLists.txt index b601f788f..d73cf4de9 100644 --- a/mep3_navigation/CMakeLists.txt +++ b/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}) diff --git a/mep3_navigation/include/mep3_navigation/temporal_obstacle_layer/temporal_obstacle_layer.hpp b/mep3_navigation/include/mep3_navigation/temporal_obstacle_layer/temporal_obstacle_layer.hpp new file mode 100644 index 000000000..7a512bd51 --- /dev/null +++ b/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 +#include + +#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::SharedPtr + add_obstacle_subscriber_; + rclcpp::Subscription::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 obstacles_; +}; + +} // namespace mep3_navigation + +#endif // MEP3_NAVIGATION__TEMPORAL_OBSTACLE_LAYER_HPP_ diff --git a/mep3_navigation/params/nav2_params_big.yaml b/mep3_navigation/params/nav2_params_big.yaml index bba200644..88ae7d19f 100644 --- a/mep3_navigation/params/nav2_params_big.yaml +++ b/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: diff --git a/mep3_navigation/src/temporal_obstacle_layer/temporal_obstacle_layer.cpp b/mep3_navigation/src/temporal_obstacle_layer/temporal_obstacle_layer.cpp new file mode 100644 index 000000000..173cf6ab2 --- /dev/null +++ b/mep3_navigation/src/temporal_obstacle_layer/temporal_obstacle_layer.cpp @@ -0,0 +1,107 @@ +// 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. + +#include "mep3_navigation/temporal_obstacle_layer/temporal_obstacle_layer.hpp" + +#include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "rclcpp/parameter_events_filter.hpp" + +using nav2_costmap_2d::LETHAL_OBSTACLE; + +namespace mep3_navigation +{ + + // ros2 topic pub -1 /big/add_obstacle mep3_msgs/msg/TemporalObstacle "{label: 'test', polygon: [{x: 0.0, y: 0.0}, {x: -0.2, y: 0.0}, {x: 0.0, y: 0.2}]}" + // ros2 topic pub -1 /big/remove_obstacle std_msgs/msg/String "{data: 'test'}" + + TemporalObstacleLayer::TemporalObstacleLayer() + { + } + + void TemporalObstacleLayer::onInitialize() + { + auto node = node_.lock(); + declareParameter("enabled", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + "." + "enabled", enabled_); + + declareParameter("add_obstacle_topic", rclcpp::ParameterValue("add_obstacle")); + node->get_parameter(name_ + "." + "add_obstacle_topic", add_obstacle_topic_); + + declareParameter("remove_obstacle_topic", rclcpp::ParameterValue("remove_obstacle")); + node->get_parameter(name_ + "." + "remove_obstacle_topic", remove_obstacle_topic_); + + add_obstacle_subscriber_ = node->create_subscription( + add_obstacle_topic_, rclcpp::QoS(rclcpp::QoS(1).reliable()), + std::bind(&TemporalObstacleLayer::on_new_obstacle, this, std::placeholders::_1)); + + remove_obstacle_subscriber_ = node->create_subscription( + remove_obstacle_topic_, rclcpp::QoS(rclcpp::QoS(1).reliable()), + std::bind(&TemporalObstacleLayer::on_remove_obstacle, this, std::placeholders::_1)); + + current_ = true; + need_update_ = true; + } + + void TemporalObstacleLayer::on_new_obstacle(const mep3_msgs::msg::TemporalObstacle::SharedPtr msg) + { + obstacles_.push_back(msg); + need_update_ = true; + } + + void TemporalObstacleLayer::on_remove_obstacle(const std_msgs::msg::String::SharedPtr msg) + { + std::vector new_obstacle_list; + for (auto obstacle : obstacles_) { + if (obstacle->label != msg->data) { + new_obstacle_list.push_back(obstacle); + } + } + obstacles_ = new_obstacle_list; + } + + void + TemporalObstacleLayer::updateBounds( + double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * /*min_x*/, + double * /*min_y*/, double * /*max_x*/, double * /*max_y*/) + { + } + + void + TemporalObstacleLayer::onFootprintChanged() + { + } + + void + TemporalObstacleLayer::updateCosts( + nav2_costmap_2d::Costmap2D &master_grid, int /* min_i */, int /* min_j */, + int /*max_i*/, + int /*max_j*/) + { + if (!enabled_) + { + return; + } + + for (auto obstacle : obstacles_) + { + master_grid.setConvexPolygonCost(obstacle->polygon, LETHAL_OBSTACLE); + } + } + +} // namespace mep3_navigation + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(mep3_navigation::TemporalObstacleLayer, nav2_costmap_2d::Layer) diff --git a/mep3_navigation/temporal_obstacle_layer.xml b/mep3_navigation/temporal_obstacle_layer.xml new file mode 100644 index 000000000..b2ef16733 --- /dev/null +++ b/mep3_navigation/temporal_obstacle_layer.xml @@ -0,0 +1,5 @@ + + + Allows obstacles to be dynamically added or removed. + +