-
Notifications
You must be signed in to change notification settings - Fork 0
/
offboard_test
129 lines (106 loc) · 4.95 KB
/
offboard_test
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
import rclpy
import numpy as np
from rclpy.node import Node
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand
from px4_msgs.msg import VehicleAttitudeSetpoint
class PX4Control(Node):
def __init__(self):
super().__init__('px4_control')
self.roll_body = 0.0
self.pitch_body = 0.0
self.yaw_body = 0.0
self.thrust_body = [0.0, 0.0, -0.5]
class VarMsgVehAttSet:
def __init__(self):
self.roll_body = np.NaN
self.pitch_body = np.NaN
self.yaw_body = np.NaN
self.q_d = [np.NaN, np.NaN, np.NaN, np.NaN]
self.yaw_sp_move_rate = np.NaN
self.thrust_body = np.NaN * np.ones(3)
self.veh_att_set = VarMsgVehAttSet()
self.offboard_control_mode_publisher_ = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', 10)
self.trajectory_setpoint_publisher_ = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10)
self.vehicle_command_publisher_ = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', 10)
self.vehicle_attitude_setpoint_publisher = self.create_publisher(VehicleAttitudeSetpoint, '/fmu/in/vehicle_attitude_setpoint', 10)
self.offboard_setpoint_counter_ = 0
self.create_timer(0.5, self.timer_callback)
def arm(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
self.get_logger().info('Arm command sent')
def disarm(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0)
self.get_logger().info('Disarm command sent')
def publish_vehicle_attitude_setpoint(self, roll=0.0, pitch=0.0, yaw=0.0, thrust=0.5):
msg = VehicleAttitudeSetpoint()
msg.roll_body = roll
msg.pitch_body = pitch
msg.yaw_body = yaw
msg.yaw_sp_move_rate = self.veh_att_set.yaw_sp_move_rate
msg.q_d[0] = self.veh_att_set.q_d[0]
msg.q_d[1] = self.veh_att_set.q_d[1]
msg.q_d[2] = self.veh_att_set.q_d[2]
msg.q_d[3] = self.veh_att_set.q_d[3]
msg.thrust_body[0] = 0.0
msg.thrust_body[1] = 0.0
msg.thrust_body[2] = -thrust
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.vehicle_attitude_setpoint_publisher.publish(msg)
def publish_offboard_control_mode(self):
msg = OffboardControlMode()
msg.position = True
msg.velocity = False
msg.acceleration = False
msg.attitude = False
msg.body_rate = False
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.offboard_control_mode_publisher_.publish(msg)
def publish_trajectory_setpoint(self, x, y, z):
msg = TrajectorySetpoint()
msg.position = [x, y, z]
msg.yaw = -3.14
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.trajectory_setpoint_publisher_.publish(msg)
def publish_vehicle_command(self, command, param1=0.0, param2=0.0):
msg = VehicleCommand()
msg.param1 = param1
msg.param2 = param2
msg.command = command
msg.target_system = 1
msg.target_component = 1
msg.source_system = 1
msg.source_component = 1
msg.from_external = True
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.vehicle_command_publisher_.publish(msg)
def timer_callback(self):
if self.offboard_setpoint_counter_ == 10:
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1.0, 6.0)
self.arm()
self.publish_offboard_control_mode()
if self.offboard_setpoint_counter_ < 40:
self.publish_trajectory_setpoint(0.0, 0.0, -5.0)
elif 40 <= self.offboard_setpoint_counter_ < 46:
self.publish_trajectory_setpoint(5.0, 0.0, -5.0)
self.publish_vehicle_attitude_setpoint(pitch=0.087)
elif 46 <= self.offboard_setpoint_counter_ < 52:
self.publish_trajectory_setpoint(5.0, -5.0, -5.0)
self.publish_vehicle_attitude_setpoint(roll=-0.087)
elif 52 <= self.offboard_setpoint_counter_ < 58:
self.publish_trajectory_setpoint(5.0, 5.0, -5.0)
self.publish_vehicle_attitude_setpoint(roll=0.087)
elif 58 <= self.offboard_setpoint_counter_ < 64:
self.publish_trajectory_setpoint(-5.0, 0.0, -5.0)
self.publish_vehicle_attitude_setpoint(pitch=-0.087)
elif 64 <= self.offboard_setpoint_counter_ < 70:
self.publish_trajectory_setpoint(0.0, 0.0, -5.0)
self.offboard_setpoint_counter_ += 1
def main(args=None):
print('Starting PX4 control node...')
rclpy.init(args=args)
px4_control = PX4Control()
rclpy.spin(px4_control)
px4_control.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()