BT interface node works with web-service
This commit is contained in:
parent
47c5e89913
commit
49e036af5e
15 changed files with 536 additions and 124 deletions
|
@ -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" />
|
||||
|
|
|
@ -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
59
rbs_bt_executor/launch/rbs_bt_web.launch.py
Normal 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)])
|
|
@ -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
53
rbs_interface/CMakeLists.txt
Normal 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
17
rbs_interface/config/interface.json
Normal 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
69
rbs_interface/launch/interface.launch.py
Normal 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
27
rbs_interface/package.xml
Normal 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
0
rbs_interface/rbs_interface/__init__.py
Normal file
64
rbs_interface/scripts/bt_param.py
Executable file
64
rbs_interface/scripts/bt_param.py
Executable 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
195
rbs_interface/scripts/rbs_interface.py
Executable 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()
|
|
@ -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": []
|
||||
}
|
|
@ -1,3 +0,0 @@
|
|||
object_name: fork
|
||||
weights_file: /home/shalenikol/robossembler_ws/fork_e47.pth
|
||||
dimensions: [0.137, 0.165, 0.202]
|
|
@ -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,3 +1,4 @@
|
|||
string sid
|
||||
string action
|
||||
string command
|
||||
---
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue