diff --git a/mini_pupper_dance/launch/dance.launch.py b/mini_pupper_dance/launch/dance.launch.py index d48d7e0..f80c7bc 100644 --- a/mini_pupper_dance/launch/dance.launch.py +++ b/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 - ]) \ No newline at end of file + ]) diff --git a/mini_pupper_dance/mini_pupper_dance/dance_client.py b/mini_pupper_dance/mini_pupper_dance/dance_client.py index 741e883..a833901 100644 --- a/mini_pupper_dance/mini_pupper_dance/dance_client.py +++ b/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() \ No newline at end of file + main() diff --git a/mini_pupper_dance/mini_pupper_dance/dance_server.py b/mini_pupper_dance/mini_pupper_dance/dance_server.py index e386fe0..1b7664c 100644 --- a/mini_pupper_dance/mini_pupper_dance/dance_server.py +++ b/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,7 +125,8 @@ def _dance_callback(self, request, response): # Give response response.executed = True return response - + + def main(): rclpy.init() minimal_service = MiniPupperDanceService() @@ -108,5 +134,6 @@ def main(): minimal_service.destroy_node() rclpy.shutdown() + if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/mini_pupper_dance/mini_pupper_dance/episode.py b/mini_pupper_dance/mini_pupper_dance/episode.py index ebe6f25..7857f5a 100644 --- a/mini_pupper_dance/mini_pupper_dance/episode.py +++ b/mini_pupper_dance/mini_pupper_dance/episode.py @@ -73,4 +73,4 @@ 'look_left', 'look_up', 'look_right' -] \ No newline at end of file +] diff --git a/mini_pupper_dance/mini_pupper_dance/math_operations.py b/mini_pupper_dance/mini_pupper_dance/math_operations.py index 6b0a10a..01df053 100644 --- a/mini_pupper_dance/mini_pupper_dance/math_operations.py +++ b/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 diff --git a/mini_pupper_dance/mini_pupper_dance/pose_controller.py b/mini_pupper_dance/mini_pupper_dance/pose_controller.py index 2762210..05670f3 100644 --- a/mini_pupper_dance/mini_pupper_dance/pose_controller.py +++ b/mini_pupper_dance/mini_pupper_dance/pose_controller.py @@ -3,7 +3,8 @@ 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): @@ -11,14 +12,21 @@ 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,25 +37,45 @@ 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_rstate_r): + elif (reference_r > state_r): state_r = state_r + self.increment - if(reference_pstate_p): + elif (reference_p > state_p): state_p = state_p + self.increment - if(reference_ystate_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() @@ -55,5 +83,6 @@ def main(args=None): minimal_controller.destroy_node() rclpy.shutdown() + if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/mini_pupper_music/launch/music.launch.py b/mini_pupper_music/launch/music.launch.py index 3c85a0f..4b62681 100644 --- a/mini_pupper_music/launch/music.launch.py +++ b/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 diff --git a/mini_pupper_music/mini_pupper_music/music_server.py b/mini_pupper_music/mini_pupper_music/music_server.py index cd2da76..d774522 100644 --- a/mini_pupper_music/mini_pupper_music/music_server.py +++ b/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 diff --git a/mini_pupper_music/test/test_copyright.py b/mini_pupper_music/test/test_copyright.py index 97a3919..cc8ff03 100644 --- a/mini_pupper_music/test/test_copyright.py +++ b/mini_pupper_music/test/test_copyright.py @@ -16,8 +16,6 @@ import pytest -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') @pytest.mark.copyright @pytest.mark.linter def test_copyright():