Additional Cleanup

qcr · Dec 11, 2023 · 57d704c · 57d704c
1 parent 615ae97
commit 57d704c
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 5 deletions.
diff --git 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
@@ -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':