Added Small comments and additions to the python_interface. Furthermo…

…re added Debug warning for the topic switch
SUTURO · Mar 15, 2024 · 391831f · 391831f
1 parent 1666dac
commit 391831f
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 9 deletions.
diff --git a/src/giskardpy/monitors/force_torque_monitor.py b/src/giskardpy/monitors/force_torque_monitor.py
@@ -30,6 +30,7 @@ def __init__(self,
                  ):
 
         if is_raw:
+            logging.logwarn("ForceTorque Monitor is now listening to the /hsrb/wrist_wrench/raw Topic!")
             topic = "/hsrb/wrist_wrench/raw"
 
         super().__init__(name=name, stay_true=False, start_condition=start_condition, run_call_in_thread=False)
@@ -68,6 +69,7 @@ def force_T_map_transform(self, picker):
 
             return torque_transformed
 
+    # TODO: Add proper checks for all necessary Items!
     def __call__(self):
 
         rob_force = self.force_T_map_transform(1)

diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py
@@ -1549,7 +1549,8 @@ def monitor_placing(self,
         force_torque_trigger = self.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
                                                          name=PayloadForceTorque.__name__,
                                                          start_condition='',
-                                                         threshold_name=ForceTorqueThresholds.FT_Placing.value)
+                                                         threshold_name=ForceTorqueThresholds.FT_Placing.value,
+                                                         is_raw=False)
 
         self.motion_goals.add_motion_goal(motion_goal_class='Placing',
                                           context=context,
@@ -1579,7 +1580,8 @@ def monitor_grasp_carefully(self,
         force_torque_trigger = self.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
                                                          name=PayloadForceTorque.__name__,
                                                          start_condition='',
-                                                         threshold_name=ForceTorqueThresholds.FT_GraspWithCare.value)
+                                                         threshold_name=ForceTorqueThresholds.FT_GraspWithCare.value,
+                                                         is_raw=False)
 
         self.motion_goals.add_motion_goal(motion_goal_class='GraspCarefully',
                                           goal_pose=goal_pose,

diff --git a/src/giskardpy/suturo_types.py b/src/giskardpy/suturo_types.py
@@ -13,12 +13,10 @@ class GripperTypes(Enum):
 # TODO: Create Enums for every Force and/or Torque sensitive action, to be used in force_torque_monitor.py
 @unique
 class ForceTorqueThresholds(Enum):
-    FT_GraspWithCare = 'FTGraspC' # For any "Conventional Object" (for other objects use extra enum?)
+    FT_GraspWithCare = 'FTGraspC'  # For any "Conventional Object" (for other objects use extra enum?)
     FT_Placing = 'FTPlace'
     FT_Door = 'FTDoor'
     FT_DishDoor = 'FTDishDoor'
     FT_GraspCutlery = 'FTGraspCutlery'
-    #FT_Tilt = 'FTTilt' #Pouring
-    #FT_Shelf_Grasp = 'FT_Shelf'
-
-
+    FT_Tilt = 'FTTilt'  # Pouring
+    # FT_Shelf_Grasp = 'FT_Shelf'
diff --git a/test/test_integration_hsr.py b/test/test_integration_hsr.py
@@ -127,7 +127,8 @@ def test_force_monitor_grasp(self, zero_pose: HSRTestWrapper):
         force_torque = zero_pose.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
                                                       name=PayloadForceTorque.__name__,
                                                       start_condition='',
-                                                      threshold_name=ForceTorqueThresholds.FT_GraspWithCare.value)
+                                                      threshold_name=ForceTorqueThresholds.FT_GraspWithCare.value,
+                                                      is_raw=False)
 
         base_goal = PoseStamped()
         base_goal.header.frame_id = 'map'
@@ -155,7 +156,8 @@ def test_force_monitor_placing(self, zero_pose: HSRTestWrapper):
         force_torque = zero_pose.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
                                                       name=PayloadForceTorque.__name__,
                                                       start_condition='',
-                                                      threshold_name=ForceTorqueThresholds.FT_Placing.value)
+                                                      threshold_name=ForceTorqueThresholds.FT_Placing.value,
+                                                      is_raw=False)
 
         base_goal = PoseStamped()
         base_goal.header.frame_id = 'map'