diff --git a/README.md b/README.md index a26ff18..7da843b 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ mkdir -p roboy_ws/src cd roboy_ws/src git clone https://github.com/Roboy/roboy3_models.git -b bullet git clone https://github.com/Roboy/roboy_communication.git -b dashing -git clone https://github.com/Roboy/bulletroboy.git +git clone https://github.com/Roboy/bulletroboy.git -b exoforce # Building packages in workspace cd roboy_ws @@ -34,10 +34,7 @@ Run the system with the operator simulation. ros2 launch bulletroboy cage_sim.launch.py # use -s to list arguments ``` -Run the system with the real exoforce cage. -(Each command must be executed in a new terminal. Operator and exoforce nodes require interaction through the terminal and cannot be added to the launch file yet.) +Run the system with the real exoforce cage. ```bash ros2 launch bulletroboy exoforce.launch.py # use -s to list arguments -ros2 run bulletroboy operator -ros2 run bulletroboy exoforce ``` diff --git a/bulletroboy/exoforce/exoforce.py b/bulletroboy/exoforce/exoforce.py index c19923c..4f1775f 100644 --- a/bulletroboy/exoforce/exoforce.py +++ b/bulletroboy/exoforce/exoforce.py @@ -7,10 +7,9 @@ from xml.dom import minidom from rclpy.node import Node -from roboy_control_msgs.msg import CageState, EndEffector as EndEffectorMsg, ViaPoint as ViaPointMsg, MuscleUnit as MuscleUnitMsg +from roboy_control_msgs.msg import EFPose, CageState, EndEffector as EndEffectorMsg, ViaPoint as ViaPointMsg, MuscleUnit as MuscleUnitMsg from roboy_control_msgs.srv import GetCageEndEffectors from roboy_simulation_msgs.msg import Collision -from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import Point from ..utils.force_decomposition import decompose_force_ef_to_tendons @@ -369,7 +368,7 @@ def __init__(self, cage_conf, node_name): self.cage_state_publisher = self.create_publisher(CageState, Topics.CAGE_STATE, 1) self.initial_conf_service = self.create_service(GetCageEndEffectors, Topics.CAGE_END_EFFECTORS, self.get_end_effectors_callback) - self.create_subscription(PoseStamped, Topics.OP_EF_POSES, self.operator_ef_pos_listener, 1) + self.create_subscription(EFPose, Topics.OP_EF_POSES, self.operator_ef_pos_listener, 1) def init_force_decomp_params(self): """Reads force decomposition params. @@ -528,21 +527,21 @@ def operator_ef_pos_listener(self, ef_pose): """Callback of the pose subscriber. Sets the pose of the end effector given in the msg. Args: - ef_pose: received PoseStamped msg. + ef_pose: received EFPose msg. Returns: - """ - ef_name = ef_pose.header.frame_id + ef_name = ef_pose.ef_name #self.get_logger().info("Received pose for " + ef_name) end_effector = self.get_ef_name(ef_name) if end_effector is None: self.get_logger().warn(ef_name + " is not an end effector!") return - link_pos = np.array([ef_pose.pose.position.x, ef_pose.pose.position.y, ef_pose.pose.position.z]) - link_orn = np.array([ef_pose.pose.orientation.x, ef_pose.pose.orientation.y, ef_pose.pose.orientation.z, ef_pose.pose.orientation.w]) + link_pos = np.array([ef_pose.ef_pose.position.x, ef_pose.ef_pose.position.y, ef_pose.ef_pose.position.z]) + link_orn = np.array([ef_pose.ef_pose.orientation.x, ef_pose.ef_pose.orientation.y, ef_pose.ef_pose.orientation.z, ef_pose.ef_pose.orientation.w]) end_effector.position = link_pos end_effector.orientation = link_orn diff --git a/bulletroboy/exoforce/exoforce_hardware.py b/bulletroboy/exoforce/exoforce_hardware.py index 97d47e3..d3d6358 100644 --- a/bulletroboy/exoforce/exoforce_hardware.py +++ b/bulletroboy/exoforce/exoforce_hardware.py @@ -1,9 +1,12 @@ import time +from functools import partial import numpy as np -from geometry_msgs.msg import PoseStamped from pyquaternion import Quaternion +from roboy_middleware_msgs.msg import ExoforceResponse, InitExoforceRequest +from roboy_middleware_msgs.srv import InitExoforce from roboy_simulation_msgs.msg import Collision, TendonUpdate +from std_msgs.msg import Empty from std_srvs.srv import Trigger from ..utils.utils import Services, Topics, call_service_async @@ -27,42 +30,91 @@ def __init__(self, cage_conf): ('max_force_apply_time', 1.0)] ) + # state variables + self.active = False + self.processing_flag = False + + # Initial parameters self.no_slack_force = self.get_parameter("no_slack_force").get_parameter_value().double_value self.tendon_init_time = self.get_parameter("tendon_init_time").get_parameter_value().double_value self.max_force_apply_time = self.get_parameter("max_force_apply_time").get_parameter_value().double_value - self.active = False + # Force timer self.apply_min_force_timer = self.create_timer(self.max_force_apply_time, lambda: self.set_target_force(self.no_slack_force)) self.apply_min_force_timer.cancel() + # Collision subscription self.create_subscription(Collision, Topics.MAPPED_COLLISIONS, self.collision_listener, 1) self.create_subscription(Collision, 'roboy/simulation/roboy/collision_hw', self.collision_listener, 1) - + + # Target force publisher self.target_force_publisher = self.create_publisher(TendonUpdate, Topics.TARGET_FORCE, 1) + # Force Control node start/stop clients self.start_force_control_client = self.create_client(Trigger, Services.START_FORCE_CONTROL) self.stop_force_control_client = self.create_client(Trigger, Services.STOP_FORCE_CONTROL) - self.start_exoforce() + # Init operator client + self.init_operator_client = self.create_client(InitExoforce, Services.INIT_OPERATOR) - def start_exoforce(self): - """Starts exoforce. + # Start exoforce topics + self.create_subscription(InitExoforceRequest, Topics.INIT_EXOFORCE_REQ, self.start_exoforce, 1) + self.init_response_publisher = self.create_publisher(ExoforceResponse, Topics.INIT_EXOFORCE_RES, 1) + + # Stop exoforce topics + self.create_subscription(Empty, Topics.STOP_EXOFORCE_REQ, self.stop_exoforce, 1) + self.stop_response_publisher = self.create_publisher(ExoforceResponse, Topics.STOP_EXOFORCE_RES, 1) + + def send_exoforce_response(self, publisher, success, message=""): + """Sends ExoforceResponse msg through response topic. Args: + publisher (Publisher): Response publisher. + success (bool): Success msg data. + message (string): Message msg data. + + Returns: - + """ + msg = ExoforceResponse() + msg.success = success + msg.message = message + publisher.publish(msg) + self.processing_flag = False + + def start_exoforce(self, init_msg): + """Starts exoforce. + + Args: + init_msg (InitExoforceRequest): Initialization message. + Returns: - """ + if self.processing_flag: + return + + self.processing_flag = True self.get_logger().info("Starting Exoforce...") - call_service_async(self.start_force_control_client, Trigger.Request(), self.start_exoforce_callback, self.get_logger()) - def start_exoforce_callback(self, future): + for ef_name in init_msg.ef_name: + if ef_name not in [ef.name for ef in self.end_effectors]: + err_msg = f"{ef_name} is not a cage end effector!" + self.get_logger().error(f"Cannot start the Exoforce: {err_msg}") + self.send_exoforce_response(self.init_response_publisher, False, err_msg) + return + + self.get_logger().info("Requesting Force Control node to start...") + call_service_async(self.start_force_control_client, Trigger.Request(), partial(self.start_exoforce_callback, init_msg), self.get_logger()) + + def start_exoforce_callback(self, init_msg, future): """Start exoforce callback. Args: - - + init_msg (InitExoforceRequest): Initialization message. + future: Service future var. Returns: - @@ -74,16 +126,68 @@ def start_exoforce_callback(self, future): self.get_logger().error(f"{self.start_force_control_client.srv_name} service call failed {e}") else: if not result.success: - self.get_logger().error(f"Failed to start force control node: {result.message}") - self.get_logger().error(f"Failed to start Exoforce!") + err_msg = f"Failed to start force control node: {result.message}" + self.get_logger().error(f"Failed to start Exoforce: {err_msg}") + self.send_exoforce_response(self.init_response_publisher, False, err_msg) else: self.get_logger().info("Force Control node started.") + + # Applying no slack force to all tendons self.set_target_force(self.no_slack_force) + + # Requesting operator node to start + self.init_operator(init_msg) + + # Sending initialization response + self.get_logger().info("Exoforce succesfully started.") + self.send_exoforce_response(self.init_response_publisher, True) + + # Waiting for pulling slack time time.sleep(self.tendon_init_time) + + # Activating exoforce self.active = True - self.get_logger().info("Exoforce succesfully started.") - def stop_exoforce(self): + def init_operator(self, init_msg): + """Inits operator node. + + Args: + init_msg (InitExoforceRequest): Initialization message. + + Returns: + - + + """ + self.get_logger().info("Requesting operator node to initialize...") + + request = InitExoforce.Request(ef_name=init_msg.ef_name, + ef_enabled=init_msg.ef_enabled, + ef_init_pose=init_msg.ef_init_pose, + operator_height=init_msg.operator_height) + + call_service_async(self.init_operator_client, request, self.init_operator_callback, self.get_logger()) + + def init_operator_callback(self, future): + """Init operator callback. + + Args: + future: Service future var. + + Returns: + - + + """ + try: + result = future.result() + except Exception as e: + self.get_logger().error(f"{self.init_operator_client.srv_name} service call failed {e}") + else: + if result.success: + self.get_logger().info("Operator node succesfully initialized.") + else: + self.get_logger().error(f"Operator node could not be initialized: {result.message}") + + def stop_exoforce(self, _): """Stops exoforce. Args: @@ -93,8 +197,15 @@ def stop_exoforce(self): - """ + if self.processing_flag: + return + + self.get_logger().info("Stopping Exoforce...") + self.processing_flag = True self.active = False self.apply_min_force_timer.cancel() + self.set_target_force(0.0) + self.get_logger().info("Stopping Force Control node...") call_service_async(self.stop_force_control_client, Trigger.Request(), self.stop_force_control_callback, self.get_logger()) def stop_force_control_callback(self, future): @@ -113,9 +224,13 @@ def stop_force_control_callback(self, future): self.get_logger().error(f"{self.stop_force_control_client.srv_name} service call failed {e}") else: if not result.success: - self.get_logger().error(f"Failed to stop force control node: {result.message}") + err_msg = f"Failed to stop force control node: {result.message}" + self.get_logger().error(f"Cannot stop Exoforce: {err_msg}") + self.send_exoforce_response(self.stop_response_publisher, False, err_msg) else: self.get_logger().info("Force Control node stopped.") + self.get_logger().info("Exoforce stopped.") + self.send_exoforce_response(self.stop_response_publisher, True) def set_target_force(self, force): """Sets a target force to all the tendons. @@ -162,7 +277,10 @@ def collision_listener(self, collision_msg): self.get_logger().info(f"Received collision: link: {collision_msg.linkid} force: {collision_msg.normalforce}") ef = self.map_link_to_ef(collision_msg.linkid) - if ef.orientation is None: + if ef is None: + self.get_logger().warn(f"Link '{collision_msg.linkid}' has no mapping to ef!") + return + elif ef.orientation is None: self.get_logger().warn(f"Orientation for ef '{ef.name}' is not initialized!") return diff --git a/bulletroboy/nodes/exoforce_node.py b/bulletroboy/nodes/exoforce_node.py index a20ebc6..c3ad5f3 100644 --- a/bulletroboy/nodes/exoforce_node.py +++ b/bulletroboy/nodes/exoforce_node.py @@ -1,20 +1,24 @@ -import os +import os, sys import rclpy from ..exoforce.exoforce import CageConfiguration from ..exoforce.exoforce_hardware import ExoforceHW +from ..utils.utils import parse_launch_arg -CONFIG_DEFAULT_PATH = os.path.dirname(os.path.realpath(__file__)) + "/" + "../../config/realCageConfiguration.xml" +CAGE_CONF_DEFAULT_PATH = os.path.dirname(os.path.realpath(__file__)) + "/" + "../../config/realCageConfiguration.xml" def main(args=None): - rclpy.init(args=args) + try: + rclpy.init(args=args) - # EXOFORCE SETUP - initial_cage_conf = CageConfiguration(CONFIG_DEFAULT_PATH) - exoforce = ExoforceHW(initial_cage_conf) - rclpy.spin(exoforce) + cage_conf_path = parse_launch_arg(sys.argv[1], CAGE_CONF_DEFAULT_PATH, rclpy.logging._root_logger.info) - exoforce.destroy_node() - rclpy.shutdown() + # EXOFORCE SETUP + initial_cage_conf = CageConfiguration(cage_conf_path) + exoforce = ExoforceHW(initial_cage_conf) + rclpy.spin(exoforce) + + except KeyboardInterrupt: + rclpy.shutdown() if __name__ == "__main__": main() diff --git a/bulletroboy/nodes/operator_node.py b/bulletroboy/nodes/operator_node.py index f345e05..f1a6224 100644 --- a/bulletroboy/nodes/operator_node.py +++ b/bulletroboy/nodes/operator_node.py @@ -2,13 +2,14 @@ from ..operator.operator_hardware import OperatorHW def main(args=None): - rclpy.init(args=args) + try: + rclpy.init(args=args) - operator = OperatorHW() - rclpy.spin(operator) + operator = OperatorHW() + rclpy.spin(operator) - operator.destroy_node() - rclpy.shutdown() + except KeyboardInterrupt: + rclpy.shutdown() if __name__ == "__main__": main() diff --git a/bulletroboy/nodes/state_mapper_node.py b/bulletroboy/nodes/state_mapper_node.py index f95848e..c8ebf23 100644 --- a/bulletroboy/nodes/state_mapper_node.py +++ b/bulletroboy/nodes/state_mapper_node.py @@ -3,14 +3,15 @@ from ..state_mapper.state_mapper import StateMapper def main(args=None): - rclpy.init(args=args) + try: + rclpy.init(args=args) - forcesMapper = StateMapper() - executor = MultiThreadedExecutor() - rclpy.spin(forcesMapper, executor) + forcesMapper = StateMapper() + executor = MultiThreadedExecutor() + rclpy.spin(forcesMapper, executor) - forcesMapper.destroy_node() - rclpy.shutdown() + except KeyboardInterrupt: + rclpy.shutdown() if __name__ == '__main__': diff --git a/bulletroboy/operator/operator.py b/bulletroboy/operator/operator.py index 2ed9173..a86bab2 100644 --- a/bulletroboy/operator/operator.py +++ b/bulletroboy/operator/operator.py @@ -1,14 +1,15 @@ from abc import ABC, abstractmethod + import numpy as np import rclpy -from rclpy.node import Node +from roboy_control_msgs.msg import EFPose from rcl_interfaces.srv import GetParameters - +from rclpy.node import Node from roboy_control_msgs.srv import GetLinkPose -from geometry_msgs.msg import PoseStamped from roboy_simulation_msgs.srv import LinkInfoFromName -from ..utils.utils import call_service_async, Topics, Services +from ..utils.utils import Services, Topics, call_service_async + class Link(): """This class handles one operator link. @@ -62,14 +63,15 @@ class Operator(Node, ABC): """This class handles the operator's ROS node. """ - def __init__(self): + def __init__(self, node_name="operator"): """ Args: - """ - super().__init__("operator") + super().__init__(node_name) self.ready = False + self.links = None # State Mapper node parameters client self.state_mapper_parameters_client = self.create_client(GetParameters, Services.STATE_MAPPER_GET) @@ -87,20 +89,14 @@ def initialize(self, future): operator_link_names = result.values[1].string_array_value self.link_map = {k: v for k, v in zip(roboy_link_names, operator_link_names)} - self.declare_parameters( - namespace='', - parameters=[('operator_link_dimentions.' + link_name, None) for link_name in operator_link_names] - ) - self.init_node() self.start_node() self.ready = True def init_node(self): self.init_links() - self.init_end_effectors(["left_wrist", "right_wrist"]) - self.ef_publisher = self.create_publisher(PoseStamped, Topics.OP_EF_POSES, 1) + self.ef_publisher = self.create_publisher(EFPose, Topics.OP_EF_POSES, 1) self.link_info_service = self.create_service(LinkInfoFromName, Services.LINK_INFO_FROM_NAME, self.link_info_from_name_callback) self.initial_pose_service = self.create_service(GetLinkPose, Services.OP_INITIAL_LINK_POSE, self.initial_link_pose_callback) @@ -111,22 +107,36 @@ def start_node(self): def start_publishing(self, period=0.02): self.timer = self.create_timer(period, self.publish_ef_state) - def init_end_effectors(self, efs): + def init_end_effectors(self, efs, init_pos=[None], init_orn=[None]): """Initilizes operator's end effector. Args: efs (list[string]): Names of the end effectors link. + init_pos (list[float[3]]): Initial positions of the end effectors. + init_orn (list[float[4]]): Initial orientations of the end effectors. Returns: - """ self.end_effectors = [] - for link in self.links: - if link.human_name in efs: - self.end_effectors.append(link) - self.get_logger().info("EF " + link.human_name + ": " + str(link.id)) + + if len(efs) != len(init_pos): init_pos *= len(efs) + if len(efs) != len(init_orn): init_orn *= len(efs) + + for ef, pos, orn in zip(efs, init_pos, init_orn): + ef_link = self.get_link(ef) + if ef_link is None: + self.get_logger().error(f"EF {ef} is not a link of the operator!") + return False + + if pos is not None and orn is not None: + ef_link.init_pose = [pos, orn] + + self.end_effectors.append(ef_link) + self.get_logger().info("EF " + ef_link.human_name + ": " + str(ef_link.id)) + return True def get_human_link(self, roboy_link_name): """Gets link object given the roboy's link name that maps to it. @@ -186,17 +196,18 @@ def publish_ef_state(self): continue # self.get_logger().info('Sending Endeffector pose: ' + ef_link.human_name) ef_pos, ef_orn = ef_link.pose - msg = PoseStamped() - msg.header.frame_id = ef_link.human_name - msg.pose.position.x = ef_pos[0] - msg.pose.position.y = ef_pos[1] - msg.pose.position.z = ef_pos[2] + msg = EFPose() + msg.ef_name = ef_link.human_name - msg.pose.orientation.x = ef_orn[0] - msg.pose.orientation.y = ef_orn[1] - msg.pose.orientation.z = ef_orn[2] - msg.pose.orientation.w = ef_orn[3] + msg.ef_pose.position.x = ef_pos[0] + msg.ef_pose.position.y = ef_pos[1] + msg.ef_pose.position.z = ef_pos[2] + + msg.ef_pose.orientation.x = ef_orn[0] + msg.ef_pose.orientation.y = ef_orn[1] + msg.ef_pose.orientation.z = ef_orn[2] + msg.ef_pose.orientation.w = ef_orn[3] self.ef_publisher.publish(msg) @@ -218,9 +229,19 @@ def initial_link_pose_callback(self, request, response): self.get_logger().info(f"Service Initial Link Pose: request received for {request.link_name}") link = self.get_link(request.link_name) + if link is None: + self.get_logger().warn(f"Link '{request.link_name}' does not exist in the operator.") + link_pos = [0.0,0.0,0.0] + link_orn = [0.0,0.0,0.0,0.0] + else: + if link.init_pose is None: + self.get_logger().warn(f"Init pose of link '{link.human_name}' is not initialized.") + link_pos = [0.0,0.0,0.0] + link_orn = [0.0,0.0,0.0,0.0] + else: + link_pos = link.init_pose[0] + link_orn = link.init_pose[1] - link_pos = link.init_pose[0] - link_orn = link.init_pose[1] response.pose.position.x = link_pos[0] response.pose.position.y = link_pos[1] response.pose.position.z = link_pos[2] diff --git a/bulletroboy/operator/operator_hardware.py b/bulletroboy/operator/operator_hardware.py index a26e822..36df280 100644 --- a/bulletroboy/operator/operator_hardware.py +++ b/bulletroboy/operator/operator_hardware.py @@ -1,11 +1,13 @@ import time -from geometry_msgs.msg import PoseStamped +from roboy_control_msgs.msg import EFPose +from roboy_middleware_msgs.srv import InitExoforce from roboy_simulation_msgs.msg import TendonUpdate from ..utils.utils import Services, Topics from .operator import Link, Operator + class OperatorHW(Operator): """This class handles operator related functions and communicates directly with the TeleportVR app. @@ -24,10 +26,8 @@ def start_node(self): self.pull_time = self.get_parameter("pull_time").get_parameter_value().double_value self.target_force_publisher = self.create_publisher(TendonUpdate, Topics.TARGET_FORCE, 1) - self.create_subscription(PoseStamped, Topics.VR_HEADSET_POSES, self.vr_pose_listener, 1) - - self.pull() - self.start_publishing() + self.create_subscription(EFPose, Topics.VR_HEADSET_POSES, self.vr_pose_listener, 1) + self.init_service = self.create_service(InitExoforce, Services.INIT_OPERATOR, self.init_callback) def init_links(self): """Initiates the links in operator class. @@ -39,6 +39,11 @@ def init_links(self): - """ + self.declare_parameters( + namespace='', + parameters=[('operator_link_dimentions.' + link_name, None) for link_name in self.link_map.values()] + ) + self.links = [] for i, key in enumerate(self.link_map): human_name = self.link_map[key] @@ -49,6 +54,34 @@ def init_links(self): else: self.links.append(Link(i, human_name, roboy_name, dims)) + def init_callback(self, request, response): + """Callback for ROS service to init the operator. + + """ + self.get_logger().info("Initializing...") + + ef_names = [name for name in request.ef_name] + init_poses = [([pose.position.x, pose.position.y, pose.position.z],[pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) for pose in request.ef_init_pose] + init_poses = list(map(lambda pose: self.transform_pose_vr_to_cage(pose[0], pose[1]), init_poses)) + + init_positions = [pose[0] for pose in init_poses] + init_orientations = [pose[1] for pose in init_poses] + + if not self.init_end_effectors(ef_names, init_positions, init_orientations): + err_msg = "Failed to initialize end effectors!" + self.get_logger().error(f"Could not be initialized: {err_msg}") + response.success = False + response.message = err_msg + return response + + self.pull() + self.start_publishing() + + self.get_logger().info("Successfully initialized.") + response.success = True + + return response + def pull(self): """Pulls operator to simulate connection to the roboy. @@ -70,25 +103,47 @@ def pull(self): msg.force = 0.0 self.target_force_publisher.publish(msg) - def vr_pose_listener(self, link_pose): + def transform_pose_vr_to_cage(self, position, orientation): + """Transforms pose from vr frame to cage frame. + + Args: + position (3darray[float]): Position in vr frame. + orientation (4darray[float]): Orientation in vr frame. + + Returns: + 3darray[float]: Position in cage frame. + 4darray[float]: Orientation in cage frame. + + """ + x_offset = 0.2 # x axis offset, because roboy arms are longer + z_offset = 1.5 # height offset, because of how we define the cage frame last semester, this will be address with the frames calibration + + position[0] += x_offset + position[2] -= z_offset + + return position, orientation + + def vr_pose_listener(self, ef_pose): """Callback of the pose subscriber. Sets the pose of the link given in the msg. Args: - link_pose: received PoseStamped msg. + ef_pose: received EFPose msg. Returns: - """ - self.get_logger().debug("Received pose for " + link_pose.header.frame_id) - link = self.get_human_link(link_pose.header.frame_id) + self.get_logger().debug("Received pose for " + ef_pose.ef_name) + + link = self.get_link(ef_pose.ef_name) + if link is None: - self.get_logger().warn(link_pose.header.frame_id + "has no mapping!") + self.get_logger().warn(ef_pose.ef_name + " is not an operator link.") return - offset = 0 if link.human_name == "neck" else 0.4 - link_pos = [link_pose.pose.position.x + offset, link_pose.pose.position.y, link_pose.pose.position.z + 1.5] - link_orn = [link_pose.pose.orientation.x, - link_pose.pose.orientation.y, - link_pose.pose.orientation.z, - link_pose.pose.orientation.w] + + link_pos = [ef_pose.ef_pose.position.x, ef_pose.ef_pose.position.y, ef_pose.ef_pose.position.z] + link_orn = [ef_pose.ef_pose.orientation.x, ef_pose.ef_pose.orientation.y, ef_pose.ef_pose.orientation.z, ef_pose.ef_pose.orientation.w] + + link_pos, link_orn = self.transform_pose_vr_to_cage(link_pos, link_orn) + link.set_pose(link_pos, link_orn) diff --git a/bulletroboy/operator/operator_simulation.py b/bulletroboy/operator/operator_simulation.py index d9e33f1..ac4c5dc 100644 --- a/bulletroboy/operator/operator_simulation.py +++ b/bulletroboy/operator/operator_simulation.py @@ -1,11 +1,13 @@ -import pybullet as p -import time import math -import numpy as np +import time from enum import Enum +import numpy as np +import pybullet as p + from ..utils.utils import draw_AABB -from .operator import Operator, Link +from .operator import Link, Operator + class OperatorSim(Operator): """This class handles the operator body and its links in the simulation. @@ -17,7 +19,7 @@ def __init__(self, body_id): body_id (int): Pybullet body indentifier. """ - super().__init__() + super().__init__("operator_sim") self.body_id = body_id def start_node(self): @@ -30,6 +32,8 @@ def start_node(self): - """ + self.init_end_effectors(self.sim_link_map.keys()) + self.movements = Movements(self) self.prevPose = [0, 0, 0] self.trailDuration = 5 @@ -77,6 +81,16 @@ def init_links(self): - """ + self.declare_parameters( + namespace='', + parameters=[('cage_efs', None), + ('urdf_links', None)] + ) + + cage_efs = self.get_parameter("cage_efs").get_parameter_value().string_array_value + urdf_links = self.get_parameter("urdf_links").get_parameter_value().string_array_value + self.sim_link_map = {k: v for k, v in zip(cage_efs, urdf_links)} + self.links = [] for key in self.link_map: human_name = self.link_map[key] @@ -100,6 +114,10 @@ def get_link_index(self, link_name): int: Index of the given link. """ + # mapping to match operator urdf to end effectors + if link_name in self.sim_link_map: + link_name = self.sim_link_map[link_name] + index = None for i in range(p.getNumJoints(self.body_id)): if link_name == str(p.getJointInfo(self.body_id,i)[12], 'utf-8'): diff --git a/bulletroboy/roboy/roboy.py b/bulletroboy/roboy/roboy.py index fd3fbc1..f50739f 100644 --- a/bulletroboy/roboy/roboy.py +++ b/bulletroboy/roboy/roboy.py @@ -6,6 +6,7 @@ from rcl_interfaces.srv import GetParameters from sensor_msgs.msg import JointState +from roboy_control_msgs.msg import EFPose from roboy_simulation_msgs.msg import Collision from geometry_msgs.msg import PoseStamped from roboy_simulation_msgs.srv import LinkInfoFromId @@ -45,8 +46,8 @@ def __init__(self, body_id): self.collision_publisher = self.create_publisher(Collision, Topics.ROBOY_COLLISIONS, 1) self.collision_for_hw_publisher = self.create_publisher(Collision, 'roboy/simulation/roboy/collision_hw', 1) # Operator EF pose subscriber - self.right_ef_pose_subscription = self.create_subscription(PoseStamped, Topics.MAPPED_OP_REF_POSE, self.ef_pose_callback, 1) - self.left_ef_pose_subscription = self.create_subscription(PoseStamped, Topics.MAPPED_OP_LEF_POSE, self.ef_pose_callback, 1) + self.right_ef_pose_subscription = self.create_subscription(EFPose, Topics.MAPPED_OP_REF_POSE, self.ef_pose_callback, 1) + self.left_ef_pose_subscription = self.create_subscription(EFPose, Topics.MAPPED_OP_LEF_POSE, self.ef_pose_callback, 1) # Initial head pose client self.operator_initial_head_pose_client = self.create_client(GetLinkPose, Services.INITIAL_HEAD_POSE) @@ -275,17 +276,17 @@ def ef_pose_callback(self, ef_pose): Args: ef_pose: end effector pose received from the operator. """ - self.get_logger().debug('Endeffector pose received: ' + ef_pose.header.frame_id) + self.get_logger().debug('Endeffector pose received: ' + ef_pose.ef_name) #process message - ef_name = ef_pose.header.frame_id + ef_name = ef_pose.ef_name ef_id = self.end_effectors[ef_name] - link_pos = [ef_pose.pose.position.x, ef_pose.pose.position.y, ef_pose.pose.position.z] - link_orn = [ef_pose.pose.orientation.x, - ef_pose.pose.orientation.y, - ef_pose.pose.orientation.z, - ef_pose.pose.orientation.w] + link_pos = [ef_pose.ef_pose.position.x, ef_pose.ef_pose.position.y, ef_pose.ef_pose.position.z] + link_orn = [ef_pose.ef_pose.orientation.x, + ef_pose.ef_pose.orientation.y, + ef_pose.ef_pose.orientation.z, + ef_pose.ef_pose.orientation.w] # link_orn = self.get_adapted_link_orientation(ef_id, # [ef_pose.pose.orientation.x, ef_pose.pose.orientation.y, diff --git a/bulletroboy/state_mapper/state_mapper.py b/bulletroboy/state_mapper/state_mapper.py index 6a660c4..031d6ad 100644 --- a/bulletroboy/state_mapper/state_mapper.py +++ b/bulletroboy/state_mapper/state_mapper.py @@ -8,10 +8,10 @@ from ..utils.utils import Topics, Services from roboy_simulation_msgs.msg import Collision +from roboy_control_msgs.msg import EFPose from roboy_simulation_msgs.srv import LinkInfoFromName from roboy_simulation_msgs.srv import LinkInfoFromId from roboy_control_msgs.srv import GetLinkPose -from geometry_msgs.msg import PoseStamped class StateMapper(Node): def __init__(self): @@ -50,14 +50,14 @@ def __init__(self): 1) # Operator EF pose subscriber self.operator_movement_subscription = self.create_subscription( - PoseStamped, + EFPose, Topics.OP_EF_POSES, self.operator_movement_listener, 1) # Define publishers self.exoforce_collision_publisher = self.create_publisher(Collision, Topics.MAPPED_COLLISIONS, 1) - self.right_ef_publisher = self.create_publisher(PoseStamped, Topics.MAPPED_OP_REF_POSE, 1) - self.left_ef_publisher = self.create_publisher(PoseStamped, Topics.MAPPED_OP_LEF_POSE, 1) + self.right_ef_publisher = self.create_publisher(EFPose, Topics.MAPPED_OP_REF_POSE, 1) + self.left_ef_publisher = self.create_publisher(EFPose, Topics.MAPPED_OP_LEF_POSE, 1) # Define service self.operator_initial_head_pose = self.create_service(GetLinkPose, Services.INITIAL_HEAD_POSE, self.operator_initial_head_pose_callback) @@ -135,7 +135,7 @@ def operator_movement_listener(self, ef_pose): ef_pose: end effector pose received from the operator. """ - self.get_logger().debug('Endeffector pose received: ' + ef_pose.header.frame_id) + self.get_logger().debug('Endeffector pose received: ' + ef_pose.ef_name) if not self.roboy_is_ready: #self.get_logger().info('Roboy simulation is not ready.') @@ -143,24 +143,24 @@ def operator_movement_listener(self, ef_pose): return #process message - self.get_logger().debug('got frame-id' + ef_pose.header.frame_id) + self.get_logger().debug('got frame-id' + ef_pose.ef_name) - ef_pose.header.frame_id = self.get_roboy_link_name(ef_pose.header.frame_id) + ef_pose.ef_name = self.get_roboy_link_name(ef_pose.ef_name) - orn = [ef_pose.pose.orientation.x, - ef_pose.pose.orientation.y, - ef_pose.pose.orientation.z, - ef_pose.pose.orientation.w] + orn = [ef_pose.ef_pose.orientation.x, + ef_pose.ef_pose.orientation.y, + ef_pose.ef_pose.orientation.z, + ef_pose.ef_pose.orientation.w] - link_orn = self.adapt_orientation_to_roboy(ef_pose.header.frame_id, orn) - ef_pose.pose.orientation.x = link_orn[0] - ef_pose.pose.orientation.y = link_orn[1] - ef_pose.pose.orientation.z = link_orn[2] - ef_pose.pose.orientation.w = link_orn[3] + link_orn = self.adapt_orientation_to_roboy(ef_pose.ef_name, orn) + ef_pose.ef_pose.orientation.x = link_orn[0] + ef_pose.ef_pose.orientation.y = link_orn[1] + ef_pose.ef_pose.orientation.z = link_orn[2] + ef_pose.ef_pose.orientation.w = link_orn[3] self.get_logger().debug('Publishing EF-Pose') - if ef_pose.header.frame_id.find("right") != -1: + if ef_pose.ef_name.find("right") != -1: self.right_ef_publisher.publish(ef_pose) else: self.left_ef_publisher.publish(ef_pose) @@ -375,7 +375,10 @@ def roboy_to_operator_orientation_diff(self, roboy_link_name): if self.operator_initial_link_poses.get(operator_link_name) == None : self.operator_initial_link_poses[operator_link_name] = self.get_initial_link_pose(operator_link_name, self.operator_initial_link_pose_client) self.get_logger().debug("Got operator initial pose for link " + operator_link_name + " : " + str(self.operator_initial_link_poses[operator_link_name])) - op_init_orn = Quaternion(np.array(self.operator_initial_link_poses[operator_link_name][1])) + if np.sum(self.operator_initial_link_poses[operator_link_name][0]) + np.sum(self.operator_initial_link_poses[operator_link_name][1]) == 0: + op_init_orn = roboy_init_orn + else: + op_init_orn = Quaternion(np.array(self.operator_initial_link_poses[operator_link_name][1])) diff = roboy_init_orn / op_init_orn diff --git a/bulletroboy/tests/dummyoperatorforce.sh b/bulletroboy/tests/dummyoperatorforce.sh index 31b4b3a..bd2a521 100755 --- a/bulletroboy/tests/dummyoperatorforce.sh +++ b/bulletroboy/tests/dummyoperatorforce.sh @@ -2,13 +2,13 @@ if [ "$1" == "-h" ] then - echo "usage: dummyoperatorforce [ef] [force] [direction]" + echo "usage: dummyoperatorforce ef force direction" echo echo "arguments:" printf " ef\t\tEnd effector ('left'/'right')\n" printf " force\t\tForce to apply\n" printf " direction\tDirection vector ('up'/'down')\n" - exit 1 + exit 0 fi TOPIC="/exoforce/simulation/collision" diff --git a/bulletroboy/tests/dummyoperatorpose.sh b/bulletroboy/tests/dummyoperatorpose.sh new file mode 100755 index 0000000..af3745b --- /dev/null +++ b/bulletroboy/tests/dummyoperatorpose.sh @@ -0,0 +1,29 @@ +#!/usr/bin/bash + +if [ "$1" == "-h" ] +then + echo "usage: dummyoperatorforce [ef]" + echo + echo "arguments:" + printf " ef\t\tEnd effector ('left'/'right')\n" + exit 0 +fi + +TOPIC="/operator/pose" +MSGTYPE="roboy_control_msgs/EFPose" + +POSELEFT="{position: {x: -0.14829842746257782, y: 0.6122045516967773, z: 0.39584545195102694}, orientation: {x: 0.0976242795586586, y: 0.1263940930366516, z: -0.8053148984909058, w: 0.5709308385848999}}" +POSERIGHT="{position: {x: 0.17322880029678345, y: 0.641670823097229, z: 0.40152866542339327}, orientation: {x: -0.1135825663805008, y: -0.09424448758363724, z: -0.7904344797134399, w: 0.5945000052452087}}" + +EF_NAME="right_hand" +EF_POSE=$POSERIGHT + +if [ "$1" == "left" ] +then + EF_NAME="left_hand" + EF_POSE=$POSELEFT +fi + + +# Publish collision msg to ROS2 +ros2 topic pub $TOPIC $MSGTYPE "{ef_name: $EF_NAME, ef_pose: $EF_POSE}" diff --git a/bulletroboy/tests/initexoforce.sh b/bulletroboy/tests/initexoforce.sh new file mode 100755 index 0000000..a9c81bf --- /dev/null +++ b/bulletroboy/tests/initexoforce.sh @@ -0,0 +1,10 @@ +#!/usr/bin/bash + +TOPIC="/exoforce/control/initialize_request" +MSGTYPE="roboy_middleware_msgs/InitExoforceRequest" + +POSELEFT="{position: {x: -0.14829842746257782, y: 0.6122045516967773, z: 0.39584545195102694}, orientation: {x: 0.0976242795586586, y: 0.1263940930366516, z: -0.8053148984909058, w: 0.5709308385848999}}" +POSERIGHT="{position: {x: 0.17322880029678345, y: 0.641670823097229, z: 0.40152866542339327}, orientation: {x: -0.1135825663805008, y: -0.09424448758363724, z: -0.7904344797134399, w: 0.5945000052452087}}" + +# Publish init msg to exoforce +ros2 topic pub $TOPIC $MSGTYPE "{ef_name: ['left_hand', 'right_hand'], ef_enabled: [True, True], ef_init_pose: [$POSELEFT, $POSERIGHT]}" diff --git a/bulletroboy/tests/initoperator.sh b/bulletroboy/tests/initoperator.sh new file mode 100755 index 0000000..ebbc62c --- /dev/null +++ b/bulletroboy/tests/initoperator.sh @@ -0,0 +1,10 @@ +#!/usr/bin/bash + +SRV="/operator/control/initialize" +SRVTYPE="roboy_middleware_msgs/InitExoforce" + +POSELEFT="{position: {x: -0.14829842746257782, y: 0.6122045516967773, z: 0.39584545195102694}, orientation: {x: 0.0976242795586586, y: 0.1263940930366516, z: -0.8053148984909058, w: 0.5709308385848999}}" +POSERIGHT="{position: {x: 0.17322880029678345, y: 0.641670823097229, z: 0.40152866542339327}, orientation: {x: -0.1135825663805008, y: -0.09424448758363724, z: -0.7904344797134399, w: 0.5945000052452087}}" + +# Call start operator service +ros2 service call $SRV $SRVTYPE "{ef_name: ['left_hand', 'right_hand'], ef_enabled: [True, True], ef_init_pose: [$POSELEFT, $POSERIGHT]}" diff --git a/bulletroboy/tests/stopexoforce.sh b/bulletroboy/tests/stopexoforce.sh new file mode 100755 index 0000000..9f54eaa --- /dev/null +++ b/bulletroboy/tests/stopexoforce.sh @@ -0,0 +1,7 @@ +#!/usr/bin/bash + +TOPIC="/exoforce/control/stop_request" +MSGTYPE="std_msgs/Empty" + +# Publish init msg to exoforce +ros2 topic pub $TOPIC $MSGTYPE "{}" diff --git a/bulletroboy/utils/force_decomposition.py b/bulletroboy/utils/force_decomposition.py index 3ba3e36..41f7aef 100644 --- a/bulletroboy/utils/force_decomposition.py +++ b/bulletroboy/utils/force_decomposition.py @@ -53,5 +53,7 @@ def decompose_force_ef_to_tendons(force_value, force_direction, end_effector, pa forces = {muscle.id: force for muscle, force in zip(end_effector.muscle_units, final_forces)} else: message = solution.message + else: + message = "No tendon can pull in the direction of the target force" return forces, message diff --git a/bulletroboy/utils/utils.py b/bulletroboy/utils/utils.py index 23619ef..c8e0007 100644 --- a/bulletroboy/utils/utils.py +++ b/bulletroboy/utils/utils.py @@ -7,8 +7,8 @@ class Topics: # link poses topics - VR_HEADSET_POSES = "/bullet_ik" - OP_EF_POSES = "/roboy/simulation/operator/pose/endeffector" + VR_HEADSET_POSES = "/operator/pose" + OP_EF_POSES = "/exoforce/operator/pose" MAPPED_OP_REF_POSE = "/roboy/exoforce/pose/endeffector/right" MAPPED_OP_LEF_POSE = "/roboy/exoforce/pose/endeffector/left" ROBOY_EF_POSES = "/roboy/simulation/roboy/ef_pose" @@ -32,6 +32,11 @@ class Topics: # force TARGET_FORCE = "/exoforce/force/target" + # construct interface + INIT_EXOFORCE_REQ = "/exoforce/control/initialize_request" + INIT_EXOFORCE_RES = "/exoforce/control/initialize_response" + STOP_EXOFORCE_REQ = "/exoforce/control/stop_request" + STOP_EXOFORCE_RES = "/exoforce/control/stop_response" class Services: @@ -53,6 +58,9 @@ class Services: START_FORCE_CONTROL = "/exoforce/force/start" STOP_FORCE_CONTROL = "/exoforce/force/stop" + # operator + INIT_OPERATOR = "/operator/control/initialize" + def parse_launch_arg(arg, default_value, logger): """Parse node args received from ros launcher. diff --git a/config/cageConfiguration.xml b/config/cageConfiguration.xml index 5e066d0..2dee852 100644 --- a/config/cageConfiguration.xml +++ b/config/cageConfiguration.xml @@ -8,7 +8,7 @@ 1.0874291915093577 0.0 1.9380437163129485 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -17,7 +17,7 @@ 0.5786176141761094 -0.9207083702844471 1.9380437163129485 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -26,7 +26,7 @@ -0.40932642921767454 -1.0074493143035055 1.9380437163129485 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -35,7 +35,7 @@ 0.0 -0.1 1.4 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -44,7 +44,7 @@ 1.1533967083671417 0.0 0.0 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -53,7 +53,7 @@ 0.74426864200629 -0.8811289096371031 0.0 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -62,7 +62,7 @@ -0.640866629642013 -0.9589650305842462 0.0 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -71,7 +71,7 @@ 0.0 -0.1 0.968 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -80,7 +80,7 @@ 1.0874291915093577 0.0 1.9380437163129485 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -89,7 +89,7 @@ 0.5786176141761094 0.9207083702844471 1.9380437163129485 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -98,7 +98,7 @@ -0.40932642921767454 1.0074493143035055 1.9380437163129485 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -107,7 +107,7 @@ 0.0 0.1 1.4 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -116,7 +116,7 @@ 1.1533967083671417 0.0 0.0 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -125,7 +125,7 @@ 0.74426864200629 0.8811289096371031 0.0 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -134,7 +134,7 @@ -0.640866629642013 0.9589650305842462 0.0 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 @@ -143,7 +143,7 @@ 0.0 0.1 0.968 - 0.0 0.0 0.0 + 0.0 0.0 0.0 100 diff --git a/config/conf.yaml b/config/conf.yaml index a2f90bb..7bf1d3c 100644 --- a/config/conf.yaml +++ b/config/conf.yaml @@ -14,10 +14,10 @@ state_mapper: [chest, right_shoulder, right_elbow, - right_wrist, + right_hand, left_shoulder, left_elbow, - left_wrist, + left_hand, neck] exoforce: @@ -29,8 +29,8 @@ exoforce: min_tendon_force: 0.5 max_collision_force: 50.0 map_link_to_ef: - left_wrist: [7, 8, 43] - right_wrist: [4, 5, 3] + left_hand: [4, 5, 6] + right_hand: [1, 2, 3] operator: ros__parameters: @@ -50,7 +50,7 @@ operator: [0.1042400000000326, 0.10424010023452646, 0.30872003341150434] - left_wrist: + left_hand: [0.09087999999999999, 0.09087999999999996, 0.09088000000000007] @@ -62,7 +62,7 @@ operator: [0.10424000000003777, 0.10424010023452582, 0.30872003341150656] - right_wrist: + right_hand: [0.09087999999999999, 0.09088000000000002, 0.09088000000000007] @@ -71,6 +71,12 @@ operator: 0.23287999999999998, 0.23287999999999975] +operator_sim: + ros__parameters: + # mapping from udrf links to cage end effectors + cage_efs: [left_hand, right_hand] + urdf_links: [left_wrist, right_wrist] + # Unmapped Robot links diff --git a/config/realCageConfiguration.xml b/config/realCageConfiguration.xml index 19ab3d6..c7e41d4 100644 --- a/config/realCageConfiguration.xml +++ b/config/realCageConfiguration.xml @@ -8,7 +8,7 @@ -0.80770826 0.9352907 -1.2293155 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -18,7 +18,7 @@ 0.3260057 0.9829631 -1.1964931 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -28,7 +28,7 @@ 0.9487667 0.10684848 -1.20575 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -38,7 +38,7 @@ -0.62747765 0.7447399 0.369662 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -48,7 +48,7 @@ 0.18399239 0.7915704 0.36415106 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -58,7 +58,7 @@ 0.70451355 0.12568104 0.354079 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -68,7 +68,7 @@ 0.03973961 0.0084914 -0.26991898 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -78,7 +78,7 @@ 0.11925459 -0.00657964 -0.4964082 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -88,7 +88,7 @@ 0.9218533 0.03041518 -1.1626878 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -98,7 +98,7 @@ 0.46096635 -0.9286474 -1.2096934 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -108,7 +108,7 @@ -0.6508255 -0.9630388 -1.1701148 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -118,7 +118,7 @@ 0.70700884 0.10039008 0.35836303 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -128,7 +128,7 @@ 0.3320167 -0.71201 0.35955417 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -138,7 +138,7 @@ -0.521482 -0.88446724 0.3624677 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -148,7 +148,7 @@ 0.12466764 -0.01253831 -0.5456812 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 @@ -158,7 +158,7 @@ 0.0299325 0.02279103 -0.28467876 - 0.0 0.0 0.0 + 0.0 0.0 0.0 30 diff --git a/launch/exoforce.launch.py b/launch/exoforce.launch.py index bb712a4..bb2423c 100644 --- a/launch/exoforce.launch.py +++ b/launch/exoforce.launch.py @@ -7,9 +7,6 @@ def generate_launch_description(): return LaunchDescription([ - DeclareLaunchArgument( 'roboy_urdf', - default_value='', - description='Path to custom roboy URDF file.'), DeclareLaunchArgument( 'cage_conf', default_value='', description='Path to the cage configuration XML.'), @@ -23,24 +20,19 @@ def generate_launch_description(): ), Node( package='bulletroboy', - executable='roboy_sim', - arguments = [LaunchConfiguration('roboy_urdf')] - )#, - # Node( - # package='bulletroboy', - # executable='operator', - # parameters = [os.path.join( - # get_package_share_directory('bulletroboy'), - # 'config', - # 'conf.yaml')] - # ), - # Node( - # package='bulletroboy', - # executable='exoforce', - # arguments = [LaunchConfiguration('cage_conf')], - # parameters = [os.path.join( - # get_package_share_directory('bulletroboy'), - # 'config', - # 'conf.yaml')] - # ) + executable='operator', + parameters = [os.path.join( + get_package_share_directory('bulletroboy'), + 'config', + 'conf.yaml')] + ), + Node( + package='bulletroboy', + executable='exoforce', + arguments = [LaunchConfiguration('cage_conf')], + parameters = [os.path.join( + get_package_share_directory('bulletroboy'), + 'config', + 'conf.yaml')] + ) ])