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
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()
|
||||
Loading…
Add table
Add a link
Reference in a new issue