From c325998d74e78decc4f353880f4836b12d735afc Mon Sep 17 00:00:00 2001 From: boom6314 Date: Tue, 2 Jul 2024 20:08:54 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=AA=E9=99=A4=E3=80=8Cros2=5Fws/src/ros2?= =?UTF-8?q?=5Fcontrol=5Fcommand=5Farm.py=E3=80=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ros2_ws/src/ros2_control_command_arm.py | 200 ------------------------ 1 file changed, 200 deletions(-) delete mode 100644 ros2_ws/src/ros2_control_command_arm.py diff --git a/ros2_ws/src/ros2_control_command_arm.py b/ros2_ws/src/ros2_control_command_arm.py deleted file mode 100644 index 227b61f..0000000 --- a/ros2_ws/src/ros2_control_command_arm.py +++ /dev/null @@ -1,200 +0,0 @@ -import rclpy -from rclpy.node import Node -from mavros_msgs.srv import CommandBool, SetMode, CommandTOL -from mavros_msgs.msg import State -from geometry_msgs.msg import PoseStamped,Quaternion -import time -import math - -class InteractiveDroneControlNode(Node): - def __init__(self): - super().__init__('interactive_drone_control_node') - self.state_sub = self.create_subscription(State, 'mavros/state', self.state_callback, 10) - self.arm_client = self.create_client(CommandBool, 'mavros/cmd/arming') - self.set_mode_client = self.create_client(SetMode, 'mavros/set_mode') - self.takeoff_client = self.create_client(CommandTOL, 'mavros/cmd/takeoff') - self.local_pos_pub = self.create_publisher(PoseStamped, 'mavros/setpoint_position/local', 10) - self.current_state = None - self.current_pose = PoseStamped() - self.get_logger().info("Interactive Drone Control Node has been started") - - - - - - def state_callback(self, msg): - self.current_state = msg - - def arm_drone(self): - req = CommandBool.Request() - req.value = True - future = self.arm_client.call_async(req) - rclpy.spin_until_future_complete(self, future) - response = future.result() - if response.success: - self.get_logger().info('Drone armed successfully') - else: - self.get_logger().error('Failed to arm drone') - - def set_guided_mode(self): - req = SetMode.Request() - req.custom_mode = "GUIDED" - future = self.set_mode_client.call_async(req) - rclpy.spin_until_future_complete(self, future) - response = future.result() - if response.mode_sent: - self.get_logger().info('GUIDED mode set successfully') - else: - self.get_logger().error('Failed to set GUIDED mode') - - def set_land_mode(self): - req = SetMode.Request() - req.custom_mode = "LAND" - future = self.set_mode_client.call_async(req) - rclpy.spin_until_future_complete(self, future) - response = future.result() - if response.mode_sent: - self.get_logger().info('LAND mode set successfully') - else: - self.get_logger().error('Failed to set LAND mode') - - def takeoff(self, takeoff_alt): - req = CommandTOL.Request() - req.altitude = takeoff_alt - req.latitude = 0.0 # 默認值，不需要修改 - req.longitude = 0.0 # 默認值，不需要修改 - req.min_pitch = 0.0 # 默認值，不需要修改 - req.yaw = 0.0 # 默認值，不需要修改 - future = self.takeoff_client.call_async(req) - rclpy.spin_until_future_complete(self, future) - response = future.result() - if response.success: - self.get_logger().info(f'Takeoff command sent successfully: {response.success}') - self.current_pose.pose.position.z = takeoff_alt # 設置當前高度 - time.sleep(3) # 延遲3秒以確保起飛完成 - else: - self.get_logger().error('Failed to send takeoff command') - - # def set_target_pose(self, x, y, z): - # self.current_pose.pose.position.x = x - # self.current_pose.pose.position.y = y - # self.current_pose.pose.position.z = z - # self.local_pos_pub.publish(self.current_pose) - # self.get_logger().info(f'Target position set to ({x}, {y}, {z})') - # time.sleep(2) # 延遲2秒以確保目標位置設置完成 - - def move_xy(self, x1, y1): - new_x = self.current_pose.pose.position.x + x1 - new_y = self.current_pose.pose.position.y + y1 - - heading = math.atan2(y1, x1) - quaternion = self.euler_to_quaternion(0, 0, heading) - - - self.current_pose.pose.position.x = new_x - self.current_pose.pose.position.y = new_y - self.current_pose.pose.orientation = quaternion - self.local_pos_pub.publish(self.current_pose) - self.get_logger().info(f'Yaw to heading {heading} radians') - time.sleep(2) # 延迟2秒以确保航向设置完成 - - self.local_pos_pub.publish(self.current_pose) - self.get_logger().info(f'Moving to ({new_x}, {new_y}) while maintaining altitude {self.current_pose.pose.position.z}') - time.sleep(2) # 延遲2秒以確保目標位置設置完成 - - def euler_to_quaternion(self, roll, pitch, yaw): - qx = math.sin(roll / 2) * math.cos(pitch / 2) * math.cos(yaw / 2) - math.cos(roll / 2) * math.sin(pitch / 2) * math.sin(yaw / 2) - qy = math.cos(roll / 2) * math.sin(pitch / 2) * math.cos(yaw / 2) + math.sin(roll / 2) * math.cos(pitch / 2) * math.sin(yaw / 2) - qz = math.cos(roll / 2) * math.cos(pitch / 2) * math.sin(yaw / 2) - math.sin(roll / 2) * math.sin(pitch / 2) * math.cos(yaw / 2) - qw = math.cos(roll / 2) * math.cos(pitch / 2) * math.cos(yaw / 2) + math.sin(roll / 2) * math.sin(pitch / 2) * math.sin(yaw / 2) - return Quaternion(x=qx, y=qy, z=qz, w=qw) - -def main(args=None): - rclpy.init(args=args) - node = InteractiveDroneControlNode() - - def print_instructions(): - print(""" - Available commands: - go - Set mode to GUIDED,Arm the drone, - takeoff - Take off to specified altitude - move_x - Move to specified x position - back_x - Move to specified x position - move_y - Move to specified y position - back_y - Move to specified y position - land - Land the drone - quit - Quit the program - """) - - print_instructions() - - try: - while rclpy.ok(): - command = input("Enter command: ") - if command == "go": - node.set_guided_mode() - node.arm_drone() - #node.takeoff(5.0) - elif command == "takeoff": - node.takeoff(5.0) - elif command == "land": - node.set_land_mode() - - elif command.startswith("move_x"): - parts = command.split() - if len(parts) == 1: - try: - x1 = 5.0 - y1 = 0.0 - node.move_xy(x1, y1) - except ValueError: - print("Invalid position values") - else: - print("Usage: move_x") - - elif command.startswith("move_y"): - parts = command.split() - if len(parts) == 1: - try: - x1 = 0.0 - y1 = 5.0 - node.move_xy(x1, y1) - except ValueError: - print("Invalid position values") - - elif command.startswith("back_x"): - parts = command.split() - if len(parts) == 1: - try: - x1 = -5.0 - y1 = 0.0 - node.move_xy(x1, y1) - except ValueError: - print("Invalid position values") - else: - print("Usage: back_x") - - elif command.startswith("back_y"): - parts = command.split() - if len(parts) == 1: - try: - x1 = 0.0 - y1 = -5.0 - node.move_xy(x1, y1) - except ValueError: - print("Invalid position values") - else: - print("Usage: back_y") - elif command == "quit": - break - else: - print("Unknown command") - print_instructions() - except KeyboardInterrupt: - pass - - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main()