diff --git a/mini_pupper_dance/mini_pupper_dance/dance_server.py b/mini_pupper_dance/mini_pupper_dance/dance_server.py index 8915151..e5303a7 100644 --- a/mini_pupper_dance/mini_pupper_dance/dance_server.py +++ b/mini_pupper_dance/mini_pupper_dance/dance_server.py @@ -62,50 +62,55 @@ def _dance_callback(self, request, response): 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) - + x, y, z, w = quaternion_from_euler(0.0, -0.3, 0.0) + pose_cmd.orientation.x = x + pose_cmd.orientation.y = y + pose_cmd.orientation.z = z + pose_cmd.orientation.w = w + self.pose_publisher_.publish(pose_cmd) self.get_logger().info('Publishing: "%s"' % request.data) 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) + x, y, z, w = quaternion_from_euler(0.0, 0.3, 0.0) + pose_cmd.orientation.x = x + pose_cmd.orientation.y = y + pose_cmd.orientation.z = z + pose_cmd.orientation.w = w self.pose_publisher_.publish(pose_cmd) self.get_logger().info('Publishing: "%s"' % request.data) 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) + x, y, z, w = quaternion_from_euler(0.0, 0.0, 0.3) + pose_cmd.orientation.x = x + pose_cmd.orientation.y = y + pose_cmd.orientation.z = z + pose_cmd.orientation.w = w self.pose_publisher_.publish(pose_cmd) self.get_logger().info('Publishing: "%s"' % request.data) 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) + x, y, z, w = quaternion_from_euler(0.0, 0.0, -0.3) + pose_cmd.orientation.x = x + pose_cmd.orientation.y = y + pose_cmd.orientation.z = z + pose_cmd.orientation.w = w self.pose_publisher_.publish(pose_cmd) self.get_logger().info('Publishing: "%s"' % request.data) 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) + x, y, z, w = quaternion_from_euler(0.0, 0.0, 0.0) + pose_cmd.orientation.x = x + pose_cmd.orientation.y = y + pose_cmd.orientation.z = z + pose_cmd.orientation.w = w self.pose_publisher_.publish(pose_cmd) self.get_logger().info('Publishing: "%s"' % request.data)