From 57d704c3fb20aff9db769357cbdc84e942ffcd59 Mon Sep 17 00:00:00 2001 From: Dasun Gunasinghe Date: Mon, 11 Dec 2023 17:38:59 +1000 Subject: [PATCH] Additional Cleanup --- armer/armer.py | 2 +- armer/robots/ROSRobot.py | 14 ++++++++++---- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/armer/armer.py b/armer/armer.py index f2ddc95..d6c30eb 100644 --- a/armer/armer.py +++ b/armer/armer.py @@ -93,7 +93,7 @@ def __init__( # Resolve robot links for collision checking # NOTE: must be done after adding to backend # NOTE: this gets an understanding of the current robot's overlapped self collisions - robot.characterise_collision_overlaps() + robot.characterise_collision_overlaps(debug=True) # This method extracts all captured collision objects (dict) from each robot # Needed to conduct global collision checking (if more than one robot instance is in play) diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py index b83bc6e..a743f4e 100644 --- a/armer/robots/ROSRobot.py +++ b/armer/robots/ROSRobot.py @@ -2420,7 +2420,7 @@ def update_link_collision_window(self): self.collision_sliced_links = self.sorted_links self.collision_sliced_links.reverse() - def characterise_collision_overlaps(self) -> bool: + def characterise_collision_overlaps(self, debug: bool = False) -> bool: """ Characterises the existing robot tree and tracks overlapped links in collision handling NOTE: needed to do collision checking, when joints are typically (neighboring) overlapped @@ -2471,7 +2471,8 @@ def characterise_collision_overlaps(self) -> bool: self.overlapped_link_dict.update(gripper_dict) # using json.dumps() to Pretty Print O(n) time complexity - rospy.loginfo(f"[OVERLAP COLLISION CHARACTERISE] -> Collision Overlaps per link: {json.dumps(self.overlapped_link_dict, indent=4)}") + if debug: + rospy.loginfo(f"[OVERLAP COLLISION CHARACTERISE] -> Collision Overlaps per link: {json.dumps(self.overlapped_link_dict, indent=4)}") # Reached end in success return True @@ -3165,7 +3166,13 @@ def int_marker_cb(self, feedback): feedback.pose.orientation.z]).SE3() shape[0].T = pose_se3.A - rospy.loginfo(f"[INTERACTIVE MARKER UPDATE] -> Obj {feedback.marker_name} updated to pose: {feedback.pose}") + + # Characterise collision overlaps when change is found + # NOTE: enable to see what links are overlapped for debugging purposes + self.characterise_collision_overlaps(debug=False) + + # Debugging + # rospy.loginfo(f"[INTERACTIVE MARKER UPDATE] -> Obj {feedback.marker_name} updated to pose: {feedback.pose}") self.interactive_marker_server.applyChanges() def normalizeQuaternion( self, quaternion_msg ): @@ -3193,7 +3200,6 @@ def interactive_marker_creation(self): 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") # Default interactive marker setup marker = Marker() if obj.shape.stype == 'sphere':