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

Cartesian Move_to_pose Skill for BT Interface Node

Browse source
This commit is contained in:
Igor Brylev 2024-09-04 08:46:27 +00:00
parent bf965bb750
commit b4b452297d
18 changed files with 743 additions and 64 deletions
Download patch file Download diff file Expand all files Collapse all files

21
rbs_interface/CMakeLists.txt
View file

@ -19,9 +19,11 @@ find_package(rbs_skill_interfaces REQUIRED)
find_package(PCL 1.12 REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_msgs REQUIRED)
# find_package(tf2_ros REQUIRED)
# find_package(tf2_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(behaviortree_ros2 REQUIRED)
find_package(btcpp_ros2_interfaces REQUIRED)
# Install Python modules
ament_python_install_package(${PROJECT_NAME})
@ -33,6 +35,21 @@ install(PROGRAMS
DESTINATION lib/${PROJECT_NAME}
)
set(THIS_PACKAGE_DEPS
rbs_skill_interfaces
behaviortree_ros2
# std_msgs
# std_srvs
btcpp_ros2_interfaces )
add_executable(bt_exec src/BTExec.cpp)
ament_target_dependencies(bt_exec ${THIS_PACKAGE_DEPS})
install(TARGETS
bt_exec
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}

3
rbs_interface/launch/interface.launch.py
View file

@ -29,6 +29,7 @@ def get_skill_list_(path: str) -> list:
excluding = {}
skills = []
for skill in data["skills"]:
node_name = skill["Module"]["node_name"]
launch = skill["Launch"]
p = launch["package"]
e = launch["executable"]
@ -37,7 +38,7 @@ def get_skill_list_(path: str) -> list:
continue
nn_skills += 1
# skills.append(Node(package=p, executable=e))
launch["parameters"] = [{launch["name"]+PARAM_SUFFIX: json.dumps(skill)}]
launch["parameters"] = [{node_name+PARAM_SUFFIX: json.dumps(skill)}]
skills.append(Node(**launch))
return skills

3
rbs_interface/package.xml
View file

@ -17,7 +17,8 @@
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>rbs_skill_interfaces</depend>
<depend>behaviortree_ros2</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

106
rbs_interface/scripts/rbs_interface.py
View file

@ -4,44 +4,54 @@
ROS 2 program for Robossembler
@shalenikol release 0.1
@shalenikol release 0.2 BT v.4
"""
import os
import json
# import yaml
import rclpy
from rclpy.node import Node
# from rclpy.action import ActionClient
from rclpy.action import ActionServer
from rclpy.callback_groups import ReentrantCallbackGroup
from ament_index_python.packages import get_package_share_directory
from rclpy.parameter import Parameter
from rcl_interfaces.srv import SetParameters #, GetParameters
from rcl_interfaces.msg import SetParametersResult #, ParameterEvent
from lifecycle_msgs.srv import ChangeState #, GetState
from lifecycle_msgs.msg import Transition
from lifecycle_msgs.srv import ChangeState, GetState
from lifecycle_msgs.msg import Transition, State
from rbs_skill_interfaces.srv import RbsBt
from rbs_skill_interfaces.action import RbsBt as RbsBtAction
# from btcpp_ros2_interfaces.action import ExecuteTree
# from rclpy.parameter_client import AsyncParameterClient # only Iron
# PARAM_NAME = "str_param"
# PARAM_SKILL_CONFIG = "skill_cfg"
PARAM_BT = "bt_path"
# BT_SERVER = "bt_execution"
CONDITION_SRV_NAME = "/condition"
BT_PARAM = "bt_path"
NODE_NAME = "rbs_interface"
SERVICE_NAME = "rbs_interface_s"
SERVER_NAME = "rbs_interface_a"
FILE_SKILLS = "skills.json"
PARAM_SUFFIX = "_cfg"
# def get_transfer_path_():
# return os.path.join(get_package_share_directory("rbs_interface"), "config")
KEY_BTPARAM = "BTAction" # TODO "bt_param"
class rbsInterface(Node):
def __init__(self, node_name):
"""Construct the node."""
self.bt_path = "" # path to the current BehaviorTree
self.cfg_data = None # config for current action
self._timer = None # for BT v.4
super().__init__(node_name)
self.declare_parameter(PARAM_BT, rclpy.Parameter.Type.STRING)
self.declare_parameter(BT_PARAM, rclpy.Parameter.Type.STRING)
self.cb_group = ReentrantCallbackGroup()
self._service = self.create_service(RbsBt, node_name, self.service_callback, callback_group=self.cb_group)
# for Condition
self._service = self.create_service(RbsBt, SERVICE_NAME, self.service_callback, callback_group=self.cb_group)
# for Action
self._action = ActionServer(self, RbsBtAction, SERVER_NAME, self.action_callback, callback_group=self.cb_group)
# self.bt_client = ActionClient(self, ExecuteTree, BT_SERVER, callback_group=self.cb_group)
# self.client = AsyncParameterClient(self.client_node, 'test_parameter_client_target') # only Iron
self.add_on_set_parameters_callback(self._on_set_btpath_param)
@ -52,12 +62,11 @@ class rbsInterface(Node):
def _on_set_btpath_param(self, parameter_list):
for parameter in parameter_list:
if parameter.name == PARAM_BT:
if parameter.name == BT_PARAM:
self.bt_path = parameter.value
# self.get_logger().info(f'$ {parameter.name}={parameter.value}')
# self.get_logger().info(f'$ {parameter.name}={parameter.value}')
return SetParametersResult(successful=True)
# def get_remote_parameter(self, remote_node_name, param_name):
# cli = self.create_client(GetParameters, remote_node_name + '/get_parameters')
# while not cli.wait_for_service(timeout_sec=1.0):
@ -85,8 +94,8 @@ class rbsInterface(Node):
future = self.cli.call_async(req)
self.executor.spin_until_future_complete(future)
res = future.result()
res = future.result()
# self.get_logger().info(f"{res}")
return res.results[0].successful
def _deserialize(self, file_path: str, sid: str):
@ -109,18 +118,11 @@ class rbsInterface(Node):
def run_action(self, command_data: dict) -> bool:
p_list = command_data["param"]
oper_type = command_data["type"]
node_name = self.cfg_data["Launch"]["name"] #["ROS2"]["node_name"]
node_name = self.cfg_data["Module"]["node_name"]
par_name = node_name + PARAM_SUFFIX
if len(p_list) > 0:
# ext = command_data["format"] # 'yaml' or 'json'
# param_file = os.path.join(self.get_transfer_path(), command_data["name"]+"."+ext)
# with open(param_file, "r") as f:
# data = f.read()
# if not self.set_remote_parameter(node_name, par_name, data):
# return False
# if not self.set_remote_parameter(node_name, par_name, yaml.dump(self.cfg_data)):
data = json.dumps(self.cfg_data)
# self.get_logger().info(f"{data}")
if not self.set_remote_parameter(node_name, par_name, data):
return False
@ -165,19 +167,53 @@ class rbsInterface(Node):
res = future.result()
if res: # is not None:
ret = res.success
elif oper_type == "action":
ret = True
return ret
def run_condition(self, command_data: dict) -> bool:
node_name = self.cfg_data["Module"]["node_name"]
self.cli = self.create_client(RbsBt, node_name + CONDITION_SRV_NAME)
while not self.cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info("'" + node_name + "' service not available, waiting again...")
req = RbsBt.Request()
req.command = command_data["name"]
future = self.cli.call_async(req)
self.executor.spin_until_future_complete(future)
res = future.result()
return res.ok
def _interface(self, sid: str, action: str, command: str, isCondition: bool) -> bool:
self.get_logger().info(f"Incoming request ({action}/{command})")
self.cfg_data = self._load_config(sid)
typeBTnode = "Condition" if isCondition else "Action"
self.get_logger().info(f'{typeBTnode}: Ok ({self.cfg_data["Module"]["description"]})')
is_success = False
for comm in self.cfg_data[KEY_BTPARAM]:
if comm["name"] == command:
is_success = self.run_condition(comm) if isCondition else self.run_action(comm)
return is_success
def action_callback(self, goal_h):
goal = goal_h.request
is_success = self._interface(goal.sid, goal.action, goal.command, isCondition=False)
res = RbsBtAction.Result()
res.ok = is_success
if is_success:
goal_h.succeed()
else:
goal_h.abort()
return res
def service_callback(self, request, response):
self.get_logger().info(f"Incoming request for Action ({request.action}/{request.command})")
self.cfg_data = self._load_config(request.sid) #, request.command)
self.get_logger().info(f'Config: Ok ({self.cfg_data["Module"]["description"]})')
is_action = False
for comm in self.cfg_data["BTAction"]:
if comm["name"] == request.command:
is_action = self.run_action(comm)
response.ok = is_action #True
response.ok = self._interface(request.sid, request.action, request.command, isCondition=True)
return response
def main():

201
rbs_interface/src/BTExec.cpp Normal file
View file

@ -0,0 +1,201 @@
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include <string>
#include <fstream>
#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"
#define STR_ACTION "do"
#define STR_SID "sid"
#define STR_COMMAND "command"
#define NODE_NAME "rbs_interface"
#define SERVICE_NAME "rbs_interface_s"
#define SERVER_NAME "rbs_interface_a"
#define FILE_BT "bt.xml"
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 RbsConditionSrv = rbs_skill_interfaces::srv::RbsBt;
using RbsActionSrv = rbs_skill_interfaces::action::RbsBt;
class RbsAction : public RosActionNode<RbsActionSrv> {
public:
RbsAction(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
: RosActionNode<RbsActionSrv>(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 RbsAction: " + 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;
};
// **********************************************************
class RbsCondition : public RosServiceNode<RbsConditionSrv> {
public:
RbsCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
: RosServiceNode<RbsConditionSrv>(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 RbsCondition: " + 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;
};
// class RbsCondition : public RosTopicSubNode {
// }
// Simple function that return a NodeStatus
// BT::NodeStatus TestIf()
// {
// std::string m_if = "if"; //getInput<std::string>("if").value();
// // std::cout << "[ Test: OK ]" << std::endl;
// std::stringstream ss;
// ss << "[ Test: OK ] " << m_if;
// std::string str = ss.str();
// std::cout << str << std::endl;
// return BT::NodeStatus::SUCCESS;
// }
int main(int argc, char** argv)
{
// filename Behavior Tree
std::string bt = std::string(argv[1]) + "/" FILE_BT;
std::ifstream fh(bt, std::ios::in);
// reading xml
std::string xml{std::istreambuf_iterator<char>(fh), std::istreambuf_iterator<char>()};
rclcpp::init(argc, argv);
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), xml.c_str());
auto nh = std::make_shared<rclcpp::Node>("bt_exec"); //"_action");
// auto nh2 = std::make_shared<rclcpp::Node>("bt_exe_condition");
RosNodeParams params; //,params2;
params.nh = nh;
params.default_port_value = SERVICE_NAME;
params.server_timeout = 3s;
BehaviorTreeFactory factory;
factory.registerNodeType<RbsCondition>("Is", params);
// params.nh = nh;
params.default_port_value = SERVER_NAME;
// params.server_timeout = 3s;
factory.registerNodeType<RbsAction>("RbsAction", params);
// factory.registerSimpleCondition("Is", [&](TreeNode&) { return TestIf(); });
// factory.registerRosAction("Is", params);
auto tree = factory.createTreeFromText(xml);
// while(rclcpp::ok())
if(rclcpp::ok())
{
tree.tickWhileRunning();
}
rclcpp::shutdown();
return 0;
}