diff --git a/.vscode/settings.json b/.vscode/settings.json index 401dd611113a87620488af152e225b613cb80334..1de818d0562b9e77025cae6af75d7bd8ffe63c48 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -74,6 +74,7 @@ "cinttypes": "cpp", "typeindex": "cpp", "typeinfo": "cpp", - "variant": "cpp" + "variant": "cpp", + "future": "cpp" } } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index f2afac981d4baa36d68f7c9ab7ae2ba178d26869..3360be4aa23d40747c6ee4350b3c91f1a03e91ca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,11 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(multi_cell_builder) # C++ 11 add_compile_options(-std=c++17) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + # Load catkin and all dependencies required for this package find_package(catkin REQUIRED COMPONENTS moveit_task_constructor_core @@ -24,9 +27,11 @@ find_package(catkin REQUIRED COMPONENTS tf_conversions trajectory_msgs pcl_ros + actionlib_msgs gb_grasp moveit_grasps + behaviortree_cpp_v3 ) find_package(yaml-cpp REQUIRED) @@ -83,7 +88,11 @@ src/reader/wing_reader.cpp src/reader/cuboid_reader.cpp src/reader/job_reader.cpp - +src/bt/trajetory.cpp +src/bt/planning_scene.cpp +src/bt/execution.cpp +src/bt/position_condition.cpp +src/bt/parallel_robot.cpp ) @@ -106,6 +115,13 @@ src/reader/wing_reader.cpp src/reader/cuboid_reader.cpp src/reader/job_reader.cpp +src/bt/trajetory.cpp +src/bt/planning_scene.cpp +src/bt/execution.cpp +src/bt/position_condition.cpp +src/bt/parallel_robot.cpp + + ) add_executable(grasp_cell_routine src/grasp_cell_routine.cpp @@ -127,6 +143,14 @@ src/reader/wing_reader.cpp src/reader/cuboid_reader.cpp src/reader/job_reader.cpp + +src/bt/trajetory.cpp +src/bt/planning_scene.cpp +src/bt/execution.cpp +src/bt/position_condition.cpp +src/bt/parallel_robot.cpp + + ) @@ -150,6 +174,7 @@ target_link_libraries(base_routine ${OCTOMAP_LIBRARIES} ${PCL_LIBRARY_DIRS} ${moveit_grasps_LIBRARIES} + ${BEHAVIOR_TREE_LIBRARY} yaml-cpp ) target_link_libraries(cell_routine @@ -157,7 +182,9 @@ target_link_libraries(cell_routine ${OCTOMAP_LIBRARIES} ${PCL_LIBRARY_DIRS} ${moveit_grasps_LIBRARIES} + ${BEHAVIOR_TREE_LIBRARY} yaml-cpp + ) target_link_libraries(grasp_cell_routine @@ -165,6 +192,7 @@ target_link_libraries(grasp_cell_routine ${OCTOMAP_LIBRARIES} ${PCL_LIBRARY_DIRS} ${moveit_grasps_LIBRARIES} + ${BEHAVIOR_TREE_LIBRARY} yaml-cpp ) diff --git a/include/bt/execution.h b/include/bt/execution.h new file mode 100644 index 0000000000000000000000000000000000000000..ef31a31c007360c5c084332192d3d2600ebe550e --- /dev/null +++ b/include/bt/execution.h @@ -0,0 +1,43 @@ +#ifndef EXECUTION_ +#define EXECUTION_ + +#include "ros/ros.h" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/tree_node.h" +#include "behaviortree_cpp_v3/basic_types.h" +#include "behaviortree_cpp_v3/leaf_node.h" +#include "impl/abstract_mediator.h" +#include "impl/moveit_mediator.h" + +#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h> + + +#include <moveit/move_group_interface/move_group_interface.h> +#include <moveit_msgs/ApplyPlanningScene.h> +#include <moveit/robot_model_loader/robot_model_loader.h> +#include <moveit/planning_scene/planning_scene.h> +#include <moveit/kinematic_constraints/utils.h> + +class Execution : public BT::StatefulActionNode{ + private: + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_; + Moveit_robot* mr_reference_; + ros::Publisher* publisher_reference_; + Moveit_mediator* mediator_reference_; + std::vector<moveit_task_constructor_msgs::SubTrajectory>::iterator it_; + + + public: + Execution(const std::string& name, const BT::NodeConfiguration& config); + void init(Moveit_mediator* mediator_reference, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets); + + inline static BT::PortsList providedPorts() { return {}; } + + BT::NodeStatus onStart() override; + BT::NodeStatus onRunning() override; + void onHalted() override; +}; +#endif + + + diff --git a/include/bt/parallel_robot.h b/include/bt/parallel_robot.h new file mode 100644 index 0000000000000000000000000000000000000000..21a51bfab17f73a141495be65d6e333a1d0b6f3d --- /dev/null +++ b/include/bt/parallel_robot.h @@ -0,0 +1,46 @@ +#ifndef PARALLEL_ROBOT_ +#define PARALLEL_ROBOT_ + +#pragma once + +#include <set> +#include "behaviortree_cpp_v3/control_node.h" + +class Parallel_robot : public BT::ControlNode{ +public: + Parallel_robot(const std::string& name, int success_threshold, int failure_threshold = 1); + Parallel_robot(const std::string& name, const BT::NodeConfiguration& config); + + static BT::PortsList providedPorts(){ + return {BT::InputPort<int>(THRESHOLD_SUCCESS, "number of childen which need to succeed " + "to " + "trigger a SUCCESS"), + BT::InputPort<int>(THRESHOLD_FAILURE, 1, + "number of childen which need to fail to trigger a FAILURE")}; + } + + ~Parallel_robot() override = default; + + virtual void halt() override; + + size_t successThreshold() const; + size_t failureThreshold() const; + void setSuccessThreshold(int threshold_M); + void setFailureThreshold(int threshold_M); + +private: + int success_threshold_; + int failure_threshold_; + + std::set<int> skip_list_; + + bool read_parameter_from_ports_; + static constexpr const char* THRESHOLD_SUCCESS = "success_threshold"; + static constexpr const char* THRESHOLD_FAILURE = "failure_threshold"; + + virtual BT::NodeStatus tick() override; +}; + +#endif + + diff --git a/include/bt/planning_scene.h b/include/bt/planning_scene.h new file mode 100644 index 0000000000000000000000000000000000000000..25cf44d7f1bdb7132733f6a25dbc76ba4514eacb --- /dev/null +++ b/include/bt/planning_scene.h @@ -0,0 +1,30 @@ +#ifndef PLANNING_SCENE_ +#define PLANNING_SCENE_ + +#include "ros/ros.h" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/tree_node.h" +#include "behaviortree_cpp_v3/basic_types.h" + +#include <moveit/move_group_interface/move_group_interface.h> +#include <moveit_msgs/ApplyPlanningScene.h> +#include <moveit/robot_model_loader/robot_model_loader.h> +#include <moveit/planning_scene/planning_scene.h> +#include <moveit/kinematic_constraints/utils.h> + +class Planning_scene : public BT::AsyncActionNode{ + private: + ros::Publisher* publisher_reference_; + moveit_msgs::PlanningScene planning_scene_; + + public: + Planning_scene(const std::string& name, const BT::NodeConfiguration& config); + void init(ros::Publisher* publisher_reference, moveit_msgs::PlanningScene& planning_scene); + + + // You must override the virtual function tick() + // this example doesn't require any port + inline static BT::PortsList providedPorts() { return {}; } + BT::NodeStatus tick() override; +}; +#endif \ No newline at end of file diff --git a/include/bt/position_condition.h b/include/bt/position_condition.h new file mode 100644 index 0000000000000000000000000000000000000000..ef66a618b399d2412415a12a7866d51ccf325f59 --- /dev/null +++ b/include/bt/position_condition.h @@ -0,0 +1,36 @@ +#ifndef POSITION_CONDITION_ +#define POSITION_CONDITION_ + +#include "ros/ros.h" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/tree_node.h" +#include "behaviortree_cpp_v3/basic_types.h" +#include "behaviortree_cpp_v3/leaf_node.h" + +#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h> + +#include <moveit/planning_scene_interface/planning_scene_interface.h> +#include <moveit/move_group_interface/move_group_interface.h> +#include <moveit_msgs/ApplyPlanningScene.h> +#include <moveit/robot_model_loader/robot_model_loader.h> +#include <moveit/planning_scene/planning_scene.h> +#include <moveit/kinematic_constraints/utils.h> + + +class Position_condition : public BT::ConditionNode { +public: +Position_condition(const std::string& name, const BT::NodeConfiguration& config); + +void init(const std::string& obj_name,tf2::Vector3& start_pos, moveit::planning_interface::PlanningSceneInterface* psi); + +inline static BT::PortsList providedPorts() { return {}; } + +protected: + +virtual BT::NodeStatus tick() override; +std::string obj_name_; +moveit::planning_interface::PlanningSceneInterface* psi_; +tf2::Vector3 start_pos_; +}; + +#endif \ No newline at end of file diff --git a/include/bt/trajetory.h b/include/bt/trajetory.h new file mode 100644 index 0000000000000000000000000000000000000000..54d4f99229aaf0b61f0d8eb9dd8bc594c02a2951 --- /dev/null +++ b/include/bt/trajetory.h @@ -0,0 +1,32 @@ +#ifndef TRAJETORY_ +#define TRAJETORY_ + +#include "ros/ros.h" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/tree_node.h" +#include "behaviortree_cpp_v3/basic_types.h" + + + +#include <moveit/move_group_interface/move_group_interface.h> +#include <moveit_msgs/ApplyPlanningScene.h> +#include <moveit/robot_model_loader/robot_model_loader.h> +#include <moveit/planning_scene/planning_scene.h> +#include <moveit/kinematic_constraints/utils.h> + +class Trajetory : public BT::AsyncActionNode{ + private: + moveit::planning_interface::MoveGroupInterface* mgi_reference_; + moveit_msgs::RobotTrajectory trajetory_; + + public: + Trajetory(const std::string& name, const BT::NodeConfiguration& config); + void init (moveit::planning_interface::MoveGroupInterface* mgi_reference, moveit_msgs::RobotTrajectory& trajetory); + + + + // You must override the virtual function tick() + inline static BT::PortsList providedPorts() { return {}; } + BT::NodeStatus tick() override; +}; +#endif \ No newline at end of file diff --git a/include/impl/moveit_mediator.h b/include/impl/moveit_mediator.h index d9866f62d8e50a6e97e044a9076767e873c782a6..b1d24493baf421f9f14dfcc8abd45c377ebfcfc3 100644 --- a/include/impl/moveit_mediator.h +++ b/include/impl/moveit_mediator.h @@ -47,6 +47,8 @@ #include <moveit/moveit_cpp/moveit_cpp.h> #include "reader/job_reader.h" #include "reader/cuboid_reader.h" +#include <thread> + #include <stdint.h> @@ -78,6 +80,7 @@ class Moveit_mediator : public Abstract_mediator{ + std::vector<std::thread*> executions_; public: @@ -85,13 +88,14 @@ class Moveit_mediator : public Abstract_mediator{ void setup_task(); inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;}; - + inline std::vector<std::thread*>& executions(){return executions_;}; bool check_collision(const int& robot) override; void mediate() override; void build_wings(std::bitset<3>& wing,int& robot) override; void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override; void connect_robots(Abstract_robot* robot) override; + void status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status); void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr); @@ -100,7 +104,10 @@ class Moveit_mediator : public Abstract_mediator{ void task_planner(); void publish_tables(); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); - void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps); + + // void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt); + void parallel_exec(); + moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;}; inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;}; diff --git a/launch/cell_routine.launch b/launch/cell_routine.launch index 373d6c3c27d1e7d073dee7ae2295b564ad27ec17..64ed8c3725cbaf8bd23dbaff4e4267a70feeb9d1 100644 --- a/launch/cell_routine.launch +++ b/launch/cell_routine.launch @@ -27,6 +27,9 @@ <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" /> </include> + <node pkg="groot" type="Groot" name="Groot" output="screen" required="true"> + </node> + <node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true"> </node> </launch> diff --git a/package.xml b/package.xml index 80f22b71a8119d9f7451eae5ac034c728e030c3a..4e3f59cb597edeedb4e5ad6e9c48138441c6969c 100644 --- a/package.xml +++ b/package.xml @@ -32,6 +32,8 @@ <build_depend>moveit_task_constructor_visualization</build_depend> <build_depend>pcl_ros</build_depend> <build_depend>moveit_task_constructor_core</build_depend> + <build_depend>behaviortree_cpp_v3</build_depend> + <build_depend>moveit_task_constructor_capabilities</build_depend> @@ -42,7 +44,7 @@ <run_depend>moveit_grasps</run_depend> <run_depend>moveit_grasps_filter</run_depend> - + <run_depend>behaviortree_cpp_v3</run_depend> <run_depend>moveit_ros_planning_interface</run_depend> <run_depend>filesystem</run_depend> <run_depend>pcl_ros</run_depend> diff --git a/src/bt/execution.cpp b/src/bt/execution.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2b64e7fcb98f6120a6b849205d7ee5b4249ac53e --- /dev/null +++ b/src/bt/execution.cpp @@ -0,0 +1,77 @@ +#include "bt/execution.h" + +Execution::Execution(const std::string& name, const BT::NodeConfiguration& config) + : BT::StatefulActionNode(name, config) {} + + +/* +BT::NodeStatus Execution::tick() { + ROS_INFO("davor"); + if (it_ != ets_.solution.sub_trajectory.end()){ + mgi_reference_->execute(it_->trajectory); + publisher_reference_->publish(it_->scene_diff); + it_++; + ROS_INFO("true"); + return BT::NodeStatus::RUNNING; + } else { + ROS_INFO("false"); + return BT::NodeStatus::SUCCESS; + } +}; +*/ + +BT::NodeStatus Execution::onStart(){ + if (it_ != ets_.solution.sub_trajectory.end()){ + //mr_reference_->execute(it_->trajectory); + ROS_INFO("start"); + // std::thread t(&Moveit_mediator::parallel_exec, mediator_reference_); + // if(t.joinable())t.join(); + // mediator_reference_->parallel_exec(); + // mediator_reference_->executions().push_back(std::thread(&Moveit_mediator::parallel_exec, mediator_reference_, std::ref(*mr_reference_), it_->trajectory)); + mediator_reference_->executions().push_back(new std::thread(&Moveit_mediator::parallel_exec, mediator_reference_)); + for (int i = 0; i < mediator_reference_->executions().size();i++){ + if(mediator_reference_->executions()[i]->joinable()) mediator_reference_->executions()[i]->join(); + } + + //publisher_reference_->publish(it_->scene_diff); + ROS_INFO("%i", mediator_reference_->executions().size()); + ROS_INFO("after join in start"); + + it_++; + return BT::NodeStatus::RUNNING; + } else { + return BT::NodeStatus::SUCCESS; + } + } + + // If onStart() returned RUNNING, we will keep calling + // this method until it return something different from RUNNING +BT::NodeStatus Execution::onRunning(){ + if (it_ != ets_.solution.sub_trajectory.end()){ + //mgi_reference_->execute(it_->trajectory); + //mediator_reference_->parallel_exec(); + ROS_INFO("running"); + //std::thread t(&Moveit_mediator::parallel_exec, mediator_reference_); + // if(t.joinable())t.join(); + mediator_reference_->executions().push_back(new std::thread(&Moveit_mediator::parallel_exec, mediator_reference_)); + ROS_INFO("after join in running"); + //mediator_reference_->executions().push_back(std::thread(&Moveit_mediator::parallel_exec, mediator_reference_)); + + //publisher_reference_->publish(it_->scene_diff); + it_++; + return BT::NodeStatus::RUNNING; + } else { + return BT::NodeStatus::SUCCESS; + } +} + + // callback to execute if the action was aborted by another node +void Execution::onHalted(){} + +void Execution::init(Moveit_mediator* mediator_reference ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) { + mr_reference_= mr_reference; + publisher_reference_ = publisher_reference; + ets_ = ets; + it_ = ets_.solution.sub_trajectory.begin(); + mediator_reference_ = mediator_reference_; +} \ No newline at end of file diff --git a/src/bt/parallel_robot.cpp b/src/bt/parallel_robot.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7e43274b63f68f322989686afc230aaca93bca1d --- /dev/null +++ b/src/bt/parallel_robot.cpp @@ -0,0 +1,145 @@ +#include "bt/parallel_robot.h" + +/* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved + * Copyright (C) 2018-2020 Davide Faconti, Eurecat - All Rights Reserved +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), +* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, +* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#include <algorithm> +#include <cstddef> + +#include "behaviortree_cpp_v3/controls/parallel_node.h" + +constexpr const char* BT::ParallelNode::THRESHOLD_FAILURE; +constexpr const char* BT::ParallelNode::THRESHOLD_SUCCESS; + +Parallel_robot::Parallel_robot(const std::string& name, int success_threshold, int failure_threshold) : + BT::ControlNode(name, {}), + success_threshold_(success_threshold), + failure_threshold_(failure_threshold), + read_parameter_from_ports_(false){ + setRegistrationID("Parallel_robot"); +} + +Parallel_robot::Parallel_robot(const std::string& name, const BT::NodeConfiguration& config) : + BT::ControlNode(name, config), + success_threshold_(1), + failure_threshold_(1), + read_parameter_from_ports_(true) +{} + +BT::NodeStatus Parallel_robot::tick(){ + if (read_parameter_from_ports_){ + if (!getInput(THRESHOLD_SUCCESS, success_threshold_)){ + throw BT::RuntimeError("Missing parameter [", THRESHOLD_SUCCESS, "] in ParallelNode"); + } + + if (!getInput(THRESHOLD_FAILURE, failure_threshold_)){ + throw BT::RuntimeError("Missing parameter [", THRESHOLD_FAILURE, "] in ParallelNode"); + } + } + + size_t success_childred_num = 0; + size_t failure_childred_num = 0; + + const size_t children_count = children_nodes_.size(); + + if (children_count < successThreshold()){ + throw BT::LogicError("Number of children is less than threshold. Can never succeed."); + } + + if (children_count < failureThreshold()){ + throw BT::LogicError("Number of children is less than threshold. Can never fail."); + } + + // Routing the tree according to the sequence node's logic: + for (unsigned int i = 0; i < children_count; i++){ + TreeNode* child_node = children_nodes_[i]; + + bool in_skip_list = (skip_list_.count(i) != 0); + + BT::NodeStatus child_status; + if (in_skip_list){ + child_status = child_node->status(); + } + else{ + child_status = child_node->executeTick(); + } + + switch (child_status){ + case BT::NodeStatus::SUCCESS: { + if (!in_skip_list){ + skip_list_.insert(i); + } + success_childred_num++; + + if (success_childred_num == successThreshold()){ + skip_list_.clear(); + BT::ControlNode::ControlNode:: + resetChildren(); + return BT::NodeStatus::SUCCESS; + } + } + break; + + case BT::NodeStatus::FAILURE: { + + // It fails if it is not possible to succeed anymore or if + // number of failures are equal to failure_threshold_ + if ((failure_childred_num > children_count - successThreshold()) || + (failure_childred_num == failureThreshold())){ + skip_list_.clear(); + resetChildren(); + return BT::NodeStatus::FAILURE; + } + } + break; + + case BT::NodeStatus::RUNNING: { + return BT::NodeStatus::RUNNING; + } + break; + + default: { + throw BT::LogicError("A child node must never return IDLE"); + } + } + } + + return BT::NodeStatus::RUNNING; +} + +void Parallel_robot::halt() +{ + skip_list_.clear(); + ControlNode::halt(); +} + +size_t Parallel_robot::successThreshold() const +{ + return success_threshold_ < 0 ? + std::max(children_nodes_.size() + success_threshold_ + 1, size_t(0)) : + success_threshold_; +} + +size_t Parallel_robot::failureThreshold() const{ + return failure_threshold_ < 0 ? + std::max(children_nodes_.size() + failure_threshold_ + 1, size_t(0)) : + failure_threshold_; +} + +void Parallel_robot::setSuccessThreshold(int threshold_M){ + success_threshold_ = threshold_M; +} + +void Parallel_robot::setFailureThreshold(int threshold_M){ + failure_threshold_ = threshold_M; +} diff --git a/src/bt/planning_scene.cpp b/src/bt/planning_scene.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b67f11b6e29bfdd9fcfaeabcde512cde75063d5a --- /dev/null +++ b/src/bt/planning_scene.cpp @@ -0,0 +1,19 @@ +#include "bt/planning_scene.h" + +Planning_scene::Planning_scene(const std::string& name, const BT::NodeConfiguration& config) + : BT::AsyncActionNode(name, config) {} + + +BT::NodeStatus Planning_scene::tick() { + try{ + publisher_reference_->publish(planning_scene_); + } catch (ros::Exception& re) { + return BT::NodeStatus::FAILURE; + } + return BT::NodeStatus::SUCCESS; +}; + +void Planning_scene::init(ros::Publisher* publisher_reference, moveit_msgs::PlanningScene& planning_scene) { + publisher_reference_ = publisher_reference; + planning_scene_ = planning_scene; +} \ No newline at end of file diff --git a/src/bt/position_condition.cpp b/src/bt/position_condition.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ec6467bd19aa2b98d458af90914eff893b084690 --- /dev/null +++ b/src/bt/position_condition.cpp @@ -0,0 +1,24 @@ +#include "bt/position_condition.h" + +Position_condition::Position_condition(const std::string& name, const BT::NodeConfiguration& config) + : BT::ConditionNode(name, config){} + +void Position_condition::init(const std::string& obj_name, tf2::Vector3& start_pos, moveit::planning_interface::PlanningSceneInterface* psi){ + obj_name_ = obj_name; + psi_ = psi; + start_pos_ = start_pos; +} + +BT::NodeStatus Position_condition::tick(){ + try { + auto ref_pose = psi_->getObjects().at(obj_name_); + if(tf2::tf2Distance2(start_pos_, tf2::Vector3(ref_pose.pose.position.x, ref_pose.pose.position.y, ref_pose.pose.position.z )) == 0){ + return BT::NodeStatus::SUCCESS; + } else { + return BT::NodeStatus::FAILURE; + } + } catch(std::out_of_range& oor) { + ROS_INFO("Position to check not found"); + return BT::NodeStatus::FAILURE; + } +} diff --git a/src/bt/trajetory.cpp b/src/bt/trajetory.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6462090a103ec4ecd558d279201bca3dfdfaa370 --- /dev/null +++ b/src/bt/trajetory.cpp @@ -0,0 +1,14 @@ +#include "bt/trajetory.h" + +Trajetory::Trajetory(const std::string& name, const BT::NodeConfiguration& config) + : BT::AsyncActionNode(name, config){} + +BT::NodeStatus Trajetory::tick() { + if (mgi_reference_->execute(trajetory_)) return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +}; + +void Trajetory::init(moveit::planning_interface::MoveGroupInterface* mgi_reference, moveit_msgs::RobotTrajectory& trajetory){ + mgi_reference_ = mgi_reference; + trajetory_ = trajetory; +} \ No newline at end of file diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp index 247672ef9e81d503c254aa80f91d9e754a3cb994..594f55c93e6f0f4909902c6586bb72586d8781aa 100644 --- a/src/impl/moveit_mediator.cpp +++ b/src/impl/moveit_mediator.cpp @@ -5,6 +5,20 @@ #include <gb_grasp/MapGenerator.h> #include <regex> +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/tree_node.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "bt/planning_scene.h" +#include "bt/trajetory.h" +#include "bt/execution.h" +#include "bt/position_condition.h" +#include "bt/parallel_robot.h" +#include <actionlib_msgs/GoalStatusArray.h> + + + + thread_local moveit::task_constructor::Task task__; thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_; @@ -541,6 +555,9 @@ void Moveit_mediator::setup_task(){ } */ + + + std::regex item("box_([0-9]+)"); std::smatch match; @@ -585,7 +602,7 @@ void Moveit_mediator::setup_task(){ if (rt){ Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r); // mr->mgi()->execute(*rt); - th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *rt, *ps)); + //th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *rt)); // First find ID from panda to start with std::regex rx_panda("panda_arm([0-9]+)"); @@ -737,16 +754,30 @@ void Moveit_mediator::task_planner(){ std::regex item("box_([0-9]+)"); std::smatch match; - // circular buffer is job-q + // ---------------------------------- + // GROOT Strategie => erst FROM TEXT + // ---------------------------------- + + BT::BehaviorTreeFactory factory; + factory.registerNodeType<Planning_scene>("PS"); + factory.registerNodeType<Trajetory>("Trajetory"); + factory.registerNodeType<Execution>("Execution"); + factory.registerNodeType<Position_condition>("Position_condition"); + factory.registerNodeType<Parallel_robot>("Parallel_robot"); + + std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different; + while(!jq.empty()){ for(auto& s_co : psi_->getObjects()){ if(!std::regex_match(s_co.first, match, item)) continue; + std::pair<std::string, job_data> temp = jq.front(); ROS_INFO("1. job entry %f %f %f", temp.second.jobs_.front().getOrigin().getX(), temp.second.jobs_.front().getOrigin().getY(), temp.second.jobs_.front().getOrigin().getZ()); ROS_INFO("object position %f %f %f", s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z); if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z )) == 0) { std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot; - // get rob by name + std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> bt_list; + Moveit_robot* mr; for(auto* r: robots_) if (r->name() == temp.first) mr = dynamic_cast<Moveit_robot*>(r); @@ -757,6 +788,7 @@ void Moveit_mediator::task_planner(){ moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); tasks_per_robot.push(e); + bt_list.push_back(e); moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first); temp_co.operation = temp_co.MOVE; @@ -770,13 +802,120 @@ void Moveit_mediator::task_planner(){ psi_->applyCollisionObject(temp_co); } } - std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> pair(temp.second.jobs_.front().getOrigin(), tasks_per_robot); - tasks_.insert(std::pair<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>(mr->name(), pair)); + + /* + if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("om");condition->init(s_co.first, temp.second.jobs_.front().getOrigin(), psi_.get()); ++node_it;} + + for(auto& ets : bt_list) + if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("tr");execution->init(planning_scene_diff_publisher_.get(), mr->mgi().get(), ets); ++node_it;} + */ + + for (int i = 0; i < cd.size(); i ++){ + std::stringstream ss; + ss << "box_" << i; + + moveit_msgs::CollisionObject item; + item.id = ss.str(); + item.header.frame_id = "world"; + item.header.stamp = ros::Time::now(); + item.primitives.resize(1); + item.primitives[0].type = item.primitives[0].BOX; + item.primitives[0].dimensions.resize(3); + item.primitives[0].dimensions[0] = cd[i].x_depth; + item.primitives[0].dimensions[1] = cd[i].y_width; + item.primitives[0].dimensions[2] = cd[i].z_heigth; + + item.pose.position.x = cd[i].Pose.position.x; + item.pose.position.y = cd[i].Pose.position.y; + item.pose.position.z = cd[i].Pose.position.z; + item.pose.orientation.x = cd[i].Pose.orientation.x; + item.pose.orientation.y = cd[i].Pose.orientation.y; + item.pose.orientation.z = cd[i].Pose.orientation.z; + item.pose.orientation.w = cd[i].Pose.orientation.w; + item.operation = item.MOVE; + + psi_->applyCollisionObject(item); + // Could also safe id's somewhere + } + + + try{ + task_but_different.at(mr->name()).push_back(std::tuple<std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list)); + } catch(std::out_of_range &oor) { + std::tuple<const std::string&, tf2::Vector3&, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>&> tuple(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list); + task_but_different.insert(std::pair<std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>>(mr->name(),{tuple})); + } + + tasks_.insert(std::pair<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>(mr->name(), {temp.second.jobs_.front().getOrigin(), tasks_per_robot})); jq.pop_front(); } else {jq.push_back(temp);} } } + std::stringstream ss; + ss << "<root main_tree_to_execute = \"MainTree\">\n"; + ss << "<BehaviorTree ID=\"MainTree\">\n"; + ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\""<< robots_.size() << "\" failure_threshold=\""<< robots_.size() << "\">\n"; + + for(int i =0; i < robots_.size();i++){ + try{ + ss << "<Control ID=\"Parallel_robot\" name=\""<< robots_[i]->name() << "\" success_threshold=\""<< task_but_different.at(robots_[i]->name()).size() << "\" failure_threshold=\""<< task_but_different.at(robots_[i]->name()).size() << "\">\n"; + for (auto& p_obj: task_but_different.at(robots_[i]->name())){ + ss << "<SequenceStar name=\"root_sequence\">\n"; + ss << "<Condition ID=\"Position_condition\" name=\"Position_condition\"/>\n"; + for(int j = 0; j < std::get<2>(p_obj).size(); j++){ + ss << "<Action ID=\"Execution\" name=\"Execution\"/>\n"; + } + ss << "</SequenceStar>\n"; + } + ss << "</Control>\n"; + } catch(std::out_of_range& oor){} + } + ss << "</Control>\n"; + ss << "</BehaviorTree>\n"; + ss << "</root>\n"; + + std::cout << ss.str(); + auto tree = factory.createTreeFromText(ss.str()); + auto node_it = tree.nodes.begin(); + + for(int i =0; i < robots_.size();i++){ + try{ + for (auto& p_obj: task_but_different.at(robots_[i]->name())){ + while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it; + if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO("Action"); + if(node_it->get()->type() == BT::NodeType::CONDITION) ROS_INFO("Condition"); + if(node_it->get()->type() == BT::NodeType::CONTROL) ROS_INFO("Control"); + if(node_it->get()->type() == BT::NodeType::DECORATOR) ROS_INFO("Decorator"); + + if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("init Condition");condition->init(std::get<0>(p_obj), std::get<1>(p_obj), psi_.get());++node_it;} + for(int j = 0; j < std::get<2>(p_obj).size(); j++){ + auto mr = dynamic_cast<Moveit_robot*>(robots_[i]); + if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("init Action");execution->init(this, planning_scene_diff_publisher_.get(), mr, std::get<2>(p_obj)[j]);++node_it;} + } + ss << "</SequenceStar>\n"; + } + ss << "</Control>\n"; + } catch(std::out_of_range& oor){} + } + + + BT::PublisherZMQ zmq(tree); + ros::Duration sleep(1); + BT::NodeStatus status = BT::NodeStatus::RUNNING; + while( status == BT::NodeStatus::RUNNING){ + status = tree.tickRoot(); + + for(int i = 0; i < executions_.size(); i++){ + if(executions_[i]->joinable()) executions_[i]->join(); + } + // IMPORTANT: you must always add some sleep if you call tickRoot() + // in a loop, to avoid using 100% of your CPU (busy loop). + // The method Tree::sleep() is recommended, because it can be + // interrupted by an internal event inside the tree. + } + + // clean up for (int i = 0; i < cd.size(); i ++){ std::stringstream ss; @@ -825,6 +964,12 @@ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){ } +void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status){ + if (!status->status_list.empty()){ + ROS_INFO("%i", status->status_list.size()); + } +} + void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){ // get full mr link list std::vector<std::string> links; @@ -867,10 +1012,16 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla } } - -void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){ + +/* +void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt){ mr.mgi()->execute(rt); } +*/ +void Moveit_mediator::parallel_exec(){ + //ROS_INFO("UwU"); + // mr.mgi()->execute(rt); +} @@ -1143,6 +1294,14 @@ moveit::task_constructor::Task Moveit_mediator::create_Task(Moveit_robot* mr, mo place->insert(std::move(stage)); } + { + auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_); + stage->setGroup(mr->map()["eef_name"]); + stage->setGoal("close"); + place->insert(std::move(stage)); + } + + // Add place container to task task_.add(std::move(place)); } @@ -1270,6 +1429,7 @@ Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> object , job_reader_(std::make_unique<Job_reader>(nh_)) , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) { + executions_.resize(2); if (planning_scene_monitor_->getPlanningScene()){ planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,