From d9074b110349bbd1fda5e50f3a3ade89ab9df0b7 Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Tue, 28 Feb 2023 09:25:50 +0000 Subject: [PATCH] Tree working again, just robot simulation for now --- src/btree_nodes/CMakeLists.txt | 2 + src/btree_nodes/package.xml | 1 + src/btree_nodes/src/Tree.cpp | 65 ++++++++++--------- src/btree_nodes/src/nodes/ActorMovement.cpp | 64 ++++++++++++++++++ src/btree_nodes/src/nodes/ActorMovement.h | 30 +++++++++ src/btree_nodes/src/nodes/MoveConnection.cpp | 25 ++++--- .../config/moveit_controllers.yaml | 2 + src/moveit_test/src/test.cpp | 14 ++-- 8 files changed, 159 insertions(+), 44 deletions(-) create mode 100644 src/btree_nodes/src/nodes/ActorMovement.cpp create mode 100644 src/btree_nodes/src/nodes/ActorMovement.h diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index 46bb692..a268ef5 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/src/btree_nodes/CMakeLists.txt @@ -22,6 +22,7 @@ set(DEPENDENCIES tf2_geometry_msgs moveit_ros_planning pluginlib + ros_actor_action_server_msgs ) foreach(dep IN LISTS DEPENDENCIES) @@ -48,6 +49,7 @@ set(CPP_FILES src/nodes/InAreaTest.cpp src/nodes/InterruptableSequence.cpp src/nodes/SetRobotVelocity.cpp + src/nodes/ActorMovement.cpp ) add_library(tree_plugins_base src/Factory.cpp) diff --git a/src/btree_nodes/package.xml b/src/btree_nodes/package.xml index a703b55..5424bb2 100644 --- a/src/btree_nodes/package.xml +++ b/src/btree_nodes/package.xml @@ -10,6 +10,7 @@ ament_cmake rclcpp geometry_msgs + ros_actor_action_server_msgs ament_lint_auto ament_lint_common diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index e615bf9..83dc29a 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -13,6 +13,7 @@ #include "nodes/InAreaTest.h" #include "nodes/InterruptableSequence.h" #include "nodes/SetRobotVelocity.h" +#include "nodes/ActorMovement.h" #include #include @@ -23,7 +24,8 @@ int main(int argc, char **argv) { rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - auto mainNode = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options); + auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options); + auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(mainNode); @@ -33,46 +35,46 @@ int main(int argc, char **argv) { const std::shared_ptr safeArea = std::make_shared(); safeArea->triangles = {std::vector({ - {-1, 3.5}, - {-1, 7}, - {3, 3.5}, - {7, 7}, - {3, -1}, - {7, -1} + { 1, 3.5 }, + { 1, 7 }, + { 3, 3.5 }, + { 7, 7 }, + { 3, -1 }, + { 7, -1 } })}; const std::shared_ptr warningArea = std::make_shared(); warningArea->triangles = {std::vector({ - {-1, 2.5}, - {-1, 3.5}, - {2, 2.5}, - {3, 3.5}, - {2, -1}, - {3, -1} + { -1, 2.5 }, + { -1, 3.5 }, + { 2, 2.5 }, + { 3, 3.5 }, + { 2, -1 }, + { 3, -1 } })}; const std::shared_ptr unsafeArea = std::make_shared(); unsafeArea->triangles = std::vector({ - {-1, 1.5}, - {-1, 2.5}, - {1, 1.5}, - {2, 2.5}, - {1, -1}, - {2, -1} + { -1, 1.5 }, + { -1, 2.5 }, + { 1, 1.5 }, + { 2, 2.5 }, + { 1, -1 }, + { 2, -1 } }); const std::shared_ptr negativeYTable = std::make_shared(); negativeYTable->triangles = std::vector({ - {0.3,-0.25}, - {-0.3,-0.25}, - {0,-0.4} + { 0.3, -0.25 }, + { -0.3, -0.25 }, + { 0, -0.4 } }); const std::shared_ptr positiveYTable = std::make_shared(); positiveYTable->triangles = std::vector({ - {0.3,0.25}, - {-0.3,0.25}, - {0,0.4} + { 0.3, 0.25 }, + { -0.3, 0.25 }, + { 0, 0.4 } }); std::cout << "Registering nodes." << std::endl; @@ -80,6 +82,7 @@ int main(int argc, char **argv) { factory.registerNodeType("GenerateXYPose"); factory.registerNodeType("AmICalled"); + factory.registerNodeType("ActorMovement"); factory.registerNodeType("MoveActorToTarget"); factory.registerNodeType("WeightedRandom"); factory.registerNodeType("OffsetPose"); @@ -88,7 +91,7 @@ int main(int argc, char **argv) { factory.registerNodeType("InterruptableSequence"); - auto connection = std::make_shared(mainNode, "iisy"); + auto connection = std::make_shared(moveNode, "arm"); NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) { return std::make_unique(name, config, &connection); @@ -119,11 +122,13 @@ int main(int argc, char **argv) { std::cout << "Creating tree." << std::endl; - Tree actorTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/actorTree.xml"); - Tree robotTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/robotTree.xml"); + Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml"); + Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml"); auto trees = {actorTree.rootBlackboard(), robotTree.rootBlackboard()}; auto init = std::make_shared(); + init->position.x = 6.0; + init->position.y = 6.0; for (const auto &item : trees){ item->set("safeArea",safeArea); @@ -147,12 +152,12 @@ int main(int argc, char **argv) { std::cout << "Looping." << std::endl; - std::thread actor([&actorTree]() { + /*std::thread actor([&actorTree]() { while (rclcpp::ok()) { actorTree.tickRoot(); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } - }); + });*/ while (rclcpp::ok()) { robotTree.tickRoot(); diff --git a/src/btree_nodes/src/nodes/ActorMovement.cpp b/src/btree_nodes/src/nodes/ActorMovement.cpp new file mode 100644 index 0000000..89f459e --- /dev/null +++ b/src/btree_nodes/src/nodes/ActorMovement.cpp @@ -0,0 +1,64 @@ +// +// Created by bastian on 26.03.22. +// + +#include "ActorMovement.h" + +BT::ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config) + : StatefulActionNode(name, config) {} + +BT::PortsList BT::ActorMovement::providedPorts() { + return { + InputPort>("current"), + InputPort>("target") + }; +} + +BT::NodeStatus BT::ActorMovement::onStart() { + std::cout << "started moving" << std::endl; + + rclcpp::Node node("targetPublisherNode"); + auto publisher = node.create_publisher("targetPose", 10); + auto res = getInput>("target", target); + + if (!res) { + std::cout << "[ no target available ]" << std::endl; + std::cout << res.error() << std::endl; + return NodeStatus::FAILURE; + } + + publisher->publish(*target); + return NodeStatus::RUNNING; +} + +BT::NodeStatus BT::ActorMovement::onRunning() { + std::shared_ptr current; + + auto res = getInput>("current", current); + if (!res) { + std::cout << "[ no current position available ]" << std::endl; + std::cout << res.error() << std::endl; + return NodeStatus::FAILURE; + } + + double dX = target->position.x - current->position.x; + double dY = target->position.y - current->position.y; + + auto distance = sqrt(dX * dX + dY * dY); + + if (distance < 0.3) { + return NodeStatus::SUCCESS; + } else { + return NodeStatus::RUNNING; + } +} + +void BT::ActorMovement::onHalted() { + std::cout << "halted move" << std::endl; + rclcpp::Node node("targetPublisherNode"); + auto publisher = node.create_publisher("targetPose", 10); + geometry_msgs::msg::Pose inf; + inf.position.x = HUGE_VAL; + inf.position.y = HUGE_VAL; + publisher->publish(inf); +} diff --git a/src/btree_nodes/src/nodes/ActorMovement.h b/src/btree_nodes/src/nodes/ActorMovement.h new file mode 100644 index 0000000..bdbaa04 --- /dev/null +++ b/src/btree_nodes/src/nodes/ActorMovement.h @@ -0,0 +1,30 @@ +// +// Created by bastian on 26.03.22. +// + +#ifndef BUILD_ACTORMOVEMENT_H +#define BUILD_ACTORMOVEMENT_H + +#include +#include +#include + +namespace BT { + class ActorMovement : public StatefulActionNode { + public: + ActorMovement(const std::string &name, const NodeConfiguration &config); + + static PortsList providedPorts(); + + std::shared_ptr target; + + NodeStatus onStart() override; + + NodeStatus onRunning() override; + + void onHalted() override; + }; +} + + +#endif //BUILD_ACTORMOVEMENT_H diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp index ae87a70..8460628 100644 --- a/src/btree_nodes/src/nodes/MoveConnection.cpp +++ b/src/btree_nodes/src/nodes/MoveConnection.cpp @@ -6,32 +6,39 @@ void MoveConnection::planAndExecute(const std::shared_ptr &pose, const std::function &callback) { + std::cout<<"planAndExecute."<moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + bool success = moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS; if(!cancelled) { callback(success); } diff --git a/src/iisy_config_2/config/moveit_controllers.yaml b/src/iisy_config_2/config/moveit_controllers.yaml index 3ad47a3..e278a35 100644 --- a/src/iisy_config_2/config/moveit_controllers.yaml +++ b/src/iisy_config_2/config/moveit_controllers.yaml @@ -1,4 +1,6 @@ # MoveIt uses this configuration for controller management +trajectory_execution: + allowed_execution_duration_scaling: 1.2 # Wait for moves to finish without timeout moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager diff --git a/src/moveit_test/src/test.cpp b/src/moveit_test/src/test.cpp index d9a6203..156d741 100644 --- a/src/moveit_test/src/test.cpp +++ b/src/moveit_test/src/test.cpp @@ -21,14 +21,18 @@ int main(int argc, char * argv[]) // Set a target Pose auto const target_pose = []{ geometry_msgs::msg::Pose msg; - msg.orientation.w = 1.0; - msg.position.x = 0.25; - msg.position.y = 0.25; - msg.position.z = 1.25; + msg.orientation.w = 0.0; + msg.orientation.x = 0.0; + msg.orientation.y = 1.0; + msg.orientation.z = 0.0; + msg.position.x = -0.043357; + msg.position.y = -0.281366; + msg.position.z = 1.1; return msg; }(); - move_group_interface.setGoalOrientationTolerance(100000); + move_group_interface.setGoalOrientationTolerance(0.01); + move_group_interface.setGoalPositionTolerance(0.01); move_group_interface.setPoseTarget(target_pose); //std::map states = {