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

BT interface node works with web-service

Browse source
This commit is contained in:
shalenikol 2024-05-27 08:35:27 +00:00 • committed by Igor Brylyov
parent 47c5e89913
commit 49e036af5e
15 changed files with 536 additions and 124 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rbs_bt_executor/bt_trees/bt_pe_stop.xml
View file

@ -2,12 +2,13 @@
<root main_tree_to_execute="Main">
<BehaviorTree ID="Main">
<Sequence>
<Action ID="RbsBtAction" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
<Action ID="RbsBtAction" sid="124" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="RbsBtAction">
<input_port name="sid" />
<input_port name="do" />
<input_port name="command" />
<input_port name="server_name" />

3
rbs_bt_executor/bt_trees/bt_pe_test.xml
View file

@ -2,12 +2,13 @@
<root main_tree_to_execute="Main">
<BehaviorTree ID="Main">
<Sequence>
<Action ID="RbsBtAction" do="PoseEstimation" command="peConfigure" server_name="rbs_interface" server_timeout="1000" />
<Action ID="RbsBtAction" sid="123" do="PoseEstimation" command="peConfigure" server_name="rbs_interface" server_timeout="1000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="RbsBtAction">
<input_port name="sid" />
<input_port name="do" />
<input_port name="command" />
<input_port name="server_name" />

59
rbs_bt_executor/launch/rbs_bt_web.launch.py Normal file
View file

@ -0,0 +1,59 @@
"""
rbs_bt_web
ROS 2 launch program for Robossembler
@shalenikol release 0.1
"""
import os
# import json
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
from launch.substitutions import LaunchConfiguration
from launch.event_handlers import OnExecutionComplete
FILE_BT = "bt.xml"
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
bt_path = LaunchConfiguration("bt_path")
bt_path = bt_path.perform(context)
rbs_bt = Node(
package = "behavior_tree",
executable = "bt_engine",
parameters = [
{"bt_file_path": os.path.join(bt_path, FILE_BT)},
{"plugins": ["rbs_interface"]}
]
)
bt_param = Node(
package="rbs_interface",
executable="bt_param.py",
parameters = [{"bt_path": bt_path}]
)
# nodes_to_start = [rbs_bt]
# return nodes_to_start
# return [bt_param]
return [
RegisterEventHandler(
event_handler=OnExecutionComplete(
target_action=bt_param,
on_completion=[rbs_bt],
)
),
bt_param
] # the order of elements is strictly required
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"bt_path",
default_value="''",
description="path to Behavior tree instance"
)
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

46
rbs_bt_executor/src/rbsBTAction.cpp
View file

@ -1,9 +1,11 @@
// #include "behavior_tree/BtAction.hpp"
#include "behavior_tree/BtService.hpp"
// #include "rbs_skill_interfaces/action/rbs_bt.hpp"
#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
// using RbsBtActionSrv = rbs_skill_interfaces::action::RbsBt;
#define STR_ACTION "do"
#define STR_SID "sid"
#define STR_COMMAND "command"
#define NODE_NAME "rbs_interface"
using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt;
class RbsBtAction : public BtService<RbsBtActionSrv> {
@ -11,45 +13,30 @@ public:
RbsBtAction(const std::string &name, const BT::NodeConfiguration &config)
: BtService<RbsBtActionSrv>(name, config) {
_action_name = getInput<std::string>("do").value();
_action_name = getInput<std::string>(STR_ACTION).value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name);
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "rbs_interface");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, NODE_NAME);
while (!_set_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(), "Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(), "'rbs_interface' service not available, waiting again...");
RCLCPP_WARN(_node->get_logger(), NODE_NAME " service not available, waiting again...");
}
// set_str_param();
}
// RbsBtActionSrv::Goal populate_goal() override {
// auto goal = RbsBtActionSrv::Goal();
// goal.action = _action_name;
// goal.command = getInput<std::string>("command").value();
// RCLCPP_INFO_STREAM(_node->get_logger(), "Goal send " + _action_name);
// return goal;
// }
RbsBtActionSrv::Request::SharedPtr populate_request() override {
auto request = std::make_shared<RbsBtActionSrv::Request>();
request->action = _action_name;
request->command = getInput<std::string>("command").value();
// RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:populate_request");
request->sid = getInput<std::string>(STR_SID).value();
request->command = getInput<std::string>(STR_COMMAND).value();
return request;
}
BT::NodeStatus handle_response(const RbsBtActionSrv::Response::SharedPtr response) override {
if (response->ok) {
// TODO We need better perfomance for call it in another place for all BT nodes
// - Make it via shared_ptr forward throught blackboard
// auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
// auto p = t->getWorkspaceInspectorTrajectory();
// setOutput("workspace", p);
// RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:handle_response");
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
@ -57,22 +44,15 @@ public:
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("do"),
BT::InputPort<std::string>("command")
BT::InputPort<std::string>(STR_SID),
BT::InputPort<std::string>(STR_ACTION),
BT::InputPort<std::string>(STR_COMMAND)
});
}
private:
std::string _action_name{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
// void set_str_param() {
// RCLCPP_INFO_STREAM(_node->get_logger(),"Set string parameter: <" + _action_name + ">");
// std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("action_name", _action_name)};
// _set_params_client->set_parameters(_parameters);
// }
// auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
};
#include "behaviortree_cpp_v3/bt_factory.h"

53
rbs_interface/CMakeLists.txt Normal file
View file

@ -0,0 +1,53 @@
cmake_minimum_required(VERSION 3.8)
project(rbs_interface)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclpy REQUIRED)
find_package(rclcpp REQUIRED)
find_package(image_transport REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
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(visualization_msgs REQUIRED)
# Install Python modules
ament_python_install_package(${PROJECT_NAME})
# Install Python executables
install(PROGRAMS
scripts/rbs_interface.py
scripts/bt_param.py
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

17
rbs_interface/config/interface.json Normal file
View file

@ -0,0 +1,17 @@
{
"SkillPackage": {
"name": "Robossembler", "version": "1.0", "format": "1"
},
"Module": {
"name": "Interface", "description": "Interface node for the Robossembler skills"
},
"Launch": {
"package": "rbs_interface",
"executable": "rbs_interface.py",
"param": ["bt_path"]
},
"ROS2": {
"node_name": "rbs_interface", "comment": "!!! no change !!!"
},
"Settings": []
}

69
rbs_interface/launch/interface.launch.py Normal file
View file

@ -0,0 +1,69 @@
"""
Launching interface node with connecting skills
ROS 2 launch program for Robossembler
@shalenikol release 0.1
"""
import os
import json
from launch_ros.actions import Node
from launch.actions import (DeclareLaunchArgument, OpaqueFunction)
from launch.substitutions import LaunchConfiguration
from launch import LaunchDescription
FILE_SKILLS = "skills.json"
PARAM_SUFFIX = "_cfg"
def get_skill_list_(path: str) -> list:
f = os.path.join(path, FILE_SKILLS)
if not os.path.isfile(f):
return []
with open(f, "r") as fh:
data = json.load(fh)
# str_data = fh.read()
# data = json.loads(str_data)
nn_skills = 0
excluding = {}
skills = []
for skill in data["skills"]:
launch = skill["Launch"]
p = launch["package"]
e = launch["executable"]
excluding[p+e] = None # unique key
if len(excluding) == nn_skills:
continue
nn_skills += 1
# skills.append(Node(package=p, executable=e))
launch["parameters"] = [{launch["name"]+PARAM_SUFFIX: json.dumps(skill)}]
skills.append(Node(**launch))
return skills
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
bt_path = LaunchConfiguration("bt_path")
bt_path = bt_path.perform(context)
skills = get_skill_list_(bt_path)
rbs_interface = Node(
package="rbs_interface",
executable="rbs_interface.py",
# parameters = [{"bt_path": bt_path}] # can be included as default path
)
nodes_to_start = [rbs_interface]
return nodes_to_start + skills
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"bt_path",
default_value="''",
description="path to Behavior tree instance"
)
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

27
rbs_interface/package.xml Normal file
View file

@ -0,0 +1,27 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rbs_interface</name>
<version>0.0.1</version>
<description>The interface node</description>
<maintainer email="shaniks77s@gmail.com">shalenikol</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>
<depend>rclpy</depend>
<depend>rclcpp</depend>
<depend>image_transport</depend>
<depend>cv_bridge</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>rbs_skill_interfaces</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

0
rbs_interface/rbs_interface/__init__.py Normal file
View file

64
rbs_interface/scripts/bt_param.py Executable file
View file

@ -0,0 +1,64 @@
#!/usr/bin/env python3
"""
btParam_node only for passing the path to the interface node
ROS 2 program for Robossembler
@shalenikol release 0.1
"""
# import os
import rclpy
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.parameter import Parameter
from rcl_interfaces.srv import SetParameters
NODE_NAME = "bt_param"
NODE_INTERFACE = "rbs_interface"
PARAM_BT = "bt_path"
class btParam(Node):
def __init__(self, node_name):
"""Construct the node."""
super().__init__(node_name)
self.declare_parameter(PARAM_BT, rclpy.Parameter.Type.STRING)
self.cb_group = ReentrantCallbackGroup()
timer_period = 0.1 # seconds
self._timer = self.create_timer(timer_period, self.timer_callback, self.cb_group)
def timer_callback(self):
bt_path = self.get_parameter(PARAM_BT).get_parameter_value().string_value
if not self.set_remote_parameter(NODE_INTERFACE, PARAM_BT, bt_path):
# self.get_logger().info(f"'{NODE_NAME}' set param '{bt_path}'")
# else:
self.get_logger().info("Error 'set_remote_parameter'")
self.destroy_timer(self._timer)
def set_remote_parameter(self, remote_node_name: str, parameter_name: str, new_parameter_value) -> bool:
self.cli = self.create_client(SetParameters, remote_node_name + "/set_parameters")
while not self.cli.wait_for_service(timeout_sec=1.1):
self.get_logger().info("'" + remote_node_name + "' service not available, waiting again...")
req = SetParameters.Request()
req.parameters = [Parameter(parameter_name, value=new_parameter_value).to_parameter_msg()]
future = self.cli.call_async(req)
self.executor.spin_until_future_complete(future)
res = future.result()
return res.results[0].successful
def main():
rclpy.init()
executor = rclpy.executors.SingleThreadedExecutor()
# executor = rclpy.executors.MultiThreadedExecutor()
i_node = btParam(NODE_NAME)
executor.add_node(i_node)
try:
executor.spin_once()
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
i_node.destroy_node()
if __name__ == '__main__':
main()

195
rbs_interface/scripts/rbs_interface.py Executable file
View file

@ -0,0 +1,195 @@
#!/usr/bin/env python3
"""
rbsInterface_node
ROS 2 program for Robossembler
@shalenikol release 0.1
"""
import os
import json
# import yaml
import rclpy
from rclpy.node import Node
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 rbs_skill_interfaces.srv import RbsBt
# from rclpy.parameter_client import AsyncParameterClient # only Iron
# PARAM_NAME = "str_param"
# PARAM_SKILL_CONFIG = "skill_cfg"
PARAM_BT = "bt_path"
NODE_NAME = "rbs_interface"
FILE_SKILLS = "skills.json"
PARAM_SUFFIX = "_cfg"
# def get_transfer_path_():
# return os.path.join(get_package_share_directory("rbs_interface"), "config")
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
super().__init__(node_name)
self.declare_parameter(PARAM_BT, rclpy.Parameter.Type.STRING)
self.cb_group = ReentrantCallbackGroup()
self._service = self.create_service(RbsBt, node_name, self.service_callback, 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)
def get_transfer_path(self):
if self.bt_path:
return self.bt_path
return os.path.join(get_package_share_directory("rbs_interface"), "config")
def _on_set_btpath_param(self, parameter_list):
for parameter in parameter_list:
if parameter.name == PARAM_BT:
self.bt_path = 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):
# self.get_logger().info('service not available, waiting again...')
# req = GetParameters.Request()
# req.names = [param_name]
# future = cli.call_async(req)
# while rclpy.ok():
# rclpy.spin_once(self)
# if future.done():
# try:
# res = future.result()
# return getattr(res.values[0], self.type_arr[res.values[0].type])
# except Exception as e:
# self.get_logger().warn('Service call failed %r' % (e,))
# break
def set_remote_parameter(self, remote_node_name: str, parameter_name: str, new_parameter_value) -> bool:
self.cli = self.create_client(SetParameters, remote_node_name + "/set_parameters")
while not self.cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info("'" + remote_node_name + "' service not available, waiting again...")
req = SetParameters.Request()
req.parameters = [Parameter(parameter_name, value=new_parameter_value).to_parameter_msg()]
future = self.cli.call_async(req)
self.executor.spin_until_future_complete(future)
res = future.result()
return res.results[0].successful
def _deserialize(self, file_path: str, sid: str):
with open(file_path, "r") as f:
# if file_path.split() == ".yaml":
# s = yaml.load(f, Loader=yaml.FullLoader)
# else: # ".json"
data = json.load(f)
for skill in data["skills"]:
if skill["sid"] == sid:
return skill
assert False, f"Error: sid not valid '{sid}'"
# return {"result": f"Error: sid not valid '{sid}'"}
def _load_config(self, sid: str):
p = os.path.join(self.get_transfer_path(), FILE_SKILLS) # action+".json")
# load config
return self._deserialize(p,sid)
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"]
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
ret = False # default return value
if oper_type == "run":
self.cli_changestate = self.create_client(ChangeState, f"/{node_name}/change_state") #, callback_group=self.cb_group)
while not self.cli_changestate.wait_for_service(timeout_sec=1.0):
self.get_logger().info(f"'{node_name}' not available... wait")
req = ChangeState.Request()
req.transition.id = Transition.TRANSITION_CONFIGURE
future = self.cli_changestate.call_async(req)
self.executor.spin_until_future_complete(future)
res = future.result()
if res: # is not None:
if res.success:
req = ChangeState.Request()
req.transition.id = Transition.TRANSITION_ACTIVATE
future = self.cli_changestate.call_async(req)
self.executor.spin_until_future_complete(future)
res = future.result()
if res: # is not None:
ret = res.success
elif oper_type == "stop":
self.cli_changestate = self.create_client(ChangeState, f"{node_name}/change_state") #, callback_group=self.cb_group)
while not self.cli_changestate.wait_for_service(timeout_sec=1.0):
self.get_logger().info(f"'{node_name}' not available... wait")
req = ChangeState.Request()
req.transition.id = Transition.TRANSITION_DEACTIVATE
future = self.cli_changestate.call_async(req)
self.executor.spin_until_future_complete(future)
res = future.result()
if res: # is not None:
if res.success:
req = ChangeState.Request()
req.transition.id = Transition.TRANSITION_CLEANUP
future = self.cli_changestate.call_async(req)
self.executor.spin_until_future_complete(future)
res = future.result()
if res: # is not None:
ret = res.success
return ret
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
return response
def main():
rclpy.init()
# executor = rclpy.executors.SingleThreadedExecutor()
executor = rclpy.executors.MultiThreadedExecutor()
i_node = rbsInterface(NODE_NAME)
executor.add_node(i_node)
try:
executor.spin()
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
i_node.destroy_node()
if __name__ == '__main__':
main()

47
rbs_perception/config/PoseEstimation.json
View file

@ -1,47 +0,0 @@
{
"SkillPackage": {
"name": "Robossembler", "version": "1.0", "format": "1"
},
"Module": {
"name": "PoseEstimation", "description": "Pose Estimation skill with DOPE"
},
"Launch": {
"executable": "pe_dope_lc.py"
},
"ROS2": {
"node_name": "lc_dope"
},
"BTAction": [
{
"name": "peConfigure",
"format": "yaml",
"type": "run",
"param":["object_name","weights_file"],
"result":["Pose"]
},
{
"name": "peStop",
"format": "yaml",
"type": "stop",
"param":[],
"result":[]
}
],
"Interface": {
"Input": [
{
"name": "cameraLink", "type": "CAMERA"
},
{
"name": "object_name", "type": "MODEL"
}
],
"Output":
[
{
"name": "pose_estimation_topic", "type": "Pose"
}
]
},
"Settings": []
}

3
rbs_perception/config/peConfigure.yaml
View file

@ -1,3 +0,0 @@
object_name: fork
weights_file: /home/shalenikol/robossembler_ws/fork_e47.pth
dimensions: [0.137, 0.165, 0.202]

73
rbs_perception/scripts/pe_dope_lc.py
View file

@ -27,32 +27,34 @@ from tf2_ros.buffer import Buffer
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
import cv2 # OpenCV library
FILE_SKILL_CONFIG = "PoseEstimation.json" # используется при запуске ноды и при запуске BT с этим навыком
FILE_DOPE_CONFIG = "pe_dope_config.yaml"
# FILE_TEMP_IMAGE = "image_rgb.png"
PARAM_NAME = "str_param"
PARAM_SKILL_CONFIG = "skill_cfg"
CAMERA_LINK_DEFAULT = "outer_rgbd_camera"
# NODE_NAME_DEFAULT = "lc_dope"
NODE_NAME_DEFAULT = "lc_dope" # the name doesn't matter in this node (defined in Launch)
PARAM_SKILL_CFG = "lc_dope_cfg"
# PARAM_SUFFIX = "_cfg"
# node_name = self.cfg_data["Launch"]["name"]
# par_name = node_name + PARAM_SUFFIX
def get_transfer_path_() -> str:
return os.path.join(get_package_share_directory("rbs_perception"), "config")
class PE_DOPE(Node):
"""Pose estimation lifecycle node with DOPE."""
def __init__(self, node_name="", **kwargs):
def __init__(self, **kwargs):
"""Construct the node."""
self.skill_cfg = self._load_config()
ros_cfg = self.skill_cfg["ROS2"]
self.nodeName = ros_cfg["node_name"] #node_name
out_par = self.skill_cfg["Interface"]["Output"][0]
self.topicSrv = self.nodeName + "/" + out_par["name"]
# for other nodes
kwargs["allow_undeclared_parameters"] = True
kwargs["automatically_declare_parameters_from_overrides"] = True
super().__init__(self.nodeName, **kwargs)
self.declare_parameter(PARAM_NAME, rclpy.Parameter.Type.STRING)
self.declare_parameter(PARAM_SKILL_CONFIG, rclpy.Parameter.Type.STRING)
super().__init__(NODE_NAME_DEFAULT, **kwargs)
str_cfg = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value
self.skill_cfg = json.loads(str_cfg)
self.nodeName = self.skill_cfg["Launch"]["name"]
out_par = self.skill_cfg["Interface"]["Output"][0]
self.topicSrv = self.nodeName + "/" + out_par["name"]
# Used to convert between ROS and OpenCV images
self.br = CvBridge()
self.dope_cfg = self._load_config_DOPE()
@ -71,6 +73,12 @@ class PE_DOPE(Node):
self._image_cnt = 0
self._K = []
def _load_config_DOPE(self):
p = os.path.join(get_transfer_path_(), FILE_DOPE_CONFIG)
with open(p, "r") as f:
y = yaml.load(f, Loader=yaml.FullLoader)
return y
def _set_camera_topic(self, camera_link: str):
""" Service for camera name topics """
self.topicImage = "/" + camera_link + "/image"
@ -92,18 +100,6 @@ class PE_DOPE(Node):
# set the indicator for receiving the camera info
self._is_camerainfo = True
def _load_config(self):
p = os.path.join(get_transfer_path_(), FILE_SKILL_CONFIG)
with open(p, "r") as f:
j = json.load(f)
return j
def _load_config_DOPE(self):
p = os.path.join(get_transfer_path_(), FILE_DOPE_CONFIG)
with open(p, "r") as f:
y = yaml.load(f, Loader=yaml.FullLoader)
return y
def on_configure(self, state: State) -> TransitionCallbackReturn:
"""
Configure the node, after a configuring transition is requested.
@ -114,15 +110,14 @@ class PE_DOPE(Node):
TransitionCallbackReturn.FAILURE transitions to "unconfigured".
TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing"
"""
yaml_param = self.get_parameter(PARAM_NAME).get_parameter_value().string_value
str_param = yaml.load(yaml_param, Loader=yaml.FullLoader)
# !!! вместо PARAM_SKILL_CONFIG можно использовать self.skill_cfg !!! (это одно и то же)
# yaml_param = self.get_parameter(PARAM_SKILL_CONFIG).get_parameter_value().string_value
# pe_cfg = yaml.load(yaml_param, Loader=yaml.FullLoader)
# with open("pe.yaml", "w") as f:
# f.write(yaml_param)
# print(skill_cfg)
json_param = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value
jdata = json.loads(json_param)
dependency = {}
for comm in jdata["BTAction"]:
for par in comm["param"]:
if par["type"] == "weights":
dependency = par["dependency"]
assert dependency, "no dependency"
# Create the subscribers.
self._sub_info = self.create_subscription(CameraInfo, self.topicCameraInfo, self.listener_camera_info, 2)
@ -130,16 +125,16 @@ class PE_DOPE(Node):
self._pub = self.create_lifecycle_publisher(Pose, self.topicSrv, 1)
# Load model weights
w = str_param["weights_file"]
w = dependency["weights_file"]
if not os.path.isfile(w):
self.get_logger().warning(f"No weights found <{w}>")
return TransitionCallbackReturn.FAILURE
obj = str_param["object_name"]
dim = str_param["dimensions"]
obj = dependency["object_name"]
dim = dependency["dimensions"]
self.dope_node = Dope(self.dope_cfg, w, obj, dim)
self.get_logger().info(f"'{self.nodeName}' configure is success (with '{obj}')")
self.get_logger().info(f"configure is success (with '{obj}')")
return TransitionCallbackReturn.SUCCESS
def on_activate(self, state: State) -> TransitionCallbackReturn:
@ -400,7 +395,7 @@ def main():
executor = rclpy.executors.SingleThreadedExecutor()
# executor = rclpy.executors.MultiThreadedExecutor()
lc_node = PE_DOPE() #NODE_NAME_DEFAULT)
lc_node = PE_DOPE()
executor.add_node(lc_node)
try:
executor.spin()

1
rbs_skill_interfaces/srv/RbsBt.srv
View file

@ -1,3 +1,4 @@
string sid
string action
string command
---