刪除「ros2_ws/src/ros2_control_command_arm.py」
parent
5f32c30239
commit
023c4ffd3e
@ -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()
|
|
||||||
Reference in New Issue