From 4eabdf04ab73aaa4405a45b116777d8c465fb5a9 Mon Sep 17 00:00:00 2001 From: Dasun Gunasinghe Date: Fri, 15 Dec 2023 16:18:21 +1000 Subject: [PATCH] Updated with new services for saving and loading a collision scene (as yaml) --- armer/cython/collision_handler.cpp | 20 +-- armer/cython/collision_handler.html | 5 +- armer/robots/ROSRobot.py | 228 +++++++++++++++++++++++++--- 3 files changed, 223 insertions(+), 30 deletions(-) diff --git a/armer/cython/collision_handler.cpp b/armer/cython/collision_handler.cpp index 8d5d2a2..0966e50 100644 --- a/armer/cython/collision_handler.cpp +++ b/armer/cython/collision_handler.cpp @@ -1,4 +1,4 @@ -/* Generated by Cython 3.0.6 */ +/* Generated by Cython 3.0.5 */ /* BEGIN: Cython Metadata { @@ -38,10 +38,10 @@ END: Cython Metadata */ #else #define __PYX_EXTRA_ABI_MODULE_NAME "" #endif -#define CYTHON_ABI "3_0_6" __PYX_EXTRA_ABI_MODULE_NAME +#define CYTHON_ABI "3_0_5" __PYX_EXTRA_ABI_MODULE_NAME #define __PYX_ABI_MODULE_NAME "_cython_" CYTHON_ABI #define __PYX_TYPE_MODULE_PREFIX __PYX_ABI_MODULE_NAME "." -#define CYTHON_HEX_VERSION 0x030006F0 +#define CYTHON_HEX_VERSION 0x030005F0 #define CYTHON_FUTURE_DIVISION 1 #include #ifndef offsetof @@ -255,7 +255,7 @@ END: Cython Metadata */ #ifndef CYTHON_UPDATE_DESCRIPTOR_DOC #define CYTHON_UPDATE_DESCRIPTOR_DOC 0 #endif -#elif defined(Py_GIL_DISABLED) || defined(Py_NOGIL) +#elif defined(PY_NOGIL) #define CYTHON_COMPILING_IN_PYPY 0 #define CYTHON_COMPILING_IN_CPYTHON 0 #define CYTHON_COMPILING_IN_LIMITED_API 0 @@ -600,8 +600,8 @@ class __Pyx_FakeReference { PyObject *types_module=NULL, *code_type=NULL, *result=NULL; #if __PYX_LIMITED_VERSION_HEX < 0x030B0000 PyObject *version_info; // borrowed - PyObject *py_minor_version = NULL; #endif + PyObject *py_minor_version = NULL; long minor_version = 0; PyObject *type, *value, *traceback; PyErr_Fetch(&type, &value, &traceback); @@ -611,7 +611,6 @@ class __Pyx_FakeReference { if (!(version_info = PySys_GetObject("version_info"))) goto end; if (!(py_minor_version = PySequence_GetItem(version_info, 1))) goto end; minor_version = PyLong_AsLong(py_minor_version); - Py_DECREF(py_minor_version); if (minor_version == -1 && PyErr_Occurred()) goto end; #endif if (!(types_module = PyImport_ImportModule("types"))) goto end; @@ -632,6 +631,7 @@ class __Pyx_FakeReference { Py_XDECREF(code_type); Py_XDECREF(exception_table); Py_XDECREF(types_module); + Py_XDECREF(py_minor_version); if (type) { PyErr_Restore(type, value, traceback); } @@ -959,7 +959,7 @@ static CYTHON_INLINE PyObject * __Pyx_PyDict_GetItemStrWithError(PyObject *dict, #endif #if CYTHON_USE_TYPE_SPECS && PY_VERSION_HEX >= 0x03080000 #define __Pyx_PyHeapTypeObject_GC_Del(obj) {\ - PyTypeObject *type = Py_TYPE((PyObject*)obj);\ + PyTypeObject *type = Py_TYPE(obj);\ assert(__Pyx_PyType_HasFeature(type, Py_TPFLAGS_HEAPTYPE));\ PyObject_GC_Del(obj);\ Py_DECREF(type);\ @@ -1782,7 +1782,7 @@ static CYTHON_INLINE int __Pyx_PyUnicode_Equals(PyObject* s1, PyObject* s2, int #define __Pyx_KwValues_FASTCALL(args, nargs) ((args) + (nargs)) static CYTHON_INLINE PyObject * __Pyx_GetKwValue_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues, PyObject *s); #if CYTHON_COMPILING_IN_CPYTHON && PY_VERSION_HEX >= 0x030d0000 - CYTHON_UNUSED static PyObject *__Pyx_KwargsAsDict_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues); + static CYTHON_UNUSED PyObject *__Pyx_KwargsAsDict_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues); #else #define __Pyx_KwargsAsDict_FASTCALL(kw, kwvalues) _PyStack_AsDict(kwvalues, kw) #endif @@ -4946,7 +4946,7 @@ static CYTHON_INLINE PyObject *__Pyx_GetItemInt_Fast(PyObject *o, Py_ssize_t i, } } #else - if (is_list || !PyMapping_Check(o)) { + if (is_list || PySequence_Check(o)) { return PySequence_GetItem(o, i); } #endif @@ -5482,7 +5482,7 @@ static CYTHON_INLINE PyObject * __Pyx_GetKwValue_FASTCALL(PyObject *kwnames, PyO return NULL; // not found (no exception set) } #if CYTHON_COMPILING_IN_CPYTHON && PY_VERSION_HEX >= 0x030d0000 -CYTHON_UNUSED static PyObject *__Pyx_KwargsAsDict_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues) { +static CYTHON_UNUSED PyObject *__Pyx_KwargsAsDict_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues) { Py_ssize_t i, nkwargs = PyTuple_GET_SIZE(kwnames); PyObject *dict; dict = PyDict_New(); diff --git a/armer/cython/collision_handler.html b/armer/cython/collision_handler.html index 2e14070..d5beb2f 100644 --- a/armer/cython/collision_handler.html +++ b/armer/cython/collision_handler.html @@ -1,5 +1,5 @@ - + @@ -305,6 +305,7 @@ .cython .cs { color: #3D7B7B; font-style: italic } /* Comment.Special */ .cython .gd { color: #A00000 } /* Generic.Deleted */ .cython .ge { font-style: italic } /* Generic.Emph */ +.cython .ges { font-weight: bold; font-style: italic } /* Generic.EmphStrong */ .cython .gr { color: #E40000 } /* Generic.Error */ .cython .gh { color: #000080; font-weight: bold } /* Generic.Heading */ .cython .gi { color: #008400 } /* Generic.Inserted */ @@ -363,7 +364,7 @@ -

Generated by Cython 3.0.6

+

Generated by Cython 3.0.5

Yellow lines hint at Python interaction.
Click on a line that starts with a "+" to see the C code that Cython generated for it. diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py index 75fdc64..bc0aac1 100644 --- a/armer/robots/ROSRobot.py +++ b/armer/robots/ROSRobot.py @@ -56,7 +56,6 @@ # TESTING NEW IMPORTS FOR KD TREE COLLISION SEARCH METHOD from sklearn.neighbors import KDTree -from armer.cython import collision_handler import math import tf2_ros @@ -81,10 +80,9 @@ class DynamicCollisionObj: marker_created: bool = False class ROSRobot(rtb.Robot): + """The ROSRobot class wraps the rtb.Robot implementing basic ROS functionality """ - The ROSRobot class wraps the rtb.ERobot implementing basic ROS functionality - """ - + def __init__(self, robot: rtb.robot.Robot, name: str = None, @@ -308,8 +306,8 @@ def __init__(self, self.tf_listener = tf.TransformListener() # --- Setup Configuration for ARMer --- # - # NOTE: this path (if not set in the cfg/ARM_real.yaml) does not find the user when run - # with systemd. This param must be loaded in via the specific robot_real.yaml file. + # NOTE: this path (if not set in the cfg/_real.yaml) does not find the user when run + # with systemd. This param must be loaded in via the specific _real.yaml file. self.config_path = config_path if config_path else os.path.join( os.getenv('HOME', '/home'), '.ros/configs/system_named_poses.yaml' @@ -317,6 +315,14 @@ def __init__(self, self.custom_configs: List[str] = [] self.__load_config() + # Load in default path to collision scene + self.collision_scene_default_path = os.path.join( + os.getenv('HOME', '/home'), + '.ros/configs/armer_collision_scene.yaml' + ) + # Load the default path + self.load_collision_scene_config() + # --- Publishes trajectory to run as marker list --- # self.display_traj_publisher: rospy.Publisher = rospy.Publisher( '{}/trajectory_display'.format(self.name.lower()), @@ -584,6 +590,18 @@ def __init__(self, self.enable_collision_debug_cb ) + rospy.Service( + '{}/save_collision_objects'.format(self.name.lower()), + CollisionSceneConfig, + self.save_collision_config_cb + ) + + rospy.Service( + '{}/load_collision_config_path'.format(self.name.lower()), + CollisionSceneConfig, + self.load_collision_config_path_cb + ) + # --------------------------------------------------------------------- # # --------- ROS Topic Callback Methods -------------------------------- # # --------------------------------------------------------------------- # @@ -2023,6 +2041,32 @@ def get_collision_obj_cb(self, req: GetCollisionObjectsRequest) -> GetCollisionO # return GetCollisionObjectsResponse(list(self.dynamic_collision_dict.keys())) return GetCollisionObjectsResponse(out_list) + def save_collision_config_cb(self, req: CollisionSceneConfigRequest) -> CollisionSceneConfigResponse: + """Attempts to save the current dynamic collision object dictionary + + :param req: A service request with a config_path (if empty, defaults to initialised path) + :type req: CollisionSceneConfigRequest + :return: True on Success or False + :rtype: CollisionSceneConfigResponse + """ + if self.write_collision_scene_config(config_path=req.config_path): + return CollisionSceneConfigResponse(success=True) + else: + return CollisionSceneConfigResponse(success=False) + + def load_collision_config_path_cb(self, req: CollisionSceneConfigRequest) -> CollisionSceneConfigResponse: + """Attempts to load a collision scene config from a given path + + :param req: Service containing a request for the 'config_path' + :type req: AddCollisionSceneConfigRequest + :return: Service containing a response to the activity 'bool' on success or failure + :rtype: AddCollisionSceneConfigResponse + """ + if self.load_collision_scene_config(config_path=req.config_path): + return CollisionSceneConfigResponse(success=True) + else: + return CollisionSceneConfigResponse(success=False) + # --------------------------------------------------------------------- # # --------- Collision and Singularity Checking Methods ---------------- # # --------------------------------------------------------------------- # @@ -2589,7 +2633,7 @@ def check_link_collision(self, target_link: str, sliced_links: list = [], ignore def neo(self, Tep, velocities): """ Runs a version of Jesse H.'s NEO controller - """ ##### Determine Slack ##### # Transform from the end-effector to desired pose @@ -2699,10 +2743,12 @@ def check_singularity(self, q=None) -> bool: return False def check_collision(self) -> bool: - """ - High-level check of collision + """High-level check of collision NOTE: this is called by loop to verify preempt of robot NOTE: This may not be needed as main armer class (high-level) will check collisions per robot + + :return: True if Collision is Found, else False + :rtype: bool """ # Check for collisions # NOTE: optimise this as much as possible @@ -2719,6 +2765,9 @@ def check_collision(self) -> bool: return False def set_safe_state(self): + """Updates a moving window of safe states + NOTE: only updated when arm is not in collision + """ # Account for pointer location if self.q_safe_window_p < len(self.q_safe_window): # Add current target bar x value @@ -2731,6 +2780,139 @@ def set_safe_state(self): # add value to end self.q_safe_window[-1] = self.q + def load_collision_scene_config(self, config_path: str = ''): + """Attempts to load in a collision scene from a config + """ + if config_path == '' or config_path == None: + rospy.logwarn(f"[LOAD COLLISION SCENE] -> Provided path is invalid. Defaulting to [{self.collision_scene_default_path}]") + config_path = self.collision_scene_default_path + + # Check if the path exists (initialised from ARMer) and loads data + if os.path.exists(config_path): + try: + config = yaml.load(open(config_path), + Loader=yaml.SafeLoader) + if config: + for key, value in config.items(): + # Convert geometry_msgs/Pose and SE3 object + pose = Pose() + pose.position.x = value['pos_x'] + pose.position.y = value['pos_y'] + pose.position.z = value['pos_z'] + pose.orientation.w = value['rot_w'] + pose.orientation.x = value['rot_x'] + pose.orientation.y = value['rot_y'] + pose.orientation.z = value['rot_z'] + + pose_se3 = SE3(value['pos_x'], + value['pos_y'], + value['pos_z']) * UnitQuaternion(value['rot_w'], [ + value['rot_x'], + value['rot_y'], + value['rot_z']]).SE3() + # Handle type selection or error + shape: sg.Shape = None + if value['shape'] == "sphere": + shape = sg.Sphere(radius=value['radius'], pose=pose_se3) + elif value['shape'] == "cylinder": + shape = sg.Cylinder(radius=value['radius'], length=value['length'], pose=pose_se3) + elif value['shape'] == "cuboid" or value['shape'] == "cube": + shape = sg.Cuboid(scale=[value['scale_x'], value['scale_y'], value['scale_z']], pose=pose_se3) + elif value['shape'] == "mesh": + rospy.logwarn(f"In progress -> not yet implemented. Exiting...") + break + else: + rospy.logerr(f"Collision shape type [{value['shape']}] is invalid -> exiting...") + break + + # Create a Dynamic Collision Object and add the configured shape to the scene + dynamic_obj = DynamicCollisionObj(shape=shape, key=key, pose=pose, is_added=False) + # NOTE: these dictionaries are accessed and checked by Armer's main loop for addition to a backend. + with self.lock: + self.dynamic_collision_dict[key] = dynamic_obj + self.collision_dict[key] = [dynamic_obj.shape] + + # Add to list of objects for NEO + self.collision_obj_list.append(dynamic_obj.shape) + + # Re-update the collision overlaps on insertion + # Needed so links expected to be in collision with shape are correctly captured + self.characterise_collision_overlaps() + + # print(f"Current collision objects: {self.collision_obj_list}") + # Add the shape as an interactive marker for easy re-updating + # NOTE: handle to not add if overwrite requested + # NOTE: wait for shape to be added to backend + rospy.sleep(rospy.Duration(secs=0.5)) + self.interactive_marker_creation() + rospy.loginfo(f"[LOAD COLLISION SCENE] -> Scene Loaded Successfully") + else: + rospy.logwarn(f"[LOAD COLLISION SCENE] -> Nothing to Load for Collision Scene...") + except IOError: + pass + else: + rospy.logerr(f"[LOAD COLLISION SCENE] -> Provided Path [{self.collision_scene_default_path}] is Invalid") + + def write_collision_scene_config(self, config_path: str = '') -> bool: + """Attempts to write out a collision shape configuration for reloading if needed + NOTE: currently only handles the default config path (as set at initialisation) + NOTE: this performs an overwrite of the collision shapes (only when called) + """ + if config_path == '': + config_path = self.collision_scene_default_path + rospy.logwarn(f"[SAVE COLLISION SCENE] -> Collision Shape Config Path Empty, defaulting to {self.collision_scene_default_path}") + + # Create the path if not in existance + if not os.path.exists(os.path.dirname(config_path)): + os.makedirs(os.path.dirname(config_path)) + + # Create a new Dictionary to Overwrite Current Config + config = {} + for key, value in self.dynamic_collision_dict.items(): + # New Dict of Items to Populate + dict_items = {} + + # Setup the shape type for saving in a simple manner + dict_items['shape'] = value.shape.stype + if hasattr(value.shape, 'radius'): + dict_items['radius'] = value.shape.radius + if hasattr(value.shape, 'length'): + dict_items['length'] = value.shape.length + if hasattr(value.shape, 'scale'): + dict_items['scale_x'] = float(value.shape.scale[0]) + dict_items['scale_y'] = float(value.shape.scale[1]) + dict_items['scale_z'] = float(value.shape.scale[2]) + + # Save the shape ID (should be unique) + dict_items['id'] = value.id + + # Save the Physical Position of Object in Scene + # NOTE: currently only supports poses from base_link + current_shape_se3 = sm.SE3(self.collision_dict[key][0].T) + uq = UnitQuaternion(current_shape_se3) + shape_pose = Pose( + position=Point(*current_shape_se3.t), + orientation=Quaternion(*np.concatenate([uq.vec3, [uq.s]])) + ) + dict_items['pos_x'] = float(shape_pose.position.x) + dict_items['pos_y'] = float(shape_pose.position.y) + dict_items['pos_z'] = float(shape_pose.position.z) + dict_items['rot_w'] = float(shape_pose.orientation.w) + dict_items['rot_x'] = float(shape_pose.orientation.x) + dict_items['rot_y'] = float(shape_pose.orientation.y) + dict_items['rot_z'] = float(shape_pose.orientation.z) + + # Update output dictionary with a dictionary of items + # NOTE: key is the actual shape's key + config[key] = dict_items + + # Dump the current shape dictionary (configured for saving) to nominated path + with open(config_path, 'w') as handle: + handle.write(yaml.dump(config)) + + rospy.loginfo(f"[SAVE COLLISION SCENE] -> Collision Scene Written to: [{config_path}]") + return True + # --------------------------------------------------------------------- # # --------- Standard Methods ------------------------------------------ # # --------------------------------------------------------------------- # @@ -2802,8 +2984,13 @@ def general_executor(self, q, pose: Pose = None, collision_ignore: bool =False, return result - # Implementation from: https://github.com/ros-controls/ros_control/issues/511 def controller_switch_service(self, ns, cls, **kwargs): + """Sends data to a ROS service + Credit: https://github.com/ros-controls/ros_control/issues/511 + + Args: + ns (str): namepsace of service + """ rospy.wait_for_service(ns) service = rospy.ServiceProxy(ns, cls) response = service(**kwargs) @@ -2813,6 +3000,15 @@ def controller_switch_service(self, ns, cls, **kwargs): rospy.loginfo(f"[CONTROLLER SWITCHER] -> Controller switched successfully") def controller_select(self, controller_type: int = 0) -> bool: + """Attempts to select a provided controller type via controller_manager service + + Args: + controller_type (int, optional): Enumerated value defining a controller type + (only 0 [joint velocity], and 1 [trajectory] are supported). Defaults to 0. + + Returns: + bool: True if successfully switched, otherwise False + """ if controller_type == None or controller_type > 1 or controller_type < 0: rospy.logerr(f"[CONTROLLER SELECT] -> invalid controller type: {controller_type}") return False @@ -2855,6 +3051,7 @@ def execute_ros_control_trajectory(self, traj, max_time: int = 0): Executes a ros_control trajectory implementation NOTE: only works when ros_control backend is available """ + # TODO: needs to handle a namespace in the topic (dependent on robot type) controller_action = "/" + self.trajectory_controller + "/follow_joint_trajectory" print(f"controller action: {controller_action}") # Create a client to loaded controller @@ -3212,7 +3409,8 @@ def interactive_marker_creation(self): dyn_col_dict_cp = self.dynamic_collision_dict.copy() for obj in list(dyn_col_dict_cp.values()): # Ensure object has been successfully added to backend first - if obj.is_added and not obj.marker_created: + # print(f"here: {obj}") + if not obj.marker_created: # Default interactive marker setup marker = Marker() if obj.shape.stype == 'sphere': @@ -3439,12 +3637,6 @@ def step(self, dt: float = 0.01) -> None: # pylint: disable=unused-argument self.event.set() - def _load_shape_config(self): - """ - TODO: Attempts to load in any configured collision shapes - """ - pass - def __load_config(self): """[summary] """ @@ -3498,4 +3690,4 @@ def __write_config(self, key: str, value: Any, config_path: str=''): config.update({key: value}) with open(config_path, 'w') as handle: - handle.write(yaml.dump(config)) + handle.write(yaml.dump(config)) \ No newline at end of file