From 7ca3fbeb1d534996191da20155090b080a4cc16e Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Wed, 2 Feb 2022 21:47:26 +0400 Subject: [PATCH] =?UTF-8?q?=F0=9F=94=A8=20add=20bt=5Fchecker=20and=20BT=20?= =?UTF-8?q?node=20with=20new=20lib?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/rasmt_moveit.launch.py | 19 +++++ robossembler/CMakeLists.txt | 13 ++++ .../atomic_skills_xml/simple_sequence.xml | 12 +-- robossembler/config/params.yaml | 7 +- robossembler/launch/bt_check.launch.py | 20 +++++ .../atomic_skills/GetEntityState.cpp | 20 ++++- .../atomic_skills/MoveToPose.cpp | 18 +++-- robossembler/src/bt_checker.cpp | 77 +++++++++++++++++++ robossembler_servers/CMakeLists.txt | 9 ++- .../src/move_topose_action_server.cpp | 31 +++++--- 10 files changed, 191 insertions(+), 35 deletions(-) create mode 100644 robossembler/launch/bt_check.launch.py create mode 100644 robossembler/src/bt_checker.cpp diff --git a/rasmt_moveit_config/launch/rasmt_moveit.launch.py b/rasmt_moveit_config/launch/rasmt_moveit.launch.py index 226f83f..6ad8756 100644 --- a/rasmt_moveit_config/launch/rasmt_moveit.launch.py +++ b/rasmt_moveit_config/launch/rasmt_moveit.launch.py @@ -156,9 +156,28 @@ def generate_launch_description(): ] ) + move_topose_action_server = Node( + package="robossembler_servers", + executable="move_topose_action_server", + name="m2p_as", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + robot_description_planning, + ompl_yaml, + planning, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + use_sim_time + ] + ) + launch_nodes = [] launch_nodes.append(rviz) launch_nodes.append(move_group_node) + launch_nodes.append(move_topose_action_server) diff --git a/robossembler/CMakeLists.txt b/robossembler/CMakeLists.txt index c3d8344..b48e01b 100644 --- a/robossembler/CMakeLists.txt +++ b/robossembler/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(ament_index_cpp REQUIRED) find_package(plansys2_bt_actions REQUIRED) find_package(gazebo_msgs REQUIRED) find_package(robossembler_interfaces REQUIRED) +find_package(behavior_tree REQUIRED) if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -43,6 +44,8 @@ set(dependencies plansys2_bt_actions gazebo_msgs robossembler_interfaces + behavior_tree + std_msgs ) include_directories(include) @@ -50,6 +53,9 @@ include_directories(include) add_library(robossembler_move_topose_bt_action SHARED src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp) list(APPEND plugin_libs robossembler_move_topose_bt_action) +add_library(robossembler_get_entity_state_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp) +list(APPEND plugin_libs robossembler_get_entity_state_bt_action_client) + add_library(robossembler_emu_pose_estimation SHARED src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp) list(APPEND plugin_libs robossembler_move_bt_node) @@ -67,6 +73,9 @@ endforeach() add_executable(move_controller_node src/move_controller_node.cpp) ament_target_dependencies(move_controller_node ${dependencies}) +add_executable(bt_check src/bt_checker.cpp) +ament_target_dependencies(bt_check ${dependencies}) + install(DIRECTORY launch pddl behavior_trees_xml config DESTINATION share/${PROJECT_NAME}) @@ -78,6 +87,10 @@ install(TARGETS RUNTIME DESTINATION bin ) +install(TARGETS bt_check + DESTINATION lib/${PROJECT_NAME} + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml b/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml index 6e1de24..c98461f 100644 --- a/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml +++ b/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml @@ -3,23 +3,17 @@ - - - + + - + - - - - - diff --git a/robossembler/config/params.yaml b/robossembler/config/params.yaml index c7e76a2..c467a22 100644 --- a/robossembler/config/params.yaml +++ b/robossembler/config/params.yaml @@ -1,10 +1,9 @@ assemble_1: ros__parameters: plugins: - - robossembler_move_bt_node - - robossembler_emu_pose_estimation - - robossembler_move_gripper_bt_node - PartName: ["rasmt"] + - robossembler_move_topose_bt_action + - robossembler_get_entity_state_bt_action_client + PartName: "rasmt" arm_group: ["rasmt_arm_group"] gripper_group: ["rasmt_hand_arm_group"] relative_gaps: ["open", "close"] diff --git a/robossembler/launch/bt_check.launch.py b/robossembler/launch/bt_check.launch.py new file mode 100644 index 0000000..1bf89a6 --- /dev/null +++ b/robossembler/launch/bt_check.launch.py @@ -0,0 +1,20 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='robossembler', + namespace='', + executable='bt_check', + # Do not declare a node name otherwise it messes with the action node names and will result in + # duplicate nodes! + parameters=[ + {'bt_file_path': os.path.join(get_package_share_directory('robossembler'), 'behavior_trees_xml/atomic_skills_xml/simple_sequence.xml')}, + {'plugins': ['robossembler_get_entity_state_bt_action_client', 'robossembler_move_topose_bt_action']} + ] + ), + ]) \ No newline at end of file diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp index 9ea24d3..133ff22 100644 --- a/robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp +++ b/robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp @@ -10,12 +10,18 @@ class GetEntityState : public BtService public: GetEntityState(const std::string & name, const BT::NodeConfiguration & config) : BtService(name, config){ - + + //getInput("PartName", part_name_); + part_name_ = getInput("PartName").value(); + RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< part_name_<<"]"); } GetEntityStateSrv::Request::SharedPtr populate_request() override { - GetEntityStateSrv::Request::SharedPtr request; + //GetEntityStateSrv::Request::SharedPtr request; + auto request = std::make_shared(); + request->name = part_name_; + RCLCPP_INFO(_node->get_logger(), "Request populated"); return request; } BT::NodeStatus handle_response(GetEntityStateSrv::Response::SharedPtr response) override @@ -23,9 +29,17 @@ class GetEntityState : public BtService RCLCPP_INFO(_node->get_logger(), "Service call complete"); return BT::NodeStatus::SUCCESS; } + + static PortsList providedPorts() + { + return providedBasicPorts({ + InputPort("PartName")}); + } + private: + std::string part_name_; }; BT_REGISTER_NODES(factory) { - factory.registerNodeType("GetEntityState"); + factory.registerNodeType("GetEntityState"); } \ No newline at end of file diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp index b01d06e..94f5900 100644 --- a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp +++ b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp @@ -12,14 +12,18 @@ class MoveToPose : public BtAction MoveToPose(const std::string & name, const BT::NodeConfiguration & config) : BtAction(name, config) { - available_groups_ = getInput("arm_group").value(); - target_pose_ = getInput("goal").value(); + robot_name_ = getInput("arm_group").value(); + target_pose_.pose.position.x = 0.1; + target_pose_.pose.position.y = 0.1; + target_pose_.pose.position.z = 0.6; + RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str()); } MoveToPoseAction::Goal populate_goal() override { MoveToPoseAction::Goal goal; - goal.robot_name = available_groups_; + //auto goal = std::make_shared(); + goal.robot_name = robot_name_; goal.end_effector_acceleration = 1; goal.end_effector_velocity = 1; goal.target_pose = target_pose_; @@ -30,15 +34,13 @@ class MoveToPose : public BtAction static PortsList providedPorts() { return providedBasicPorts({ - InputPort("arm_group"), - InputPort("goal")}); + InputPort("arm_group")}); } private: - std::string available_groups_; - std::map waypoints_; + //std::map waypoints_; geometry_msgs::msg::PoseStamped target_pose_; - std::string arm_group_; + std::string robot_name_; }; // class MoveToPose diff --git a/robossembler/src/bt_checker.cpp b/robossembler/src/bt_checker.cpp new file mode 100644 index 0000000..d112f41 --- /dev/null +++ b/robossembler/src/bt_checker.cpp @@ -0,0 +1,77 @@ + +#include "behavior_tree/BtEngine.hpp" +#include + +BtEngine::BtEngine() +: Node("bt_engine") +{ + configure_parameters(); + load_plugins(); + load_tree(); + if (run_groot_monitoring_) { + add_groot_monitoring(); + } + run(); +} + +void BtEngine::configure_parameters() +{ + bt_file_path_ = this->declare_parameter("bt_file_path", "simple_sequence.xml"); + loop_timeout_ = std::chrono::milliseconds(this->declare_parameter("loop_timeout", 100)); + plugins_ = this->declare_parameter("plugins", std::vector()); + // Groot + run_groot_monitoring_ = this->declare_parameter("run_groot_monitoring", true); + publisher_port_ = this->declare_parameter("publisher_port", 1666); + server_port_ = this->declare_parameter("server_port", 1667); + max_msg_per_second_ = this->declare_parameter("max_msg_per_second", 25); +} + +void BtEngine::load_tree() +{ + auto blackboard = Blackboard::create(); + blackboard->set("node", std::make_shared("bt_node")); +// blackboard->set>("tf_buffer", tf_); + RCLCPP_INFO(this->get_logger(), "Loading tree from file: " + bt_file_path_); + tree_ = std::make_shared(factory_.createTreeFromFile(bt_file_path_, blackboard)); +} + +void BtEngine::run() +{ + rclcpp::WallRate loop_rate(loop_timeout_); + RCLCPP_INFO_STREAM( + this->get_logger(), "Running tree at frequency " << + 1.0 / loop_timeout_.count() * 1e3 << "Hz"); + tree_->tickRoot(); + loop_rate.sleep(); +} + +void BtEngine::add_groot_monitoring() +{ + RCLCPP_INFO_STREAM( + this->get_logger(), "Groot monitoring enabled with server port [" << + server_port_ << "] and publisher port [" << publisher_port_ << "]"); + groot_monitor_ = std::make_unique( + *tree_, max_msg_per_second_, publisher_port_, server_port_); +} + +void BtEngine::load_plugins() +{ + for (const auto & p : plugins_) { + RCLCPP_INFO(this->get_logger(), "Loading plugin: " + SharedLibrary::getOSName(p)); + factory_.registerFromPlugin(SharedLibrary::getOSName(p)); + } +} + +void sigint_handler(__attribute__((unused)) int signal_num) { // Silences compiler warnings + rclcpp::shutdown(); + exit(0); +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + signal(SIGINT, sigint_handler); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/robossembler_servers/CMakeLists.txt b/robossembler_servers/CMakeLists.txt index dad18bc..2acae6b 100644 --- a/robossembler_servers/CMakeLists.txt +++ b/robossembler_servers/CMakeLists.txt @@ -51,7 +51,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() - +# add_library(move_topose_action SHARED src/move_topose_action_server.cpp ) @@ -63,8 +63,13 @@ ament_target_dependencies(move_topose_action ${deps}) rclcpp_components_register_node(move_topose_action PLUGIN "robossembler_actions::MoveToPoseActionServer" EXECUTABLE move_topose_action_server) +#[[ +add_executable(move_topose_action_server src/move_topose_action_server.cpp) +target_include_directories(move_topose_action_server PRIVATE include) +ament_target_dependencies(move_topose_action_server ${deps}) +]] install( - TARGETS move_topose_action + TARGETS move_topose_action_server ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/robossembler_servers/src/move_topose_action_server.cpp b/robossembler_servers/src/move_topose_action_server.cpp index cd4ed94..3454f9f 100644 --- a/robossembler_servers/src/move_topose_action_server.cpp +++ b/robossembler_servers/src/move_topose_action_server.cpp @@ -11,13 +11,13 @@ #include "robossembler_interfaces/msg/action_feedback_status_constants.hpp" #include "robossembler_interfaces/action/moveit_send_pose.hpp" -#include -#include -#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform.hpp" // moveit libs -#include -#include +#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/planning_interface/planning_interface.h" /* #include #include @@ -38,13 +38,14 @@ class MoveToPoseActionServer : public rclcpp::Node public: using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose; - explicit MoveToPoseActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("move_topose_action_server", options) + //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveToPoseActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("move_topose_action_server", options)//, node_(node) { using namespace std::placeholders; this->action_server_ = rclcpp_action::create_server( this, - "MoveToPoseActionServer", + "move_topose", std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); @@ -123,4 +124,16 @@ private: }// namespace robossembler_actions -RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_actions::MoveToPoseActionServer) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_actions::MoveToPoseActionServer) + +/* +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("move_topose", "", node_options); + rclcpp::spin(node); + //rclcpp::shutdown(); + return 0; +}*/