Skip to content

Commit

Permalink
continue fix ci errors
Browse files Browse the repository at this point in the history
  • Loading branch information
CullenSUN committed Nov 4, 2023
1 parent 93704e1 commit d06c5c2
Show file tree
Hide file tree
Showing 9 changed files with 157 additions and 71 deletions.
5 changes: 2 additions & 3 deletions mini_pupper_dance/launch/dance.launch.py
Original file line number Diff line number Diff line change
@@ -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(
Expand All @@ -26,4 +25,4 @@ def generate_launch_description():
dance_server_node,
dance_client_node,
pose_controller_node
])
])
8 changes: 3 additions & 5 deletions mini_pupper_dance/mini_pupper_dance/dance_client.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):

Expand Down Expand Up @@ -64,4 +62,4 @@ def main():


if __name__ == '__main__':
main()
main()
99 changes: 63 additions & 36 deletions mini_pupper_dance/mini_pupper_dance/dance_server.py
Original file line number Diff line number Diff line change
Expand Up @@ -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'):

Check failure on line 28 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:28:11: E275 missing whitespace after keyword
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'):

Check failure on line 34 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:34:13: E275 missing whitespace after keyword
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'):

Check failure on line 40 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:40:13: E275 missing whitespace after keyword
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'):

Check failure on line 46 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:46:13: E275 missing whitespace after keyword
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'):

Check failure on line 52 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:52:13: E275 missing whitespace after keyword
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'):

Check failure on line 58 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:58:13: E275 missing whitespace after keyword
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'):

Check failure on line 64 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:64:13: E275 missing whitespace after keyword
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'):

Check failure on line 74 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:74:13: E275 missing whitespace after keyword
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'):

Check failure on line 84 in mini_pupper_dance/mini_pupper_dance/dance_server.py

View workflow job for this annotation

GitHub Actions / flake8

[flake8] reported by reviewdog 🐶 missing whitespace after keyword Raw Output: ./mini_pupper_dance/mini_pupper_dance/dance_server.py:84:13: E275 missing whitespace after keyword
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()
Expand All @@ -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()
2 changes: 1 addition & 1 deletion mini_pupper_dance/mini_pupper_dance/episode.py
Original file line number Diff line number Diff line change
Expand Up @@ -73,4 +73,4 @@
'look_left',
'look_up',
'look_right'
]
]
13 changes: 8 additions & 5 deletions mini_pupper_dance/mini_pupper_dance/math_operations.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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):
Expand All @@ -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
67 changes: 48 additions & 19 deletions mini_pupper_dance/mini_pupper_dance/pose_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()

Expand All @@ -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()
17 changes: 17 additions & 0 deletions mini_pupper_music/launch/music.launch.py
Original file line number Diff line number Diff line change
@@ -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

Expand Down
15 changes: 15 additions & 0 deletions mini_pupper_music/mini_pupper_music/music_server.py
Original file line number Diff line number Diff line change
@@ -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
Expand Down
Loading

0 comments on commit d06c5c2

Please sign in to comment.