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

interface node synchronized + readme for perception

Browse source
This commit is contained in:
shalenikol 2024-12-05 20:15:37 +03:00 • committed by Bill Finger
parent b382148b74
commit 988800abc0
3 changed files with 328 additions and 320 deletions
Download patch file Download diff file Expand all files Collapse all files

32
rbs_bt_executor/scripts/rbs_interface.py
View file

@ -5,13 +5,13 @@
@shalenikol release 0.1
@shalenikol release 0.2 BT v.4
@shalenikol release 0.3 synchronize
"""
import os
import json
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
@ -19,14 +19,15 @@ 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, State
from lifecycle_msgs.srv import ChangeState
from lifecycle_msgs.msg import Transition
from rbs_skill_interfaces.srv import RbsBt
from rbs_skill_interfaces.action import RbsBt as RbsBtAction
# from rclpy.parameter_client import AsyncParameterClient # only Iron
CONDITION_SRV_NAME = "/condition"
COMPLETION_SRV_NAME = "/completion"
BT_PARAM = "bt_path"
NODE_NAME = "rbs_interface"
SERVICE_NAME = "rbs_interface_s"
@ -40,7 +41,6 @@ class rbsInterface(Node):
"""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(BT_PARAM, rclpy.Parameter.Type.STRING)
self.cb_group = ReentrantCallbackGroup()
@ -164,18 +164,23 @@ class rbsInterface(Node):
ret = res.success
elif oper_type == "action":
# cnt = 0
while not self.run_condition(COMPLETION_SRV_NAME, command_data["name"]):
pass
# cnt += 1
# self.get_logger().info(f"action + run_condition: {cnt}")
ret = True
return ret
def run_condition(self, command_data: dict) -> bool:
node_name = self.cfg_data["Module"]["node_name"]
def run_condition(self, srv_name: str, command: str) -> bool:
srv_topic = self.cfg_data["Module"]["node_name"] + srv_name
self.cli = self.create_client(RbsBt, node_name + CONDITION_SRV_NAME)
self.cli = self.create_client(RbsBt, srv_topic)
while not self.cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info("'" + node_name + "' service not available, waiting again...")
self.get_logger().info(f"'{srv_topic}' service not available, waiting again...")
req = RbsBt.Request()
req.command = command_data["name"]
req.command = command
future = self.cli.call_async(req)
self.executor.spin_until_future_complete(future)
@ -183,15 +188,14 @@ class rbsInterface(Node):
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"]})')
self.get_logger().info(f'Incoming request ({action}/{command}) {typeBTnode} ({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)
is_success = self.run_condition(CONDITION_SRV_NAME, command) if isCondition else self.run_action(comm)
return is_success
@ -213,8 +217,8 @@ class rbsInterface(Node):
def main():
rclpy.init()
# executor = rclpy.executors.SingleThreadedExecutor()
executor = rclpy.executors.MultiThreadedExecutor()
executor = rclpy.executors.SingleThreadedExecutor()
# executor = rclpy.executors.MultiThreadedExecutor() # can't be used
i_node = rbsInterface(NODE_NAME)
executor.add_node(i_node)
try: