🔨 add bt_checker and BT node with new lib

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)
 
 
 

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()

robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml

@@ -3,23 +3,17 @@
     <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
         <Sequence>
-            <Action ID="EmuPoseEstimation" PartName="${arg1}"/>
+            <Action ID="GetEntityState" server_name="/get_entity_state" server_timeout="10" PartName="rasmt"/>
-            <Action ID="MoveToPose" arm_group="${arg0}" goal="{goal}" />
+            <Action ID="MoveToPose" server_name="/move_topose" server_timeout="10" cancel_timeout="5" arm_group="${arg0}" />
-            <Action ID="MoveGripper" gripper_group="ramst_hand_arm_group" relative_gap="open" />
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->
     <TreeNodesModel>
-        <Action ID="EmuPoseEstimation">
+        <Action ID="GetEntityState">
             <input_port name="PartName"/>
         </Action>
-        <Action ID="MoveGripper">
-            <input_port name="gripper_group"/>
-            <input_port name="relative_gap"/>
-        </Action>
         <Action ID="MoveToPose">
             <input_port name="arm_group"/>
-            <input_port name="goal"/>
         </Action>
     </TreeNodesModel>
     <!-- ////////// -->

robossembler/config/params.yaml

@@ -1,10 +1,9 @@
 assemble_1:
   ros__parameters:
     plugins:
-      - robossembler_move_bt_node
+      - robossembler_move_topose_bt_action
-      - robossembler_emu_pose_estimation
+      - robossembler_get_entity_state_bt_action_client
-      - robossembler_move_gripper_bt_node
+    PartName: "rasmt"
-    PartName: ["rasmt"]
     arm_group: ["rasmt_arm_group"]
     gripper_group: ["rasmt_hand_arm_group"]
     relative_gaps: ["open", "close"]

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']}
+            ]
+        ),
+    ])

robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp

@@ -10,12 +10,18 @@ class GetEntityState : public BtService<GetEntityStateSrv>
     public:
         GetEntityState(const std::string & name, const BT::NodeConfiguration & config)
         : BtService<GetEntityStateSrv>(name, config){
-
+            
+            //getInput<std::string>("PartName", part_name_);
+            part_name_ = getInput<std::string>("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<GetEntityStateSrv::Request>();
+            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<GetEntityStateSrv>
             RCLCPP_INFO(_node->get_logger(), "Service call complete");
             return BT::NodeStatus::SUCCESS;
         }
+
+        static PortsList providedPorts()
+        {
+            return providedBasicPorts({
+                    InputPort<std::string>("PartName")});
+        }
+    private:
+        std::string part_name_;
 };
 
 BT_REGISTER_NODES(factory)
 {
-    factory.registerNodeType<GetEntityStateSrv>("GetEntityState");
+    factory.registerNodeType<GetEntityState>("GetEntityState");
 }

robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp

@@ -12,14 +12,18 @@ class MoveToPose : public BtAction<MoveToPoseAction>
         MoveToPose(const std::string & name, const BT::NodeConfiguration & config) 
         : BtAction<MoveToPoseAction>(name, config) {
 
-            available_groups_ = getInput<std::string>("arm_group").value();
+            robot_name_ = getInput<std::string>("arm_group").value();
-            target_pose_ = getInput<geometry_msgs::msg::PoseStamped>("goal").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<MoveToPoseAction::Goal>();
+            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<MoveToPoseAction>
         static PortsList providedPorts()
         {
             return providedBasicPorts({
-                    InputPort<std::string>("arm_group"),
+                    InputPort<std::string>("arm_group")});
-                    InputPort<geometry_msgs::msg::PoseStamped>("goal")});
         }
 
     private:
-        std::string available_groups_;
+        //std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
-        std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
         geometry_msgs::msg::PoseStamped target_pose_;
-        std::string arm_group_;
+        std::string robot_name_;
 
 }; // class MoveToPose
 

robossembler/src/bt_checker.cpp

@@ -0,0 +1,77 @@
+
+#include "behavior_tree/BtEngine.hpp"
+#include <csignal>
+
+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<std::string>());
+  // 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<rclcpp::Node::SharedPtr>("node", std::make_shared<rclcpp::Node>("bt_node"));
+//  blackboard->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_);
+  RCLCPP_INFO(this->get_logger(), "Loading tree from file: " + bt_file_path_);
+  tree_ = std::make_shared<Tree>(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<BT::PublisherZMQ>(
+    *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<BtEngine>());
+  rclcpp::shutdown();
+  return 0;
+}

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

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 <geometry_msgs/msg/pose_stamped.hpp>
+#include "geometry_msgs/msg/pose_stamped.hpp"
-#include <geometry_msgs/msg/quaternion.hpp>
+#include "geometry_msgs/msg/quaternion.hpp"
-#include <geometry_msgs/msg/transform.hpp>
+#include "geometry_msgs/msg/transform.hpp"
 
 // moveit libs
-#include <moveit/move_group_interface/move_group_interface.h>
+#include "moveit/move_group_interface/move_group_interface.h"
-#include <moveit/planning_interface/planning_interface.h>
+#include "moveit/planning_interface/planning_interface.h"
 /*
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -38,13 +38,14 @@ class MoveToPoseActionServer : public rclcpp::Node
 public:
     using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
 
-    explicit MoveToPoseActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 
+    //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) 
-    : Node("move_topose_action_server", options)
+    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<MoveitSendPose>(
             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)
+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;
+}*/