diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt index 0bb5ed0..3cd889c 100644 --- a/rbs_bt_executor/CMakeLists.txt +++ b/rbs_bt_executor/CMakeLists.txt @@ -83,8 +83,8 @@ list(APPEND plugin_libs rbs_skill_move_joint_state) # add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp) # list(APPEND plugin_libs rbs_pose_estimation) -add_library(rbs_object_detection SHARED src/ObjectDetection.cpp) -list(APPEND plugin_libs rbs_object_detection) +# add_library(rbs_object_detection SHARED src/ObjectDetection.cpp) +# list(APPEND plugin_libs rbs_object_detection) # add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp) # list(APPEND plugin_libs rbs_env_manager_starter) @@ -95,8 +95,11 @@ list(APPEND plugin_libs rbs_skill_move_topose_array_bt_action_client) add_library(rbs_get_workspace SHARED src/GetWorkspace.cpp) list(APPEND plugin_libs rbs_get_workspace) -add_library(rbs_act SHARED src/rbsBTAction.cpp) -list(APPEND plugin_libs rbs_act) +add_library(rbss_act SHARED src/rbssAction.cpp) +list(APPEND plugin_libs rbss_act) + +add_library(rbss_cond SHARED src/rbssCondition.cpp) +list(APPEND plugin_libs rbss_cond) add_executable(rbs_bt_executor src/TreeRunner.cpp) ament_target_dependencies(rbs_bt_executor ${dependencies}) diff --git a/rbs_bt_executor/config/skills.json b/rbs_bt_executor/config/skills.json new file mode 100644 index 0000000..c7e33ab --- /dev/null +++ b/rbs_bt_executor/config/skills.json @@ -0,0 +1,241 @@ +{ + "skills": [ + { + "sid": "c4aaffa2-7bef-4f0e-981f-ab7cd627b93d", + "SkillPackage": { "name": "Robossembler", "version": "1", "format": "1.0" }, + "Module": { "node_name": "lc_yolo", "name": "ObjectDetection", "description": "Object detection skill with YOLOv8" }, + "BTAction": [ + { + "name": "odConfigure", + "type": "run", + "param": [ + { + "type": "topic", + "dependency": { + "type": "topic", + "topicType": "sensor_msgs/msg/Image", + "topicOut": "/rgbd_camera/image" + }, + "isFilled": true + }, + { + "type": "formBuilder", + "dependency": { + "result": "{\"process\":\\${:OBJECT:{\"type\":\"WEIGHTS\"},\"object_name\":\\${object_name:string:}}", + "context": "", + "form": [ + "{\"name\":\"\",\"type\":\"OBJECT\",\"defaultValue\":{\"type\":\"WEIGHTS\"},\"totalValue\":\"{\\\"type\\\":\\\"WEIGHTS\\\",\\\"selectProcess\\\":{\\\"value\\\":{\\\"_id\\\":\\\"67af21227ccd2ccaf17318fb\\\",\\\"script\\\":\\\"python3 $PYTHON_EDUCATION\\\",\\\"type\\\":\\\"WEIGHTS\\\",\\\"instanceName\\\":\\\"wod_shildik_sh\\\",\\\"name\\\":\\\"pWod\\\",\\\"isEnd\\\":true,\\\"createDate\\\":\\\"1737729633581\\\",\\\"card\\\":\\\"pose_estimate\\\",\\\"path\\\":\\\"/home/shalenikol/webservice/server/build/public/process/pWod\\\",\\\"instancePath\\\":\\\"/home/shalenikol/webservice/server/build/public/process/pWod/wod_shildik_sh\\\",\\\"project\\\":{\\\"_id\\\":\\\"678a2a17b960c6f1e7228577\\\",\\\"description\\\":\\\"exhibition_01\\\",\\\"rootDir\\\":\\\"/home/shalenikol/webservice/server/build/public/a54164c1-c4f8-44ff-adef-0d4f377fba65\\\",\\\"isActive\\\":true,\\\"__v\\\":0},\\\"__v\\\":0,\\\"lastProcessExecCommand\\\":\\\"python3 $PYTHON_EDUCATION --path /home/shalenikol/webservice/server/build/public/process/pWod/wod_shildik_sh --form /home/shalenikol/webservice/server/build/public/process/pWod/wod_shildik_sh/form.json\\\",\\\"processStatus\\\":\\\"endOk\\\",\\\"lastProcessLogs\\\":\\\"Downloading https://github.com/ultralytics/assets/releases/download/v8.1.0/yolov8n.pt to '/home/shalenikol/webservice/server/build/public/process/pWod/wod_shildik_sh/yolov8n.pt'...\\\\n\\\\n\\\\n 0%| | 0.00/6.23M [00:00:[]}}", + "context": "type ITEM = {\"name\": \\${NAME:string:default},\"value\": \\${VALUE:string:default}};", + "form": [ + "{\"name\":\"ITEM\",\"type\":\"Array\",\"defaultValue\":\"[]\",\"values\":[{\"name\":\"NAME\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"fa8b442a-101b-448b-b5fd-55ad64f8e578\"},{\"name\":\"VALUE\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"c59b86d8-a54b-42da-b2bf-5fdfeff7f711\"}],\"totalValue\":[],\"isOpen\":true,\"subType\":\"ITEM\",\"id\":\"8be7af67-5860-4a3f-bdab-09c75a53bff7\"}" + ], + "output": { + "params": [ + { + "name": "is_image_mode", + "value": "True" + }, + { + "name": "threshold", + "value": "0.25" + }, + { + "name": "publishDelay", + "value": "0.33" + } + ] + }, + "type": "formBuilder" + }, + "__v": 0 + }, + { + "_id": "67b33b15f3412f3530fdb337", + "bgColor": "rgba(5, 26, 39, 1)", + "borderColor": "rgba(25, 130, 196, 1)", + "sid": "4006188a-a4b4-4ca3-b0bf-11fad7f81263", + "SkillPackage": { + "name": "Robossembler", + "version": "1", + "format": "1.0" + }, + "Module": { + "node_name": "lc_yolo", + "name": "ObjectDetection", + "description": "Object detection skill with YOLOv8" + }, + "BTAction": [ + { + "name": "odStop", + "type": "stop", + "param": [], + "typeAction": "ACTION" + } + ], + "topicsOut": [ + { + "name": "lc_yolo/object_detection", + "type": "rbs_skill_interfaces/msg/BoundBox" + }, + { + "name": "lc_yolo/detect_image", + "type": "sensor_msgs/msg/Image" + } + ], + "Launch": { + "executable": "od_yolo_lc.py", + "package": "rbss_objectdetection" + }, + "Settings": { + "result": "{\"params\": \\${ITEM:Array:[]}}", + "context": "type ITEM = {\"name\": \\${NAME:string:default},\"value\": \\${VALUE:string:default}};", + "form": [ + "{\"name\":\"ITEM\",\"type\":\"Array\",\"defaultValue\":\"[]\",\"values\":[{\"name\":\"NAME\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"fa8b442a-101b-448b-b5fd-55ad64f8e578\"},{\"name\":\"VALUE\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"c59b86d8-a54b-42da-b2bf-5fdfeff7f711\"}],\"totalValue\":[],\"isOpen\":true,\"subType\":\"ITEM\",\"id\":\"8be7af67-5860-4a3f-bdab-09c75a53bff7\"}" + ], + "output": { + "params": [] + }, + "type": "formBuilder" + }, + "__v": 0 + }, + { + "sid": "805dcd18-264c-4c5f-a874-76d98e98f2b7", + "SkillPackage": { "name": "Robossembler", "version": "1", "format": "1.0" }, + "Module": { "node_name": "skill_mtp", "name": "MoveToPose", "description": "Move to Pose skill" }, + "Launch": { "executable": "skill_movetopose.py", "package": "rbss_movetopose" }, + "BTAction": [ + { + "name": "move", + "type": "action", + "param": [ + { + "type": "formBuilder", + "dependency": { + "result": "{\"robot_name\": \\${ROBOT_NAME:string:rbs_arm}, \"pose\": {\"position\": { \"x\": \\${X:number:0.1}, \"y\": \\${Y:number:0.1}, \"z\": \\${Z:number:0.7} }, \"orientation\": { \"x\": \\${X:number:0.1}, \"y\": \\${Y:number:0.1}, \"z\": \\${Z:number:0.7}, \"w\": \\${W:number:1.0} }}}", + "context": "type ITEM = {\"name\": \\${NAME:string:default}};", + "form": [ + "{\"name\":\"ROBOT_NAME\",\"type\":\"string\",\"defaultValue\":\"rbs_arm\",\"isOpen\":false,\"id\":\"7b2611d6-ff4d-4eb9-a210-e5c146722906\"}", + "{\"name\":\"X\",\"type\":\"number\",\"defaultValue\":\"0.1\",\"totalValue\":\"0\",\"isOpen\":false,\"id\":\"5df2228b-fd4a-4937-9386-d6e149eedb89\"}", + "{\"name\":\"Y\",\"type\":\"number\",\"defaultValue\":\"0.1\",\"totalValue\":\"0.2\",\"isOpen\":false,\"id\":\"9161f3c4-5d8f-4f76-9e1f-375bb664c51b\"}", + "{\"name\":\"Z\",\"type\":\"number\",\"defaultValue\":\"0.7\",\"totalValue\":\"0.6\",\"isOpen\":false,\"id\":\"6798619e-6ff5-46d5-9477-90dd7454c123\"}", + "{\"name\":\"X\",\"type\":\"number\",\"defaultValue\":\"0.1\",\"totalValue\":\"0.55\",\"isOpen\":false,\"id\":\"60e3c96a-101c-4fe5-9899-b2dbdf0448c1\"}", + "{\"name\":\"Y\",\"type\":\"number\",\"defaultValue\":\"0.1\",\"totalValue\":\"0.44\",\"isOpen\":false,\"id\":\"31539e2b-f081-4d58-ac4b-044326f87fac\"}", + "{\"name\":\"Z\",\"type\":\"number\",\"defaultValue\":\"0.7\",\"totalValue\":\"0.0\",\"isOpen\":false,\"id\":\"7fa23bfd-5dc5-4397-b559-f45dc7fc5cff\"}", + "{\"name\":\"W\",\"type\":\"number\",\"defaultValue\":\"1.0\",\"isOpen\":false,\"id\":\"a9411bdf-b600-4616-9cd4-4c4d34da72fd\"}" + ], + "output": { + "robot_name": "rbs_arm", + "pose": { + "position": { + "x": 0.104, + "y": -0.28, + "z": 0.1 + }, + "orientation": { + "x": 0, + "y": 1, + "z": 0, + "w": 0 + } + } + }, + "type": "formBuilder" + }, + "isFilled": true + } + ], + "result": [], + "typeAction": "ACTION" + } + ], + "topicsOut": [], + "Settings": { + "result": "{\"params\": \\${ITEM:Array:[]}}", + "context": "type ITEM = {\"name\": \\${NAME:string:default},\"value\": \\${VALUE:string:default}};", + "form": [], + "output": { + "params": [ + { + "name": "server_name", + "value": "mtp_jtc", + "value2": "mtp_moveit", + "value3": "mtp_cart" + }, + { + "name": "duration", + "value": "2.0" + }, + { + "name": "end_effector_velocity", + "value": "1.0" + }, + { + "name": "end_effector_acceleration", + "value": "1.0" + } + ] + }, + "type": "formBuilder" + }, + "__v": 0 + } + ] +} \ No newline at end of file diff --git a/rbs_bt_executor/src/ObjectDetection.cpp b/rbs_bt_executor/src/ObjectDetection.cpp deleted file mode 100644 index b010271..0000000 --- a/rbs_bt_executor/src/ObjectDetection.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include "behaviortree_ros2/bt_service_node.hpp" -#include "lifecycle_msgs/msg/transition.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" -#include "rcl_interfaces/msg/parameter.hpp" -#include "rcl_interfaces/srv/set_parameters.hpp" -#include -#include -#include -#include -#include - -using namespace BT; -using namespace std::chrono_literals; -using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState; - -class ObjectDetection : public RosServiceNode { -public: - ObjectDetection(const std::string &name, const NodeConfig &conf, - const RosNodeParams ¶ms) - : RosServiceNode(name, conf, params) { - m_params_client = std::make_shared( - node_.lock(), "/object_detection"); - while (!m_params_client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(logger(), - "Interrupted while waiting for the service. Exiting."); - break; - } - } - getInput("SettingPath", m_settings_path); - } - - static PortsList providedPorts() { - return providedBasicPorts({ - InputPort("ReqKind"), - InputPort("SettingPath"), - }); - } - - bool setRequest(Request::SharedPtr &request) override { - getInput("ReqKind", m_req_type); - auto transition = transition_event(m_req_type); - request->set__transition(transition); - return true; - } - - NodeStatus onResponseReceived(const Response::SharedPtr &response) override { - if (!response->success) { - return NodeStatus::FAILURE; - } - return NodeStatus::SUCCESS; - } - -private: - uint8_t transition_id_{}; - std::string m_settings_path{}; - // std::string _model_path{}; - std::string m_req_type{}; - std::shared_ptr m_params_client; - std::vector _params; - rcl_interfaces::msg::Parameter m_param; - - lifecycle_msgs::msg::Transition - transition_event(const std::string &req_type) { - lifecycle_msgs::msg::Transition translation{}; - if (req_type == "configure") { - set_str_param(); - transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE; - } else if (req_type == "activate") { - transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE; - } else if (req_type == "deactivate") { - transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE; - } else if (req_type == "cleanup") { - transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP; - } - translation.set__id(transition_id_); - return translation; - } - - void set_str_param() { - RCLCPP_INFO_STREAM(logger(), - "Set string parameter: <" + m_settings_path + ">"); - - std::vector t_parameters{ - rclcpp::Parameter("setting_path", m_settings_path)}; - m_params_client->set_parameters(t_parameters); - } -}; - -CreateRosNodePlugin(ObjectDetection, "ObjectDetection"); diff --git a/rbs_bt_executor/src/PoseEstimation.cpp b/rbs_bt_executor/src/PoseEstimation.cpp deleted file mode 100644 index df63330..0000000 --- a/rbs_bt_executor/src/PoseEstimation.cpp +++ /dev/null @@ -1,126 +0,0 @@ -#include "behaviortree_ros2/bt_service_node.hpp" - -#include "rcl_interfaces/msg/parameter.hpp" -#include "rcl_interfaces/srv/set_parameters.hpp" - -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "lifecycle_msgs/msg/transition.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" -#include -#include - -using namespace BT; -using namespace std::chrono_literals; -using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState; - -class PoseEstimation : public RosServiceNode { -public: - PoseEstimation(const std::string &name, const NodeConfig &conf, - const RosNodeParams ¶ms) - : RosServiceNode(name, conf, params) { - RCLCPP_INFO_STREAM(logger(), "Start node."); - - m_params_client = std::make_shared( - node_.lock(), "/pose_estimation"); - - while (!m_params_client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(logger(), - "Interrupted while waiting for the service. Exiting."); - break; - } - RCLCPP_WARN(logger(), "service not available, waiting again..."); - } - - // Get model name paramter from BT ports - getInput("ObjectName", m_model_name); - } - - bool setRequest(Request::SharedPtr &request) override { - getInput("ReqKind", m_req_type); - request->set__transition(transition_event(m_req_type)); - return true; - } - - NodeStatus onResponseReceived(const Response::SharedPtr &response) override { - if (!response->success) { - return NodeStatus::FAILURE; - } - return NodeStatus::SUCCESS; - } - - static PortsList providedPorts() { - return providedBasicPorts({ - InputPort("ReqKind"), - InputPort("ObjectName"), - InputPort("ObjectPath"), - }); - } - -private: - uint8_t m_transition_id{}; - std::string m_model_name{}; - std::string m_model_type{}; - std::string m_req_type{}; - std::shared_ptr m_params_client; - std::vector m_params; - rcl_interfaces::msg::Parameter m_param; - - lifecycle_msgs::msg::Transition - transition_event(const std::string &req_type) { - lifecycle_msgs::msg::Transition translation{}; - // ParamSetter param_setter(m_params_client); - if (req_type == "configure") { - set_mesh_param(); - // auto str_mesh_param = - // std::make_shared("model_name", - // m_model_name); param_setter.setStrategy(str_mesh_param); - // param_setter.setParam() - - m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE; - } else if (req_type == "calibrate") { - set_str_param(); - m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE; - } else if (req_type == "activate") { - m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE; - } else if (req_type == "deactivate") { - m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE; - } else if (req_type == "cleanup") { - m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP; - } - // calibrate - translation.set__id(m_transition_id); - return translation; - } - - inline std::string build_model_path(const std::string &model_name, - const std::string &package_path) { - return package_path + "/config/" + model_name + ".ply"; - } - - inline std::string build_model_path(const std::string &model_path) { - return model_path; - } - - void set_str_param() { - RCLCPP_INFO_STREAM(logger(), - "Set string parameter: <" + m_model_name + ">"); - - std::vector params{ - rclcpp::Parameter("model_name", m_model_name)}; - m_params_client->set_parameters(params); - } - - void set_mesh_param() { - auto t_package_name = - ament_index_cpp::get_package_share_directory("rbs_perception"); - m_model_type = build_model_path(m_model_name, t_package_name); - RCLCPP_INFO_STREAM(logger(), m_model_type); - - std::vector params{ - rclcpp::Parameter("mesh_path", m_model_name)}; - m_params_client->set_parameters(params); - } -}; - -CreateRosNodePlugin(PoseEstimation, "PoseEstimation"); diff --git a/rbs_bt_executor/src/rbsBTAction.cpp b/rbs_bt_executor/src/rbsBTAction.cpp deleted file mode 100644 index 0675785..0000000 --- a/rbs_bt_executor/src/rbsBTAction.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// #include "behavior_tree/BtService.hpp" -#include "behaviortree_ros2/bt_service_node.hpp" -#include "rbs_skill_interfaces/srv/rbs_bt.hpp" -#include -#include -#include - -#define STR_ACTION "do" -#define STR_SID "sid" -#define STR_COMMAND "command" -#define NODE_NAME "rbs_interface" - -using namespace BT; -using namespace std::chrono_literals; -using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt; - -class RbsBtAction : public RosServiceNode { -public: - RbsBtAction(const std::string &name, const NodeConfig& conf, const RosNodeParams& params) - : RosServiceNode(name, conf, params) { - - m_action_name = getInput(STR_ACTION).value(); - RCLCPP_INFO_STREAM(logger(), "Start node RbsBtAction: " + m_action_name); - - m_params_client = std::make_shared(node_.lock(), NODE_NAME); - - while (!m_params_client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting."); - break; - } - RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again..."); - } - } - - bool setRequest(Request::SharedPtr &request) override { - request->action = m_action_name; - getInput(STR_SID, request->sid); - getInput(STR_COMMAND, request->command); - return true; - } - - NodeStatus onResponseReceived(const Response::SharedPtr &response) override { - if (!response->ok) { - return NodeStatus::FAILURE; - } - return NodeStatus::SUCCESS; - } - - static PortsList providedPorts() { - return providedBasicPorts({ - InputPort(STR_SID), - InputPort(STR_ACTION), - InputPort(STR_COMMAND) - }); - } - -private: - std::string m_action_name{}; - std::shared_ptr m_params_client; -}; - -// !!! теперь устаревшая версия !!! -CreateRosNodePlugin(RbsBtAction, "RbsBtAction") diff --git a/rbs_bt_executor/src/rbssAction.cpp b/rbs_bt_executor/src/rbssAction.cpp new file mode 100644 index 0000000..6f1ce6f --- /dev/null +++ b/rbs_bt_executor/src/rbssAction.cpp @@ -0,0 +1,84 @@ +// #include "behavior_tree/BtService.hpp" +// #include "behaviortree_ros2/bt_service_node.hpp" +#include "behaviortree_ros2/bt_action_node.hpp" +// #include "rbs_skill_interfaces/srv/rbs_bt.hpp" +#include "rbs_skill_interfaces/action/rbs_bt.hpp" +#include +#include +#include + +#define STR_ACTION "do" +#define STR_SID "sid" +#define STR_COMMAND "command" +#define NODE_NAME "rbs_interface" + +template std::string to_string(const T &t) +{ + std::stringstream ss; + ss << t; + return ss.str(); +} + +using namespace BT; +using namespace std::chrono_literals; +using RbssActionSrv = rbs_skill_interfaces::action::RbsBt; + +class RbssAction : public RosActionNode { +public: + RbssAction(const std::string &name, const NodeConfig &conf, const RosNodeParams ¶ms) + : RosActionNode(name, conf, params) { + + m_action = getInput(STR_ACTION).value(); + m_command = getInput(STR_COMMAND).value(); + RCLCPP_INFO_STREAM(logger(), "Start node RbssAction: " + m_action + "/" + m_command); + + m_client = std::make_shared(node_.lock(), NODE_NAME); + + while (!m_client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting."); + break; + } + RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again..."); + } + + } + + static BT::PortsList providedPorts() { + return providedBasicPorts({ + BT::InputPort(STR_SID), + BT::InputPort(STR_ACTION), + BT::InputPort(STR_COMMAND) + }); + } + + bool setGoal(RosActionNode::Goal& goal) override { + getInput(STR_ACTION, goal.action); + getInput(STR_COMMAND, goal.command); + getInput(STR_SID, goal.sid); + m_action = goal.action; + m_command = goal.command; + + m_i++; + RCLCPP_INFO_STREAM(logger(), "setGoal: " + to_string(m_i) + ") " + goal.action + "/" + goal.command + " goal.sid = " + to_string(goal.sid)); + return true; + } + + NodeStatus onResultReceived(const WrappedResult& wr) override { + m_i++; + RCLCPP_INFO_STREAM(logger(), "onResultReceived: " + to_string(m_i) + ") " + m_action + "/" + m_command + " wr.result->ok - " + to_string(wr.result->ok)); + + if (!wr.result->ok) { + return NodeStatus::FAILURE; + } + return NodeStatus::SUCCESS; + } + +private: + int m_i{0}; + std::string m_action{}; + std::string m_command{}; + std::shared_ptr m_client; +}; + +CreateRosNodePlugin(RbssAction, "RbssAction") diff --git a/rbs_bt_executor/src/rbssCondition.cpp b/rbs_bt_executor/src/rbssCondition.cpp new file mode 100644 index 0000000..95cc33b --- /dev/null +++ b/rbs_bt_executor/src/rbssCondition.cpp @@ -0,0 +1,81 @@ +// #include "behavior_tree/BtService.hpp" +#include "behaviortree_ros2/bt_service_node.hpp" +// #include "behaviortree_ros2/bt_action_node.hpp" +#include "rbs_skill_interfaces/srv/rbs_bt.hpp" +// #include "rbs_skill_interfaces/action/rbs_bt.hpp" +#include +#include +#include + +#define STR_ACTION "do" +#define STR_SID "sid" +#define STR_COMMAND "command" +#define NODE_NAME "rbs_interface" + +template std::string to_string(const T &t) +{ + std::stringstream ss; + ss << t; + return ss.str(); +} + +using namespace BT; +using namespace std::chrono_literals; +using RbssConditionSrv = rbs_skill_interfaces::srv::RbsBt; + +class RbssCondition : public RosServiceNode { + public: + RbssCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params) + : RosServiceNode(name, conf, params) { + + m_action_name = getInput(STR_ACTION).value(); + m_command = getInput(STR_COMMAND).value(); + RCLCPP_INFO_STREAM(logger(), "Start node RbssCondition: " + m_action_name + "/" + m_command); + + m_params_client = std::make_shared(node_.lock(), NODE_NAME); + + while (!m_params_client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting."); + break; + } + RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again..."); + } + } + + bool setRequest(Request::SharedPtr &request) override { + request->action = m_action_name; + request->command = m_command; + getInput(STR_SID, request->sid); + + m_i++; + RCLCPP_INFO_STREAM(logger(), "setRequest: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " request->sid = " + to_string(request->sid)); + return true; + } + + NodeStatus onResponseReceived(const Response::SharedPtr &response) override { + m_i++; + RCLCPP_INFO_STREAM(logger(), "onResponseReceived: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " response->ok - " + to_string(response->ok)); + + if (!response->ok) { + return NodeStatus::FAILURE; + } + return NodeStatus::SUCCESS; + } + + static PortsList providedPorts() { + return providedBasicPorts({ + InputPort(STR_SID), + InputPort(STR_ACTION), + InputPort(STR_COMMAND) + }); + } + + private: + int m_i{0}; + std::string m_action_name{}; + std::string m_command{}; + std::shared_ptr m_params_client; +}; + +CreateRosNodePlugin(RbssCondition, "RbssCondition") diff --git a/rbss_objectdetection/scripts/od_yolo_lc.py b/rbss_objectdetection/scripts/od_yolo_lc.py index f1d5f62..4128440 100755 --- a/rbss_objectdetection/scripts/od_yolo_lc.py +++ b/rbss_objectdetection/scripts/od_yolo_lc.py @@ -41,9 +41,11 @@ class ObjectDetection(Node): """Construct the node.""" self._count: int = 0 self._pub: Optional[Publisher] = None + self._pubI: Optional[Publisher] = None self._timer: Optional[Timer] = None self._image_cnt: int = 0 self._sub = None + self._threshold = 0.0 self._is_image_mode = False self.yolov8_weights_file = "" self.model = None @@ -75,14 +77,15 @@ class ObjectDetection(Node): def _Settings(self): # Initialization service settings - # TODO - # for prop in self.skill_cfg["Settings"]: - # nam = prop["name"] - # val = prop["value"] - # if nam == "publishDelay": - # self.publishDelay = val - # elif nam == "is_image_mode": - # self._is_image_mode = val + for prop in self.skill_cfg["Settings"]["output"]["params"]: + nam = prop["name"] + val = prop["value"] + if nam == "threshold": + self._threshold = float(val) + elif nam == "publishDelay": + self.publishDelay = float(val) + elif nam == "is_image_mode": + self._is_image_mode = (val == "True") for prop in self.skill_cfg["topicsOut"]: if prop["type"] == OUT_TOPIC_TYPE: @@ -133,10 +136,9 @@ class ObjectDetection(Node): return TransitionCallbackReturn.FAILURE # Create the publisher. + self._pub = self.create_lifecycle_publisher(BoundBox, self.topicSrv, 10) if self._is_image_mode: - self._pub = self.create_lifecycle_publisher(Image, self.topicDetectImage, 3) - else: - self._pub = self.create_lifecycle_publisher(BoundBox, self.topicSrv, 10) + self._pubI = self.create_lifecycle_publisher(Image, self.topicDetectImage, 1) self._timer = self.create_timer(self.publishDelay, self.publish) @@ -149,7 +151,7 @@ class ObjectDetection(Node): def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: self.get_logger().info('on_activate() is called.') # Create the main subscriber. - self._sub = self.create_subscription(Image, self.topicImage, self.listener_callback, 3) + self._sub = self.create_subscription(Image, self.topicImage, self.listener_callback, 1) return super().on_activate(state) def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: @@ -176,6 +178,7 @@ class ObjectDetection(Node): self.destroy_timer(self._timer) self.destroy_publisher(self._pub) + self.destroy_publisher(self._pubI) self.destroy_subscription(self._sub) self.get_logger().info('on_cleanup() is called.') @@ -187,6 +190,7 @@ class ObjectDetection(Node): """ self.destroy_timer(self._timer) self.destroy_publisher(self._pub) + self.destroy_publisher(self._pubI) self.destroy_subscription(self._sub) self.get_logger().info('on_shutdown() is called.') @@ -198,36 +202,38 @@ class ObjectDetection(Node): if self._pub is not None and self._pub.is_activated: # Publish result + if not self.bbox_res: + return + msg = BoundBox() + msg.is_detection = False + #from ultralytics.engine.results + cconf = self._threshold # threshold + bb = None + for c in self.bbox_res.boxes: + idx = int(c.cls) + if self.bbox_res.names[idx] == self.objName: + conf = float(c.conf) + if cconf < conf: + cconf = conf + bb = c + if bb: + msg.is_detection = True + msg.name = self.objName + msg.x = float(bb.xywhn[0,0]) + msg.y = float(bb.xywhn[0,1]) + msg.w = float(bb.xywhn[0,2]) + msg.h = float(bb.xywhn[0,3]) + msg.conf = float(bb.conf) + # Publish the message. + self._pub.publish(msg) + if self._is_image_mode: if len(self.image_det) == 0: return # The 'cv2_to_imgmsg' method converts an OpenCV image to a ROS 2 image message msg = self.br.cv2_to_imgmsg(self.image_det, encoding="bgr8") - else: - if not self.bbox_res: - return - msg = BoundBox() - msg.is_detection = False - #from ultralytics.engine.results - cconf = 0.0 # threshold - bb = None - for c in self.bbox_res.boxes: - idx = int(c.cls) - if self.bbox_res.names[idx] == self.objName: - conf = float(c.conf) - if cconf < conf: - cconf = conf - bb = c - if bb: - msg.is_detection = True - msg.name = self.objName - msg.x = float(bb.xywhn[0,0]) - msg.y = float(bb.xywhn[0,1]) - msg.w = float(bb.xywhn[0,2]) - msg.h = float(bb.xywhn[0,3]) - msg.conf = float(bb.conf) - # Publish the message. - self._pub.publish(msg) + # Publish the message. + self._pubI.publish(msg) def listener_callback(self, data): """ @@ -235,7 +241,7 @@ class ObjectDetection(Node): """ if self.model: self._image_cnt += 1 - if self._image_cnt % 100 == 1: + if self._image_cnt % 200 == 1: self.get_logger().info(f"detection: {self._image_cnt}") # Convert ROS Image message to OpenCV image @@ -243,12 +249,12 @@ class ObjectDetection(Node): # run inference results = self.model(current_frame) + for r in results: + self.bbox_res = r + if self._is_image_mode: for r in results: self.image_det = r.plot() # plot a BGR numpy array of predictions - else: - for r in results: - self.bbox_res = r # self.get_logger().info(f"detection: end {self._image_cnt}")