fixed pr2 traj mode

SemRoCo · Dec 3, 2024 · 3b79f95 · 3b79f95
1 parent 82f371d
commit 3b79f95
Show file tree

Hide file tree

Showing 5 changed files with 12 additions and 6 deletions.
diff --git a/launch/giskardpy_pr2_iai.launch b/launch/giskardpy_pr2_iai.launch
@@ -1,14 +1,14 @@
 <launch>
 
-  <node pkg="giskardpy" type="iai_pr2.py" name="giskard" output="screen"/>
+  <node pkg="giskardpy_ros" type="iai_pr2.py" name="giskard" output="screen"/>
 
-  <node pkg="giskardpy" type="joystick_e_stop.py" name="giskard_e_stop" output="screen">
+  <node pkg="giskardpy_ros" type="joystick_e_stop.py" name="giskard_e_stop" output="screen">
     <rosparam param="button_ids">
         [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]
     </rosparam>
   </node>
 
-  <node pkg="giskardpy" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">
+  <node pkg="giskardpy_ros" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">
     <rosparam param="enable_self_collision">False</rosparam>
     <rosparam param="interactive_marker_chains">
       - [odom_combined, r_gripper_tool_frame]

diff --git a/scripts/iai_robots/hsr/iai_hsr.py b/scripts/iai_robots/hsr/iai_hsr.py
@@ -5,9 +5,12 @@
 from giskardpy_ros.configs.giskard import Giskard
 from giskardpy_ros.configs.iai_robots.hsr import WorldWithHSRConfig, HSRCollisionAvoidanceConfig, \
     HSRJointTrajInterfaceConfig
+from giskardpy_ros.ros1.interface import ROS1Wrapper
+from middleware import set_middleware
 
 if __name__ == '__main__':
     rospy.init_node('giskard')
+    set_middleware(ROS1Wrapper())
     debug_mode = rospy.get_param('~debug_mode', False)
     giskard = Giskard(world_config=WorldWithHSRConfig(),
                       collision_avoidance_config=HSRCollisionAvoidanceConfig(),

diff --git a/scripts/iai_robots/pr2/iai_pr2.py b/scripts/iai_robots/pr2/iai_pr2.py
@@ -5,6 +5,8 @@
 from giskardpy_ros.configs.behavior_tree_config import OpenLoopBTConfig
 from giskardpy_ros.configs.giskard import Giskard
 from giskardpy_ros.configs.iai_robots.pr2 import PR2CollisionAvoidance, WorldWithPR2Config, PR2JointTrajServerIAIInterface
+from giskardpy_ros.ros1.interface import ROS1Wrapper
+from giskardpy.middleware import set_middleware
 
 
 class WorldWithPR2ConfigBlue(WorldWithPR2Config):
@@ -16,6 +18,7 @@ def setup(self):
 
 if __name__ == '__main__':
     rospy.init_node('giskard')
+    set_middleware(ROS1Wrapper())
     giskard = Giskard(world_config=WorldWithPR2ConfigBlue(),
                       collision_avoidance_config=PR2CollisionAvoidance(),
                       robot_interface_config=PR2JointTrajServerIAIInterface(),

diff --git a/src/giskardpy_ros/tree/behaviors/send_trajectory.py b/src/giskardpy_ros/tree/behaviors/send_trajectory.py
@@ -183,7 +183,7 @@ def update(self):
 
         result = self.action_client.get_result()
         if result:
-            if current_time < self.min_deadline:
+            if current_time.to_sec() < self.min_deadline.to_sec():
                 msg = f'\'{self.namespace}\' executed too quickly, stopping execution.'
                 e = ExecutionSucceededPrematurely(msg)
                 raise_to_blackboard(e)
@@ -192,7 +192,7 @@ def update(self):
             get_middleware().loginfo(f'\'{self.namespace}\' successfully executed the trajectory.')
             return py_trees.Status.SUCCESS
 
-        if current_time > self.max_deadline:
+        if current_time.to_sec() > self.max_deadline.to_sec():
             self.action_client.cancel_goal()
             msg = f'Cancelling \'{self.namespace}\' because it took to long to execute the goal.'
             get_middleware().logerr(msg)

diff --git a/src/giskardpy_ros/tree/branches/send_trajectories.py b/src/giskardpy_ros/tree/branches/send_trajectories.py
@@ -23,7 +23,7 @@ def __init__(self, name: str = 'execute traj'):
         self.prepare_base_control = PrepareBaseTrajControlLoop()
         self.insert_child(self.prepare_base_control, 0)
 
-        self.base_closed_loop = ControlLoop(log_traj=False, max_hz=GiskardBlackboard().control_loop_max_hz)
+        self.base_closed_loop = ControlLoop(log_traj=False)
         self.base_closed_loop.add_closed_loop_behaviors()
         self.move_robots.add_child(self.base_closed_loop)