Removed infinite loops, redundant code - launch now shuts down properly

nicholasl23638 · Jul 19, 2024 · 648eca3 · 648eca3
1 parent 495601a
commit 648eca3
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 9 deletions.
diff --git a/hyperdog_ctrl/body_motion_planner/body_motion_planner.py b/hyperdog_ctrl/body_motion_planner/body_motion_planner.py
@@ -1,4 +1,5 @@
 import numpy as np
+import rclpy
 from cmd_manager.hyperdog_variables import Cmds, Body, Leg
 import time
 # cmd = Cmds()
@@ -40,7 +41,7 @@ def change_height(self):
         return True
 
     def run(self):
-        while True:
+        while rclpy.ok():
             self.cmd.leg.foot_zero_pnt[:,2] = np.array(self.cmd.body.height) 
             """ uncomment below 2 lines to activate slant from joystick"""
             self.cmd.leg.foot_zero_pnt[:,1] = self.__L1

diff --git a/hyperdog_ctrl/cmd_manager/main.py b/hyperdog_ctrl/cmd_manager/main.py
@@ -46,7 +46,7 @@ def main(args=None):
         thread_bmp.start()
         thread_gait_planner.start()
 
-        while 1:
+        while rclpy.ok():
             print("tnumber of threads in background: {}".format(threading.active_count()))
             # print("current thread: {}\n".format(threading.current_thread().name))
             # print(cmd.body.height, '----', leg.FR.pose.cur_coord)

diff --git a/hyperdog_ctrl/gait_generator/gait_planner.py b/hyperdog_ctrl/gait_generator/gait_planner.py
@@ -1,4 +1,5 @@
 import numpy as np
+import rclpy
 import matplotlib.pyplot as plt
 # from time import time
 import time
@@ -510,7 +511,7 @@ def give_hand(self):
 
 
     def run(self):
-        while True:
+        while rclpy.ok():
             if self.cmd.mode.walk:
                 if self.cmd.mode.gait_type == 1:
                     self.cmd.gait.cycle_time = 0.8

diff --git a/hyperdog_launch/launch/hyperdog.launch.py b/hyperdog_launch/launch/hyperdog.launch.py
@@ -37,11 +37,6 @@ def generate_launch_description():
         output='screen'
     )
 
-    hyperdog_gazebo_joint_cmd = ExecuteProcess(
-        cmd=['ros2', 'run', 'hyperdog_gazebo_joint_cmd', 'hyperdog_gazebo_joint_controller'],
-        output='screen'
-    )
-
 
     return LaunchDescription([
         node_joy,
@@ -64,7 +59,10 @@ def generate_launch_description():
                     node_IK_node
                 ]
             )
-        ),
+        )
+
+        ### BELOW IS WHERE WE'LL PUT THE LAUNCH STUFF FOR A REAL ROBOT ###
+
         # RegisterEventHandler(
         #     OnProcessStart(
         #     target_action=node_IK_node,