From 3b2ed6823ddc10247df12cc9b59c6eb4c3b746bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marija=20Golubovi=C4=87?= <93673097+MarijaGolubovic@users.noreply.github.com> Date: Tue, 11 Jun 2024 22:54:10 +0200 Subject: [PATCH] Port behavior tree sub node to mep3 (#300) * Implement node for following path action * change strategy in simulation * add support for follow path in groot * fix simulation * fix simulation strategy * fix simulation strategy * calibrate robot and reduce speed for curve speed * calibrate robot and implement reversing move * implement c++ suport for subscriber node * implement node for camera subscription * fix simulation and simulation strategy * refactor plant detection * refactor node name * add additional joints in simulation * delete comment --- .../mep3_behavior/bt_topic_sub_node.hpp | 318 ++++++++++++++++++ .../mep3_behavior/camera_detection.hpp | 51 +++ .../mep3_behavior/navigate_to_action.hpp | 6 - .../include/mep3_behavior/ros_node_params.hpp | 45 +++ mep3_behavior/src/mep3_behavior_tree.cpp | 6 + mep3_behavior/strategies/project.btproj | 3 + mep3_behavior/strategies/try_translate.xml | 45 ++- mep3_bringup/launch/simulation_launch.py | 2 +- mep3_simulation/resource/big_description.urdf | 35 ++ .../webots_data/controllers/judge/judge.py | 19 +- .../webots_data/worlds/.eurobot.wbproj | 4 +- .../webots_data/worlds/eurobot.wbt | 2 +- 12 files changed, 499 insertions(+), 37 deletions(-) create mode 100644 mep3_behavior/include/mep3_behavior/bt_topic_sub_node.hpp create mode 100644 mep3_behavior/include/mep3_behavior/camera_detection.hpp create mode 100644 mep3_behavior/include/mep3_behavior/ros_node_params.hpp diff --git a/mep3_behavior/include/mep3_behavior/bt_topic_sub_node.hpp b/mep3_behavior/include/mep3_behavior/bt_topic_sub_node.hpp new file mode 100644 index 000000000..86f61999a --- /dev/null +++ b/mep3_behavior/include/mep3_behavior/bt_topic_sub_node.hpp @@ -0,0 +1,318 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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. + +#pragma once + +#include +#include +#include +#include +#include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "mep3_behavior/ros_node_params.hpp" +#include + +namespace BT +{ + + /** + * @brief Abstract class to wrap a Topic subscriber. + * Considering the example in the tutorial: + * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html + * + * The corresponding wrapper would be: + * + * class SubscriberNode: RosTopicSubNode + * + * The name of the topic will be determined as follows: + * + * 1. If a value is passes in the InputPort "topic_name", use that + * 2. Otherwise, use the value in RosNodeParams::default_port_value + */ + template + class RosTopicSubNode : public BT::ConditionNode + { + public: + // Type definitions + using Subscriber = typename rclcpp::Subscription; + + protected: + struct SubscriberInstance + { + SubscriberInstance(std::shared_ptr node, const std::string &topic_name); + + std::shared_ptr subscriber; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::executors::SingleThreadedExecutor callback_group_executor; + boost::signals2::signal)> broadcaster; + std::shared_ptr last_msg; + }; + + static std::mutex ®istryMutex() + { + static std::mutex sub_mutex; + return sub_mutex; + } + + using SubscribersRegistry = + std::unordered_map>; + + // contains the fully-qualified name of the node and the name of the topic + static SubscribersRegistry &getRegistry() + { + static SubscribersRegistry subscribers_registry; + return subscribers_registry; + } + + std::shared_ptr node_; + std::shared_ptr sub_instance_; + std::shared_ptr last_msg_; + std::string topic_name_; + boost::signals2::connection signal_connection_; + std::string subscriber_key_; + + rclcpp::Logger logger() + { + return node_->get_logger(); + } + + public: + /** You are not supposed to instantiate this class directly, the factory will do it. + * To register this class into the factory, use: + * + * RegisterRosAction(factory, params) + * + * Note that if the external_action_client is not set, the constructor will build its own. + * */ + explicit RosTopicSubNode(const std::string &instance_name, const BT::NodeConfig &conf, + const RosNodeParams ¶ms); + + virtual ~RosTopicSubNode() + { + signal_connection_.disconnect(); + } + + /** + * @brief Any subclass of RosTopicNode that accepts parameters must provide a + * providedPorts method and call providedBasicPorts in it. + * @param addition Additional ports to add to BT port list + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedBasicPorts(PortsList addition) + { + PortsList basic = {InputPort("topic_name", "__default__placeholder__", + "Topic name")}; + basic.insert(addition.begin(), addition.end()); + return basic; + } + + /** + * @brief Creates list of BT ports + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedPorts() + { + return providedBasicPorts({}); + } + + NodeStatus tick() override final; + + /** Callback invoked in the tick. You must return either SUCCESS of FAILURE + * + * @param last_msg the latest message received, since the last tick. + * Will be empty if no new message received. + * + * @return the new status of the Node, based on last_msg + */ + virtual NodeStatus onTick(const std::shared_ptr &last_msg) = 0; + + /** latch the message that has been processed. If returns false and no new message is + * received, before next call there will be no message to process. If returns true, + * the next call will process the same message again, if no new message received. + * + * This can be equated with latched vs non-latched topics in ros 1. + * + * @return false will clear the message after ticking/processing. + */ + virtual bool latchLastMessage() const + { + return false; + } + + private: + bool createSubscriber(const std::string &topic_name); + }; + + //---------------------------------------------------------------- + //---------------------- DEFINITIONS ----------------------------- + //---------------------------------------------------------------- + template + inline RosTopicSubNode::SubscriberInstance::SubscriberInstance( + std::shared_ptr node, const std::string &topic_name) + { + // create a callback group for this particular instance + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor.add_callback_group(callback_group, + node->get_node_base_interface()); + + rclcpp::SubscriptionOptions option; + option.callback_group = callback_group; + + // The callback will broadcast to all the instances of RosTopicSubNode + auto callback = [this](const std::shared_ptr msg) + { + last_msg = msg; + broadcaster(msg); + }; + subscriber = node->create_subscription(topic_name, 1, callback, option); + } + + template + inline RosTopicSubNode::RosTopicSubNode(const std::string &instance_name, + const NodeConfig &conf, + const RosNodeParams ¶ms) + : BT::ConditionNode(instance_name, conf), node_(params.nh) + { + // check port remapping + auto portIt = config().input_ports.find("topic_name"); + if (portIt != config().input_ports.end()) + { + const std::string &bb_topic_name = portIt->second; + + if (bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") + { + if (params.default_port_value.empty()) + { + throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams " + "are empty."); + } + else + { + createSubscriber(params.default_port_value); + } + } + else if (!isBlackboardPointer(bb_topic_name)) + { + // If the content of the port "topic_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createSubscriber(bb_topic_name); + } + else + { + // do nothing + // createSubscriber will be invoked in the first tick(). + } + } + else + { + if (params.default_port_value.empty()) + { + throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams " + "are empty."); + } + else + { + createSubscriber(params.default_port_value); + } + } + } + + template + inline bool RosTopicSubNode::createSubscriber(const std::string &topic_name) + { + if (topic_name.empty()) + { + throw RuntimeError("topic_name is empty"); + } + if (sub_instance_) + { + throw RuntimeError("Can't call createSubscriber more than once"); + } + + // find SubscriberInstance in the registry + std::unique_lock lk(registryMutex()); + subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; + + auto ®istry = getRegistry(); + auto it = registry.find(subscriber_key_); + if (it == registry.end() || it->second.expired()) + { + sub_instance_ = std::make_shared(node_, topic_name); + registry.insert({subscriber_key_, sub_instance_}); + + RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), + topic_name.c_str()); + } + else + { + sub_instance_ = it->second.lock(); + } + + // Check if there was a message received before the creation of this subscriber action + if (sub_instance_->last_msg) + { + last_msg_ = sub_instance_->last_msg; + } + + // add "this" as received of the broadcaster + signal_connection_ = sub_instance_->broadcaster.connect( + [this](const std::shared_ptr msg) + { last_msg_ = msg; }); + + topic_name_ = topic_name; + return true; + } + + template + inline NodeStatus RosTopicSubNode::tick() + { + // First, check if the subscriber_ is valid and that the name of the + // topic_name in the port didn't change. + // otherwise, create a new subscriber + std::string topic_name; + getInput("topic_name", topic_name); + + if (!topic_name.empty() && topic_name != "__default__placeholder__" && + topic_name != topic_name_) + { + sub_instance_.reset(); + } + + if (!sub_instance_) + { + createSubscriber(topic_name); + } + + auto CheckStatus = [](NodeStatus status) + { + if (!isStatusCompleted(status)) + { + throw std::logic_error("RosTopicSubNode: the callback must return" + "either SUCCESS or FAILURE"); + } + return status; + }; + sub_instance_->callback_group_executor.spin_some(); + auto status = CheckStatus(onTick(last_msg_)); + if (!latchLastMessage()) + { + last_msg_.reset(); + } + return status; + } + +} // namespace BT diff --git a/mep3_behavior/include/mep3_behavior/camera_detection.hpp b/mep3_behavior/include/mep3_behavior/camera_detection.hpp new file mode 100644 index 000000000..8466c066f --- /dev/null +++ b/mep3_behavior/include/mep3_behavior/camera_detection.hpp @@ -0,0 +1,51 @@ +#include "mep3_behavior/bt_topic_sub_node.hpp" +#include "std_msgs/msg/int32_multi_array.hpp" +#include +#include + +using namespace BT; +namespace mep3_behavior +{ + class CameraDetection : public RosTopicSubNode + { + public: + CameraDetection(const std::string &name, + const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosTopicSubNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + + return {BT::InputPort("plant_position")}; + } + + NodeStatus onTick(const std::shared_ptr &last_msg) override + { + if (last_msg == nullptr) + { + return NodeStatus::FAILURE; + } + + int plant_position; + bool is_plant_detected; + getInput("plant_position", plant_position); + + std::vector detection_results = last_msg->data; + + if (detection_results.at(plant_position - 1) == 0) + is_plant_detected = false; + else + is_plant_detected = true; + + std::cout << "Detection result " << detection_results.at(plant_position - 1) << " at position: " << plant_position << std::endl; + + if (is_plant_detected == true) + return NodeStatus::SUCCESS; + + return NodeStatus::FAILURE; + } + }; +} diff --git a/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp b/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp index d04ddb941..aaf847c0a 100644 --- a/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp +++ b/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp @@ -136,12 +136,6 @@ namespace mep3_behavior bool setGoal(Goal &goal) override { - // std::cout << "Foll to x=" << target_pose_.x - // << " y=" << target_pose_.y - // << " θ=" << target_pose_.theta << "°" << std::endl; - - // // Position - std::istringstream iss(target_pose_); std::string entry; std::vector poses; diff --git a/mep3_behavior/include/mep3_behavior/ros_node_params.hpp b/mep3_behavior/include/mep3_behavior/ros_node_params.hpp new file mode 100644 index 000000000..1c72bce63 --- /dev/null +++ b/mep3_behavior/include/mep3_behavior/ros_node_params.hpp @@ -0,0 +1,45 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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. + +#pragma once + +#include +#include +#include +#include + +namespace BT +{ + + struct RosNodeParams + { + std::shared_ptr nh; + + // This has different meaning based on the context: + // + // - RosActionNode: name of the action server + // - RosServiceNode: name of the service + // - RosTopicPubNode: name of the topic to publish to + // - RosTopicSubNode: name of the topic to subscribe to + std::string default_port_value; + + // parameters used only by service client and action clients + + // timeout when sending a request + std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000); + // timeout used when detecting the server the first time + std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); + }; + +} // namespace BT diff --git a/mep3_behavior/src/mep3_behavior_tree.cpp b/mep3_behavior/src/mep3_behavior_tree.cpp index 103c9acf3..c44c3d831 100644 --- a/mep3_behavior/src/mep3_behavior_tree.cpp +++ b/mep3_behavior/src/mep3_behavior_tree.cpp @@ -43,6 +43,7 @@ #include "mep3_behavior/set_shared_blackboard_action.hpp" #include "mep3_behavior/add_obstacle_action.hpp" #include "mep3_behavior/remove_obstacle_action.hpp" +#include "mep3_behavior/camera_detection.hpp" #include "rclcpp/rclcpp.hpp" using KeyValueT = diagnostic_msgs::msg::KeyValue; @@ -141,6 +142,11 @@ int main(int argc, char **argv) BT::RegisterRosAction(factory, "Rotate", {node, "move/move", std::chrono::seconds(30)}); BT::RegisterRosAction(factory, "Move", {node, "move/move", std::chrono::seconds(30)}); + BT::RosNodeParams params; + params.nh = node; + params.default_port_value = "/camera_topic"; + factory.registerNodeType("PlantDetected", params); + factory.registerNodeType( "ScoreboardTask"); factory.registerNodeType( diff --git a/mep3_behavior/strategies/project.btproj b/mep3_behavior/strategies/project.btproj index d82a16f39..9ac05bc88 100644 --- a/mep3_behavior/strategies/project.btproj +++ b/mep3_behavior/strategies/project.btproj @@ -57,6 +57,9 @@ + + + diff --git a/mep3_behavior/strategies/try_translate.xml b/mep3_behavior/strategies/try_translate.xml index ea111a0a8..4ff767715 100644 --- a/mep3_behavior/strategies/try_translate.xml +++ b/mep3_behavior/strategies/try_translate.xml @@ -15,7 +15,7 @@ linear_acceleration="1.5" angular_acceleration="1.8" reversing="0"/> - - - - - - - - + + + + + + + + + + + + + + + + @@ -370,6 +365,10 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_simulation/webots_data/controllers/judge/judge.py b/mep3_simulation/webots_data/controllers/judge/judge.py index 9b05ba595..ba9dfd4f4 100644 --- a/mep3_simulation/webots_data/controllers/judge/judge.py +++ b/mep3_simulation/webots_data/controllers/judge/judge.py @@ -6,10 +6,21 @@ INITIAL_POSE_MATRIX = [ - ('big', 'blue', [0.815, -1.33, pi/2]), - ('small', 'blue', [-0.815, -1.33, pi/2]), - ('big', 'green', [-0.72, 1.16, -pi/2]), - ('small', 'green', [0.72, 1.16, -pi/2]), + # ('big', 'blue', [0.03, -1.15, pi/2]), # centralno polje + ('big', 'blue', [0.815, -1.33, -pi/2]), # polje kod panela + ('big', 'yellow', [0.80, 1.33, -pi/2]), # polje kod panela + + ('big', 'blue_a', [0.045, 1.31, -pi/2]), # centralno polje + ('big', 'yellow_a', [0.045, -1.31, pi/2]), # centralno polje + + ('big', 'blue_b', [0.81, -1.34, -pi/2]), # unazad kod panela + ('big', 'yellow_b', [0.045, -1.31, pi/2]), # unazad kod panela + + # ('small', 'blue', [-0.72, -1.16, pi/2]), + + # ('small', 'green', [-0.72, 1.16, pi/2]), + # ('big', 'green', [-0.72, -1.16, pi/2]) + ] diff --git a/mep3_simulation/webots_data/worlds/.eurobot.wbproj b/mep3_simulation/webots_data/worlds/.eurobot.wbproj index b44e8fcc7..0fe487f05 100644 --- a/mep3_simulation/webots_data/worlds/.eurobot.wbproj +++ b/mep3_simulation/webots_data/worlds/.eurobot.wbproj @@ -1,6 +1,6 @@ Webots Project File version R2023b -perspectives: 000000ff00000000fd00000002000000010000011c00000307fc0200000001fb0000001400540065007800740045006400690074006f00720000000013000003070000003f00ffffff0000000300000780000000b6fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000003b600000001000000020000000100000008fc00000000 -simulationViewPerspectives: 000000ff000000010000000200000101000002c40100000002010000000100 +perspectives: 000000ff00000000fd00000002000000010000011c00000307fc0200000001fb0000001400540065007800740045006400690074006f00720000000013000003070000003f00ffffff00000003000003be000000a7fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000003be0000006900ffffff000003be000003c300000001000000020000000100000008fc00000000 +simulationViewPerspectives: 000000ff000000010000000200000195000005e90100000002010000000100 sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000fa0100000002010000000200 maximizedDockId: -1 centralWidgetVisible: 1 diff --git a/mep3_simulation/webots_data/worlds/eurobot.wbt b/mep3_simulation/webots_data/worlds/eurobot.wbt index 0c45d3067..47e9b8527 100644 --- a/mep3_simulation/webots_data/worlds/eurobot.wbt +++ b/mep3_simulation/webots_data/worlds/eurobot.wbt @@ -72,7 +72,7 @@ DEF ROBOT_SMALL GenericRobot { DEF ROBOT_BIG GenericRobot { name "robot_big" translation 0.815 -1.33 0 - rotation 0 0 1 1.5708 + rotation 0 0 1 -1.5708 arucoNumber 7 baseSlot [ Solid {