Minor fixes in ROS2 Python packages (#208)

* PEP8 styling
* minor fixes

Signed-off-by: Paweł Lech <[email protected]>

kraken_nav/o3de_kraken_nav/o3de_kraken_nav/joy_to_ackermann.py

@@ -21,19 +21,19 @@ from std_msgs.msg import String
 from sensor_msgs.msg import Joy
 from ackermann_msgs.msg import AckermannDrive
 
+
 class JoyToAckermann(Node):
 
     def __init__(self):
         super().__init__('joy_to_ackermann')
         self.publisher = self.create_publisher(AckermannDrive, 'ackermann_vel', 10)
-        self.subscription = self.create_subscription(Joy, "/joy", self.joy_callback, 10) 
+        self.subscription = self.create_subscription(Joy, "/joy", self.joy_callback, 10)
 
     def joy_callback(self, msg):
         drive = AckermannDrive()
-        drive.speed = 1.5*msg.axes[1]
-        drive.steering_angle = 1.1*msg.axes[0]
-        self.publisher .publish(drive)
-    
+        drive.speed = 1.5 * msg.axes[1]
+        drive.steering_angle = 1.1 * msg.axes[0]
+        self.publisher.publish(drive)
 
 
 def main(args=None):

kraken_nav/o3de_kraken_nav/o3de_kraken_nav/twist_to_ackermann.py

@@ -21,62 +21,66 @@ from rclpy.duration import Duration
 from geometry_msgs.msg import Twist
 from ackermann_msgs.msg import AckermannDrive
 
+
 class TwistToAckermann(Node):
-  def __init__(self):
-    super().__init__('twist_to_ackermann')
-
-    self.declare_parameter('wheelbase', 2.2)
-    
-    self.declare_parameter('publish_zeros_on_timeout', True)
-    self.declare_parameter('timeout_control_interval', 0.1)
-    self.declare_parameter('control_timeout', 0.2)
-    
-    self.wheelbase = self.get_parameter('wheelbase').get_parameter_value().double_value
-    self.timeout_control_interval = self.get_parameter('timeout_control_interval').get_parameter_value().double_value
-    self.control_timeout = self.get_parameter('control_timeout').get_parameter_value().double_value
-    self.publish_zeros_on_timeout =  self.get_parameter('publish_zeros_on_timeout').get_parameter_value().bool_value
-
-    self.last_message_time = self.get_clock().now()
-    self.sub_ = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_cb, 10)
-    self.pub_ = self.create_publisher(AckermannDrive, 'ackermann_vel', 10)
-    if self.publish_zeros_on_timeout:
-      self.timer = self.create_timer(self.timeout_control_interval, self.control)
-
-  def control(self):
-    time_diff = (self.get_clock().now() - self.last_message_time)
-    if time_diff.nanoseconds * 1e-9 > self.control_timeout:
-      msg = AckermannDrive()
-      msg.steering_angle = 0.0
-      msg.speed = 0.0
-      self.pub_.publish(msg)
-      
-  def twist_to_ackermann(self, vel, omega):
-    if omega == 0 or vel == 0:
-      return 0
-
-    radius = vel/ omega
-    steering = math.atan(self.wheelbase / radius)
-    return float(steering)
-
-  def cmd_vel_cb(self, data):
-    self.last_message_time = self.get_clock().now()
-    
-    vel = data.linear.x
-    steering = self.twist_to_ackermann(vel, data.angular.z)
-    
-    msg = AckermannDrive()
-    msg.steering_angle = float(steering)
-    msg.speed = vel
-    
-    self.pub_.publish(msg)
-
-def main(args=None): 
-  rclpy.init(args=args)
-  tta = TwistToAckermann()
-  rclpy.spin(tta)
-  tta.timer.cancel()
-  tta.destroy_node()
-  rclpy.shutdown()
+    def __init__(self):
+        super().__init__('twist_to_ackermann')
+
+        self.declare_parameter('wheelbase', 2.2)
+
+        self.declare_parameter('publish_zeros_on_timeout', True)
+        self.declare_parameter('timeout_control_interval', 0.1)
+        self.declare_parameter('control_timeout', 0.2)
+
+        self.wheelbase = self.get_parameter('wheelbase').get_parameter_value().double_value
+        self.timeout_control_interval = self.get_parameter(
+            'timeout_control_interval').get_parameter_value().double_value
+        self.control_timeout = self.get_parameter('control_timeout').get_parameter_value().double_value
+        self.publish_zeros_on_timeout = self.get_parameter('publish_zeros_on_timeout').get_parameter_value().bool_value
+
+        self.last_message_time = self.get_clock().now()
+        self.sub_ = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_cb, 10)
+        self.pub_ = self.create_publisher(AckermannDrive, 'ackermann_vel', 10)
+        if self.publish_zeros_on_timeout:
+            self.timer = self.create_timer(self.timeout_control_interval, self.control)
+
+    def control(self):
+        time_diff = (self.get_clock().now() - self.last_message_time)
+        if time_diff.nanoseconds * 1e-9 > self.control_timeout:
+            msg = AckermannDrive()
+            msg.steering_angle = 0.0
+            msg.speed = 0.0
+            self.pub_.publish(msg)
+
+    def twist_to_ackermann(self, vel, omega):
+        if omega == 0 or vel == 0:
+            return 0
+
+        radius = vel / omega
+        steering = math.atan(self.wheelbase / radius)
+        return float(steering)
+
+    def cmd_vel_cb(self, data):
+        self.last_message_time = self.get_clock().now()
+
+        vel = data.linear.x
+        steering = self.twist_to_ackermann(vel, data.angular.z)
+
+        msg = AckermannDrive()
+        msg.steering_angle = float(steering)
+        msg.speed = vel
+
+        self.pub_.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    tta = TwistToAckermann()
+    rclpy.spin(tta)
+    tta.timer.cancel()
+    tta.destroy_node()
+    rclpy.shutdown()
+
 
 if __name__ == '__main__':
     main()

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/global_kraken_orchestration.py

@@ -8,6 +8,7 @@ from geometry_msgs.msg import PoseStamped
 from .state_machine import GlobalOrchestrationSM
 from .orchestration_node import KrakenOrchestrationNode
 
+
 class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
     def __init__(self):
         rclpy.init()
@@ -26,7 +27,7 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
         if self.current_index >= len(self.poses):
             self.logger.error('Invalid index!')
             return PoseStamped()
-        
+
         pose = self.poses[self.current_index]
         self.logger.info("Current goal id is %d " % self.current_index)
         self.current_index += 1
@@ -40,11 +41,7 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
         self.logger.info('Has the simulation started?')
         self.send_state("Wait for connection")
         # Perform a check whether the simulation has started or not.
-        if (self.sim_started):
-            self.started()
-        else:
-            self.sim_started = True
-            self.await_start()
+        self.started()
 
     def on_enter_spawning(self):
         self.logger.info('Spawning the robot.')
@@ -61,25 +58,19 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
         # Give nav stack a few seconds before we start first goal point
         self.logger.info('Waiting for robot sensors to latch...')
         time.sleep(3)
-        
+
     def on_enter_gathering(self):
         self.logger.info('Is the robot in a gathering position?')
         # Perform a check whether the robot is in a gathering position or not.
-        if (self.can_gather):
+        if self.can_gather:
             self.gather()
         else:
             self.navigate()
 
-    def on_gather(self):
-        self.logger.info('Gathering the apples.')
-        # Instruct the robot to start gathering apples.
-        self.has_finished = not self.has_finished
-        pass
-
     def on_enter_finishing(self):
         self.logger.info('Has the robot completed its job?')
         # Perform a check whether the robot has finished its job or not.
-        if (self.current_index >= len(self.poses)):
+        if self.current_index >= len(self.poses):
             self.finished()
         else:
             self.finish()
@@ -93,21 +84,21 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
 
         while not self.goal_pose_reached:
             time.sleep(1)
-            
+
         self.goal_pose_reached = False
         self.can_gather = True
 
     def on_gather(self):
         self.logger.info("Requesting gathering...")
-        self.send_state("Gather %.1f "% self.apple_progress)
+        self.send_state("Gather %.1f " % self.apple_progress)
         trigger_res = self.trigger_apple_gathering()
         self.logger.info(trigger_res.message)
 
         if trigger_res.success:
             while not self.gathering_done:
-                self.send_state("Gather %.0f %%"% self.apple_progress)
+                self.send_state("Gather %.0f %%" % self.apple_progress)
                 time.sleep(0.2)
-            
+
             self.logger.info("Done gathering apples.")
         else:
             self.logger.info("Freeing up the AppleKraken...")
@@ -123,11 +114,12 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
         self.send_state("Completed")
         self.logger.info('The robot has completed its job.')
         # The robot has finished its job.
-        pass
+
 
 def main():
     global_orch = GlobalOrchestration()
     global_orch.run()
 
+
 if __name__ == '__main__':
     main()

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/orchestration_node.py

@@ -11,7 +11,6 @@ from std_msgs.msg import String
 from std_msgs.msg import Float32
 
 
-
 class KrakenOrchestrationNode(Node):
     def init_spawn_entity_client(self):
         self.spawn_entity_client = self.create_client(SpawnEntity, 'spawn_entity')
@@ -22,7 +21,7 @@ class KrakenOrchestrationNode(Node):
         self.get_spawn_point_info_client = self.create_client(GetModelState, 'get_spawn_point_info')
         while not self.get_spawn_point_info_client.wait_for_service(timeout_sec=1.0):
             self.get_logger().info('Get spawn point info service not available waiting again...')
-    
+
     def init_get_plan_client(self):
         self.get_plan_client = self.create_client(GetPlan, 'get_gathering_plan')
         while not self.get_plan_client.wait_for_service(timeout_sec=1.0):
@@ -35,42 +34,45 @@ class KrakenOrchestrationNode(Node):
 
     def apple_progress_callback(self, msg):
         self.apple_progress = 100.0 * float(msg.data)
-        
+
     def init_apple_gathering_srvs(self):
-        self.done_apple_gathering_service = self.create_service(Empty, f'{self.kraken_name}/done_apple_gathering', self.apple_gathering_done_callback)
+        self.done_apple_gathering_service = self.create_service(Empty, f'{self.kraken_name}/done_apple_gathering',
+                                                                self.apple_gathering_done_callback)
 
         self.trigger_apple_gathering_client = self.create_client(Trigger, f'{self.kraken_name}/trigger_apple_gathering')
         self.cancel_apple_gathering_client = self.create_client(Trigger, f'{self.kraken_name}/cancel_apple_gathering')
 
         while (not self.trigger_apple_gathering_client.wait_for_service(timeout_sec=1.0)) \
-            and (not self.cancel_apple_gathering_client.wait_for_service(timeout_sec=1.0)):
+                and (not self.cancel_apple_gathering_client.wait_for_service(timeout_sec=1.0)):
             self.get_logger().info('Apple gathering services not available waiting again...')
 
     def init_apple_subscriptions(self):
         qos = QoSProfile(
-            depth = 5,
-            history = QoSHistoryPolicy.KEEP_LAST,
-            reliability = QoSReliabilityPolicy.RELIABLE,
-            durability = QoSDurabilityPolicy.VOLATILE
+            depth=5,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE
         )
-        self.progress_apple_gathering_sub = self.create_subscription(Float32, f'{self.kraken_name}/progress_apple_gathering', self.apple_progress_callback, qos)
+        self.progress_apple_gathering_sub = self.create_subscription(Float32,
+                                                                     f'{self.kraken_name}/progress_apple_gathering',
+                                                                     self.apple_progress_callback, qos)
 
     def init_goal_pose_publisher(self):
         qos = QoSProfile(
-            depth = 5,
-            history = QoSHistoryPolicy.KEEP_LAST,
-            reliability = QoSReliabilityPolicy.RELIABLE,
-            durability = QoSDurabilityPolicy.VOLATILE
+            depth=5,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE
         )
 
         self.goal_pose_publisher = self.create_publisher(PoseStamped, f'{self.kraken_name}/goal_pose', qos)
 
     def init_status_publisher(self):
         qos = QoSProfile(
-            depth = 5,
-            history = QoSHistoryPolicy.KEEP_LAST,
-            reliability = QoSReliabilityPolicy.RELIABLE,
-            durability = QoSDurabilityPolicy.VOLATILE
+            depth=5,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE
         )
         self.status_publisher = self.create_publisher(String, f'{self.kraken_name}/orchestration_status', qos)
 
@@ -78,16 +80,16 @@ class KrakenOrchestrationNode(Node):
         msg = String()
         msg.data = status_str
         self.status_publisher.publish(msg)
-        
+
     def __init__(self):
         super().__init__('kraken_orchestration_node')
-        
+
         self.declare_parameter('robot_name', 'apple_kraken_rusty_1')
         self.kraken_name = self.get_parameter('robot_name').get_parameter_value().string_value
-        
+
         self.declare_parameter('spawn_line', 'line1')
         self.spawn_point = self.get_parameter('spawn_line').get_parameter_value().string_value
-        
+
         self.log_sub = self.create_subscription(Log, "/rosout", self.log_cb, 10)
         self.goal_pose_reached = False
         self.gathering_done = False
@@ -107,10 +109,10 @@ class KrakenOrchestrationNode(Node):
 
     def spawn_kraken(self):
         req = SpawnEntity.Request()
-        req.name = self.kraken_name[:len(self.kraken_name)-2]
+        req.name = self.kraken_name[:len(self.kraken_name) - 2]
         req.xml = self.spawn_point
 
-        res = self.spawn_entity_client.call(req) # TODO - Add exception handling
+        res = self.spawn_entity_client.call(req)  # TODO - Add exception handling
         if res.success:
             self.init_apple_gathering_srvs()
 
@@ -119,10 +121,10 @@ class KrakenOrchestrationNode(Node):
     def get_spawn_point_pose(self):
         req = GetModelState.Request()
         req.model_name = self.spawn_point
-        
+
         res = self.get_spawn_point_info_client.call(req)
 
-        return res.pose # TODO - Add exception handling
+        return res.pose  # TODO - Add exception handling
 
     def get_plan_poses(self, pose):
         req = GetPlan.Request()
@@ -141,5 +143,5 @@ class KrakenOrchestrationNode(Node):
         return self.cancel_apple_gathering_client.call(req)
 
     def navigate_to_pose(self, pose):
-        pose.header = Header(stamp = self.get_clock().now().to_msg(), frame_id = 'map')
-        self.goal_pose_publisher.publish(pose)
+        pose.header = Header(stamp=self.get_clock().now().to_msg(), frame_id='map')
+        self.goal_pose_publisher.publish(pose)

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/state_machine.py

@@ -1,15 +1,15 @@
 from statemachine import StateMachine, State
 
+
 class GlobalOrchestrationSM(StateMachine):
     #### Members for debug purposes ###
-    sim_started = True # Simplification for now
     can_gather = False
     has_finished = True
     ###################################
 
     start = State('Start', initial=True)
-    waiting = State('Waiting') # Waiting for the simulation start
-    spawning = State('Spawning') # Spawning the robot
+    waiting = State('Waiting')  # Waiting for the simulation start
+    spawning = State('Spawning')  # Spawning the robot
     gathering = State('Gathering')
     finishing = State('Finishing')
     # This state may be removed if the simulation is restarted upon completion.
@@ -25,4 +25,4 @@ class GlobalOrchestrationSM(StateMachine):
     finished = finishing.to(terminating)
 
     def __init__(self) -> None:
-        super().__init__()
+        super().__init__()