robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

launching a unified behavior tree (+rbss)

Browse source
This commit is contained in:
shalenikol 2025-03-11 15:58:38 +03:00
parent 4c64536b80
commit cf692ff4c1
8 changed files with 460 additions and 325 deletions
Download patch file Download diff file Expand all files Collapse all files

11
rbs_bt_executor/CMakeLists.txt
View file

@ -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})

241
rbs_bt_executor/config/skills.json Normal file
View file

File diff suppressed because one or more lines are too long

90
rbs_bt_executor/src/ObjectDetection.cpp
View file

@ -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 <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include <memory>
#include <rclcpp/parameter_client.hpp>
using namespace BT;
using namespace std::chrono_literals;
using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
class ObjectDetection : public RosServiceNode<ObjDetectionSrv> {
public:
ObjectDetection(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<ObjDetectionSrv>(name, conf, params) {
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
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<std::string>("ReqKind"),
InputPort<std::string>("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<rclcpp::AsyncParametersClient> m_params_client;
std::vector<rcl_interfaces::msg::Parameter> _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<rclcpp::Parameter> t_parameters{
rclcpp::Parameter("setting_path", m_settings_path)};
m_params_client->set_parameters(t_parameters);
}
};
CreateRosNodePlugin(ObjectDetection, "ObjectDetection");

126
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -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 <behaviortree_ros2/plugins.hpp>
#include <memory>
using namespace BT;
using namespace std::chrono_literals;
using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
class PoseEstimation : public RosServiceNode<PoseEstimationSrv> {
public:
PoseEstimation(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<PoseEstimationSrv>(name, conf, params) {
RCLCPP_INFO_STREAM(logger(), "Start node.");
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
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<std::string>("ReqKind"),
InputPort<std::string>("ObjectName"),
InputPort<std::string>("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<rclcpp::AsyncParametersClient> m_params_client;
std::vector<rcl_interfaces::msg::Parameter> 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<SetParamShareDirectoryStrategy>("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<rclcpp::Parameter> 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<rclcpp::Parameter> params{
rclcpp::Parameter("mesh_path", m_model_name)};
m_params_client->set_parameters(params);
}
};
CreateRosNodePlugin(PoseEstimation, "PoseEstimation");

64
rbs_bt_executor/src/rbsBTAction.cpp
View file

@ -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 <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#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<RbsBtActionSrv> {
public:
RbsBtAction(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
: RosServiceNode<RbsBtActionSrv>(name, conf, params) {
m_action_name = getInput<std::string>(STR_ACTION).value();
RCLCPP_INFO_STREAM(logger(), "Start node RbsBtAction: " + m_action_name);
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(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<std::string>(STR_SID),
InputPort<std::string>(STR_ACTION),
InputPort<std::string>(STR_COMMAND)
});
}
private:
std::string m_action_name{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
};
// !!! теперь устаревшая версия !!!
CreateRosNodePlugin(RbsBtAction, "RbsBtAction")

84
rbs_bt_executor/src/rbssAction.cpp Normal file
View file

@ -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 <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#define STR_ACTION "do"
#define STR_SID "sid"
#define STR_COMMAND "command"
#define NODE_NAME "rbs_interface"
template<typename T> 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<RbssActionSrv> {
public:
RbssAction(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
: RosActionNode<RbssActionSrv>(name, conf, params) {
m_action = getInput<std::string>(STR_ACTION).value();
m_command = getInput<std::string>(STR_COMMAND).value();
RCLCPP_INFO_STREAM(logger(), "Start node RbssAction: " + m_action + "/" + m_command);
m_client = std::make_shared<rclcpp::AsyncParametersClient>(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<std::string>(STR_SID),
BT::InputPort<std::string>(STR_ACTION),
BT::InputPort<std::string>(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<rclcpp::AsyncParametersClient> m_client;
};
CreateRosNodePlugin(RbssAction, "RbssAction")

81
rbs_bt_executor/src/rbssCondition.cpp Normal file
View file

@ -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 <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#define STR_ACTION "do"
#define STR_SID "sid"
#define STR_COMMAND "command"
#define NODE_NAME "rbs_interface"
template<typename T> 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<RbssConditionSrv> {
public:
RbssCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
: RosServiceNode<RbssConditionSrv>(name, conf, params) {
m_action_name = getInput<std::string>(STR_ACTION).value();
m_command = getInput<std::string>(STR_COMMAND).value();
RCLCPP_INFO_STREAM(logger(), "Start node RbssCondition: " + m_action_name + "/" + m_command);
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(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<std::string>(STR_SID),
InputPort<std::string>(STR_ACTION),
InputPort<std::string>(STR_COMMAND)
});
}
private:
int m_i{0};
std::string m_action_name{};
std::string m_command{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
};
CreateRosNodePlugin(RbssCondition, "RbssCondition")

88
rbss_objectdetection/scripts/od_yolo_lc.py
View file

@ -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}")