continue fix ci errors

mini_pupper_dance/launch/dance.launch.py

@@ -1,7 +1,6 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument, LogInfo
+
 
 def generate_launch_description():
     dance_server_node = Node(
@@ -26,4 +25,4 @@ def generate_launch_description():
         dance_server_node,
         dance_client_node,
         pose_controller_node
-    ])
+    ])

mini_pupper_dance/mini_pupper_dance/dance_client.py

@@ -4,10 +4,8 @@
 from rclpy.node import Node
 from mini_pupper_interfaces.srv import DanceCommand
 from std_srvs.srv import SetBool
-from geometry_msgs.msg import Twist
-from geometry_msgs.msg import Pose
-import sys
-from .episode import * # change different espisodes here
+from .episode import dance_commands
+
 
 class MiniPupperDanceClientAsync(Node):
 
@@ -64,4 +62,4 @@ def main():
 
 
 if __name__ == '__main__':
-    main()
+    main()

mini_pupper_dance/mini_pupper_dance/dance_server.py

@@ -6,92 +6,117 @@
 from geometry_msgs.msg import Twist
 from geometry_msgs.msg import Pose
 import time
-from .math_operations import *
+from .math_operations import quaternion_from_euler
+
 
 class MiniPupperDanceService(Node):
 
     def __init__(self):
         super().__init__('mini_pupper_dance_service')
-        self.srv = self.create_service(DanceCommand, 'dance_command', self._dance_callback)
+        self.srv = self.create_service(DanceCommand,
+                                       'dance_command',
+                                       self._dance_callback)
+
         self.vel_publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
-        self.pose_publisher_ = self.create_publisher(Pose, 'reference_body_pose', 10)
-        self.interval = 0.5 #seconds
+        self.pose_publisher_ = self.create_publisher(Pose,
+                                                     'reference_body_pose', 10)
+        self.interval = 0.5  # seconds
 
     def _dance_callback(self, request, response):
         velocity_cmd = Twist()
         pose_cmd = Pose()
-        if(request.data == 'move_forward' ):
+        if(request.data == 'move_forward'):
             velocity_cmd.linear.x = 0.5
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'move_backward' ):
+        elif(request.data == 'move_backward'):
             velocity_cmd.linear.x = -0.5
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'move_left' ):
+        elif(request.data == 'move_left'):
             velocity_cmd.linear.y = 0.5
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'move_right' ):
+        elif(request.data == 'move_right'):
             velocity_cmd.linear.y = -0.5
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'turn_left' ):
+        elif(request.data == 'turn_left'):
             velocity_cmd.angular.z = 1.0
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'turn_right' ):
+        elif(request.data == 'turn_right'):
             velocity_cmd.angular.z = -1.0
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif(request.data == 'look_up'):
+            pose_cmd.orientation.x,
+            pose_cmd.orientation.y,
+            pose_cmd.orientation.z,
+            pose_cmd.orientation.w = quaternion_from_euler(0.0, -0.3, 0.0)
 
-        elif(request.data == 'look_up' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, -0.3, 0.0)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif(request.data == 'look_down'):
+            pose_cmd.orientation.x,
+            pose_cmd.orientation.y,
+            pose_cmd.orientation.z,
+            pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.3, 0.0)
 
-        elif(request.data == 'look_down' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.3, 0.0)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif(request.data == 'look_left'):
+            pose_cmd.orientation.x,
+            pose_cmd.orientation.y,
+            pose_cmd.orientation.z,
+            pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, 0.3)
 
-        elif(request.data == 'look_left' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, 0.3)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif(request.data == 'look_right'):
+            pose_cmd.orientation.x,
+            pose_cmd.orientation.y,
+            pose_cmd.orientation.z,
+            pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, -0.3)
 
-        elif(request.data == 'look_right' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, -0.3)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif(request.data == 'look_middle'):
+            pose_cmd.orientation.x,
+            pose_cmd.orientation.y,
+            pose_cmd.orientation.z,
+            pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, 0.0)
 
-        elif(request.data == 'look_middle' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, 0.0)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'stay' ):
-            time.sleep(self.interval) # do nothing
+        elif(request.data == 'stay'):
+            time.sleep(self.interval)  # do nothing
 
         else:
             self.get_logger().info('Invalid command: "%s"' % request.data)
-            time.sleep(self.interval) # do nothing
+            time.sleep(self.interval)  # do nothing
 
         # Stop the robot from moving
         velocity_cmd = Twist()
@@ -100,13 +125,15 @@ def _dance_callback(self, request, response):
         # Give response
         response.executed = True
         return response
-
+
+
 def main():
     rclpy.init()
     minimal_service = MiniPupperDanceService()
     rclpy.spin(minimal_service)
     minimal_service.destroy_node()
     rclpy.shutdown()
 
+
 if __name__ == '__main__':
-    main()
+    main()

mini_pupper_dance/mini_pupper_dance/episode.py

@@ -73,4 +73,4 @@
     'look_left',
     'look_up',
     'look_right'
-]
+]

mini_pupper_dance/mini_pupper_dance/math_operations.py

@@ -8,6 +8,8 @@
 
 # this implementation assumes normalized quaternion
 # converts to Euler angles in 3-2-1 sequence
+
+
 def euler_from_quaternion(x, y, z, w):
     # roll (x-axis rotation)
     sinr_cosp = 2.0 * (w * x + y * z)
@@ -24,8 +26,9 @@ def euler_from_quaternion(x, y, z, w):
     siny_cosp = +2.0 * (w * z + x * y)
     cosy_cosp = +1.0 - 2.0 * (y * y + z * z)
     yaw_z = math.atan2(siny_cosp, cosy_cosp)
-
-    return roll_x, pitch_y, yaw_z # in radius
+
+    return roll_x, pitch_y, yaw_z  # in radius
+
 
 # roll (x), pitch (Y), yaw (z)
 def quaternion_from_euler(roll, pitch, yaw):
@@ -34,11 +37,11 @@ def quaternion_from_euler(roll, pitch, yaw):
     cp = math.cos(pitch * 0.5)
     sp = math.sin(pitch * 0.5)
     cy = math.cos(yaw * 0.5)
-    sy = math.sin(yaw * 0.5)    
+    sy = math.sin(yaw * 0.5)
 
     w = cr * cp * cy + sr * sp * sy
     x = sr * cp * cy - cr * sp * sy
     y = cr * sp * cy + sr * cp * sy
-    z = cr * cp * sy - sr * sp * cy  
-    
+    z = cr * cp * sy - sr * sp * cy
+
     return x, y, z, w

mini_pupper_dance/mini_pupper_dance/pose_controller.py

@@ -3,22 +3,30 @@
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Pose
-from .math_operations import *
+from .math_operations import euler_from_quaternion, quaternion_from_euler
+
 
 class PoseController(Node):
 
     def __init__(self):
         super().__init__('minimal_publisher')
         self.publisher_ = self.create_publisher(Pose, 'body_pose', 10)
         timer_period = 0.001  # seconds
-        self.publisher_timer = self.create_timer(timer_period, self.publisher_timer_callback)
+        self.publisher_timer = self.create_timer(timer_period,
+                                                 self.publisher_timer_callback)
 
-        self.subscription = self.create_subscription(Pose,'reference_body_pose',self.listener_callback,10)
+        self.subscription = self.create_subscription(Pose,
+                                                     'reference_body_pose',
+                                                     self.listener_callback,
+                                                     10)
         self.subscription  # prevent unused variable warning
 
-        self.interpolation_timer = self.create_timer(timer_period, self.interpolation_timer_callback)
+        self.interpolation_timer = self.create_timer(
+            timer_period,
+            self.interpolation_timer_callback
+        )
 
-        self.increment = 0.001 # resolution of interpolation
+        self.increment = 0.001  # resolution of interpolation
         self.pose_state = Pose()
         self.pose_reference = Pose()
 
@@ -29,31 +37,52 @@ def listener_callback(self, msg):
         self.pose_reference = msg
 
     def interpolation_timer_callback(self):
-        state_r,state_p,state_y = euler_from_quaternion(self.pose_state.orientation.x, self.pose_state.orientation.y, self.pose_state.orientation.z, self.pose_state.orientation.w)
-        reference_r,reference_p,reference_y = euler_from_quaternion(self.pose_reference.orientation.x, self.pose_reference.orientation.y, self.pose_reference.orientation.z, self.pose_reference.orientation.w)
-        if(reference_r<state_r):
+        state_r, state_p, state_y = euler_from_quaternion(
+            self.pose_state.orientation.x,
+            self.pose_state.orientation.y,
+            self.pose_state.orientation.z,
+            self.pose_state.orientation.w
+            )
+
+        reference_r, reference_p, reference_y = euler_from_quaternion(
+            self.pose_reference.orientation.x,
+            self.pose_reference.orientation.y,
+            self.pose_reference.orientation.z,
+            self.pose_reference.orientation.w
+        )
+
+        if (reference_r < state_r):
             state_r = state_r - self.increment
-        elif(reference_r>state_r):
+        elif (reference_r > state_r):
             state_r = state_r + self.increment
-        if(reference_p<state_p):
+        if (reference_p < state_p):
             state_p = state_p - self.increment
-        elif(reference_p>state_p):
+        elif (reference_p > state_p):
             state_p = state_p + self.increment
-        if(reference_y<state_y):
+        if (reference_y < state_y):
             state_y = state_y - self.increment
-        elif(reference_y>state_y):
+        elif (reference_y > state_y):
             state_y = state_y + self.increment
-        self.pose_state.orientation.x,self.pose_state.orientation.y,self.pose_state.orientation.z,self.pose_state.orientation.w = quaternion_from_euler(state_r, state_p, state_y)
-        self.pose_state.position.x=self.pose_reference.position.x
-        self.pose_state.position.y=self.pose_reference.position.y
-        self.pose_state.position.z=self.pose_reference.position.z
-
+
+        self.pose_state.orientation.x,
+        self.pose_state.orientation.y,
+        self.pose_state.orientation.z,
+        self.pose_state.orientation.w = quaternion_from_euler(state_r,
+                                                              state_p,
+                                                              state_y)
+
+        self.pose_state.position.x = self.pose_reference.position.x
+        self.pose_state.position.y = self.pose_reference.position.y
+        self.pose_state.position.z = self.pose_reference.position.z
+
+
 def main(args=None):
     rclpy.init(args=args)
     minimal_controller = PoseController()
     rclpy.spin(minimal_controller)
     minimal_controller.destroy_node()
     rclpy.shutdown()
 
+
 if __name__ == '__main__':
-    main()
+    main()

mini_pupper_music/launch/music.launch.py

@@ -1,3 +1,20 @@
+#!/usr/bin/env python3
+#
+# Copyright 2023 MangDang
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+# @Author  : Cullen SUN
+
 from launch import LaunchDescription
 from launch_ros.actions import Node
 

mini_pupper_music/mini_pupper_music/music_server.py

@@ -1,4 +1,19 @@
 #!/usr/bin/env python3
+#
+# Copyright 2023 MangDang
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+# @Author  : Cullen SUN
 
 import rclpy
 from rclpy.node import Node