반응형

들어가며

PX4는 아래와 같이 제어 아키텍쳐가 잘 구성되어있으며, 제어기가 올바르게 설계 혹은 튜닝되었다면 이를 어떻게 움직일 것인가에 대한 문제를 해결했다고 볼 수 있다.

Multi-rotor Control Architecture of PX4

 

그렇다면 이를 어디로 움직일 것인가 의사결정을 할 수 있으면 좋을 것이다. 운이 좋게도, PX4는 uXCRE-DDS 를 통해서 각 제어 단계에 침투하여 제어 명령을 인가할 수 있는 인터페이스가 있다.

 

본 글은 멀티로터를 Offboard 모드를 통해서 제어해보는 것을 예제로 다룬다.

Offboard 모드로 위치, 속도, 가속도, 자세를 제어하는 것을 예제로 보여주며, 추가적인 제어 기능도 사용가능하다.

좀 더 알고 싶다면 다음 글을 참고하자.

https://docs.px4.io/main/en/flight_modes/offboard.html

 

시작에 앞서

본 글의 예제를 해보기에 앞서, 다음이 설치되어 있어야 한다.

  • PX4 + Gazebo Garden
  • ROS2
  • uXCRE-DDS
  • px4_msgs, px4_ros_com 저장소 설치 및 빌드 완료

 

Offboard 제어 예시 영상

 

영상처럼 어떻게 하지?

Terminal 1 : uXCRE-DDS Agent

MicroXRCEAgent udp4 -p 8888

 

Terminal 2 : Run Simulator and uXCRE-DDS Client

cd [PX4_Autopilot]
make px4_sitl gz_x500

 

Terminal 3 : Run QGC

cd [QGC_PATH]
./QGroundControl.AppImage

 

Terminal 4 : Run User Node

cd [ROS_WS_PATH]
colcon build --packages-select px4_ros_com # Build packages
source install/local_setup.bash 
ros2 run px4_ros_com offboard_mc_ctrl.py # Run ros2 node

 

아래의 코드를 offboard_mc_ctrl.py 라는 이름으로 해당 위치에 두자 (px4_ros_com/src/examples/offboard_py)

그리 해당 파일을 실행가능하게 하도록 하기 위해 Install Python ececutables 으로 CMakeLists.txt 에 추가하자.

 

 

Offboard 제어 소스코드

접은 글을 펼치면 나온다.

더보기
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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
#!/usr/bin/env python3
 
import rclpy
import numpy as np
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
 
from px4_msgs.msg import OffboardControlMode, VehicleCommand, VehicleStatus
from px4_msgs.msg import TrajectorySetpoint, VehicleAttitudeSetpoint
from px4_msgs.msg import VehicleLocalPosition, VehicleAttitude, VehicleRatesSetpoint
 
# sudo apt install ros-foxy-tf-transformations
from tf_transformations import euler_from_quaternion, quaternion_from_euler 
 
class OffboardControl(Node):
    """Node for controlling a vehicle in offboard mode."""
 
    def __init__(self-> None:
        super().__init__('offboard_control_takeoff_and_land')
 
        # Configure QoS profile for publishing and subscribing
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )
 
        # Create publishers
        self.offboard_control_mode_publisher = self.create_publisher(
            OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
        self.attitude_setpoint_publisher = self.create_publisher(
            VehicleAttitudeSetpoint, '/fmu/in/vehicle_attitude_setpoint', qos_profile)
        self.trajectory_setpoint_publisher = self.create_publisher(
            TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
        self.vehicle_command_publisher = self.create_publisher(
            VehicleCommand, '/fmu/in/vehicle_command', qos_profile)
 
        # Create subscribers
        self.vehicle_local_position_subscriber = self.create_subscription(
            VehicleLocalPosition, '/fmu/out/vehicle_local_position'self.vehicle_local_position_callback, qos_profile)
        self.vehicle_attitude_subscriber = self.create_subscription(
            VehicleAttitude, '/fmu/out/vehicle_attitude'self.vehicle_attitude_callback, qos_profile)
        self.vehicle_status_subscriber = self.create_subscription(
            VehicleStatus, '/fmu/out/vehicle_status'self.vehicle_status_callback, qos_profile)
 
        # Initialize variables
        self.state = 0
        self.offboard_setpoint_counter = 0
        self.vehicle_local_position = VehicleLocalPosition()
        self.vehicle_attitude = VehicleAttitude()
        self.vehicle_status = VehicleStatus()
        self.takeoff_height = -20.0
        self.pos_x = 0.0
        self.pos_y = 0.0
        self.pos_z = 0.0
        self.pos_yaw = 0.0
        self.dist = 0.0
 
        # Create a timer to publish control commands
        self.timer = self.create_timer(0.1self.timer_callback)
 
    def vehicle_local_position_callback(self, vehicle_local_position):
        """Callback function for vehicle_local_position topic subscriber."""
        self.vehicle_local_position = vehicle_local_position
 
    def vehicle_attitude_callback(self, vehicle_attitude):
        """Callback function for vehicle_local_position topic subscriber."""
        self.vehicle_attitude = vehicle_attitude
 
    def vehicle_status_callback(self, vehicle_status):
        """Callback function for vehicle_status topic subscriber."""
        self.vehicle_status = vehicle_status
 
    def arm(self):
        """Send an arm command to the vehicle."""
        self.publish_vehicle_command(
            VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0)
        self.get_logger().info('Arm command sent')
 
    def disarm(self):
        """Send a disarm command to the vehicle."""
        self.publish_vehicle_command(
            VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=0.0)
        self.get_logger().info('Disarm command sent')
 
    def engage_offboard_mode(self):
        """Switch to offboard mode."""
        self.publish_vehicle_command(
            VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0)
        self.get_logger().info("Switching to offboard mode")
 
    def land(self):
        """Switch to land mode."""
        self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND)
        self.get_logger().info("Switching to land mode")
 
    def publish_heartbeat_ob_pos_sp(self):
        """Publish the offboard control mode."""
        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_heartbeat_ob_vel_sp(self):
        """Publish the offboard control mode."""
        msg = OffboardControlMode()
        msg.position    = False
        msg.velocity    = True
        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_heartbeat_ob_acc_sp(self):
        """Publish the offboard control mode."""
        msg = OffboardControlMode()
        msg.position    = False
        msg.velocity    = False
        msg.acceleration= True
        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_heartbeat_ob_att_sp(self):
        """Publish the offboard control mode."""
        msg = OffboardControlMode()
        msg.position    = False
        msg.velocity    = False
        msg.acceleration= False
        msg.attitude    = True
        msg.body_rate   = False
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.offboard_control_mode_publisher.publish(msg)
 
    def publish_position_setpoint(self, x, y, z, yaw_d:float):
        """Publish the trajectory setpoint."""
        msg = TrajectorySetpoint()
        msg.position    = [float(x), float(y), float(z)]
        msg.velocity    = [np.nan, np.nan, np.nan]
        msg.acceleration= [np.nan, np.nan, np.nan]
        msg.yaw = np.max([-np.pi, np.min([np.deg2rad(yaw_d), np.pi])])
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.trajectory_setpoint_publisher.publish(msg)
 
    def publish_velocity_setpoint(self, x, y, z, yaw_d:float):
        """Publish the trajectory setpoint."""
        msg = TrajectorySetpoint()
        msg.position    = [np.nan, np.nan, np.nan]
        msg.velocity    = [float(x), float(y), float(z)]
        msg.acceleration= [np.nan, np.nan, np.nan]
        msg.yaw = np.max([-np.pi, np.min([np.deg2rad(yaw_d), np.pi])])
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.trajectory_setpoint_publisher.publish(msg)
 
    def publish_acceleration_setpoint(self, x, y, z, yaw_d:float):
        """Publish the trajectory setpoint."""
        msg = TrajectorySetpoint()
        msg.position    = [np.nan, np.nan, np.nan]
        msg.velocity    = [np.nan, np.nan, np.nan]
        msg.acceleration= [float(x), float(y), float(z)]
        msg.yaw = np.max([-np.pi, np.min([np.deg2rad(yaw_d), np.pi])])
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.trajectory_setpoint_publisher.publish(msg)
 
    def publish_attitude_setpoint(self, roll_d, pitch_d, yaw_d, thr):
        """
            Publish the attitude and thrust setpoint.
            roll_d [deg]    
            pitch_d [deg]   
            yaw_d [deg]     
            thr [-1 ~ 1]
        """
        roll  = np.deg2rad(roll_d)
        pitch = np.deg2rad(pitch_d)
        yaw   = np.deg2rad(yaw_d)
        msg = VehicleAttitudeSetpoint()
        # msg.roll_body = roll
        # msg.pitch_body = pitch
        # msg.yaw_body = yaw
        msg.q_d = quaternion_from_euler(yaw, pitch, roll)
        msg.thrust_body[2= -thr # For Multi-rotor
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.attitude_setpoint_publisher.publish(msg)
 
    def publish_angular_rate_setpoint(self, rollrate_d, pitchrate_d, yawrate_d, thr):
        """
            Publish the attitude and thrust setpoint.
            rollrate_d  [deg/s]    
            pitchrate_d [deg/s]   
            yawrate_d   [deg/s]     
            thr [-1 ~ 1]
        """
        rollrate  = np.deg2rad(rollrate_d)
        pitchrate = np.deg2rad(pitchrate_d)
        yawrate   = np.deg2rad(yawrate_d)
        msg = VehicleRatesSetpoint()
        msg.roll    = rollrate  # [rad/s]
        msg.pitch   = pitchrate # [rad/s]
        msg.yaw     = yawrate   # [rad/s]
        msg.thrush[2= thr # For Multi-rotor
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.attitude_setpoint_publisher.publish(msg)
 
    def publish_vehicle_command(self, command, **params) -> None:
        """Publish a vehicle command."""
        msg = VehicleCommand()
        msg.command = command
        msg.param1 = params.get("param1"0.0)
        msg.param2 = params.get("param2"0.0)
        msg.param3 = params.get("param3"0.0)
        msg.param4 = params.get("param4"0.0)
        msg.param5 = params.get("param5"0.0)
        msg.param6 = params.get("param6"0.0)
        msg.param7 = params.get("param7"0.0)
        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 get_distance(self):
        dx = self.vehicle_local_position.x - self.pos_x
        dy = self.vehicle_local_position.y - self.pos_y
        dz = self.vehicle_local_position.z - self.pos_z
        return np.linalg.norm([dx,dy,dz])
 
    def timer_callback(self-> None:
        """Callback function for the timer."""
 
        self.vehicle_euler = euler_from_quaternion(self.vehicle_attitude.q)
        self._roll_d  = np.rad2deg(self.vehicle_euler[2])
        self._pitch_d = np.rad2deg(self.vehicle_euler[1])
        self._yaw_d   = np.rad2deg(self.vehicle_euler[0])
        print("S{:d} Time {:.2f}, ".format(self.state, (self.get_clock().now().nanoseconds/1000000000)%1000.0), end=' ')
        # print("Pxyz  {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.x, self.vehicle_local_position.y, self.vehicle_local_position.z), end=' ')
        # print("Vxyz  {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.vx, self.vehicle_local_position.vy, self.vehicle_local_position.vz), end=' ')
        # print("Axyz  {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.ax, self.vehicle_local_position.ay, self.vehicle_local_position.az), end=' ')
        # print("Euler {:6.2f}, {:6.2f}, {:6.2f}".format(self._roll_d, self._pitch_d, self._yaw_d), end=' ')
 
        # Take-off
        if(self.state == 0):
            self.publish_heartbeat_ob_pos_sp()
            if(self.offboard_setpoint_counter < 10):
                self.offboard_setpoint_counter += 1
            self.offboard_setpoint_counter %= 11
            if self.offboard_setpoint_counter < 5:
                self.pos_x = 0.0
                self.pos_y = 0.0
                self.pos_z = self.takeoff_height
                self.pos_yaw = np.rad2deg(self.vehicle_euler[0])
                self.engage_offboard_mode()
            if self.offboard_setpoint_counter == 9:
                self.arm()
                
            self.publish_position_setpoint(self.pos_x, self.pos_y, self.pos_z, self.pos_yaw)
            self.dist = self.get_distance()
            print("Pxyz {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.x, 
                                                          self.vehicle_local_position.y, 
                                                          self.vehicle_local_position.z), end=' ')
            print(" / Move to Home")
            if(self.vehicle_local_position.z <= self.takeoff_height + 1 and
               self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD):
                self.state = 1
        
        # First WP
        elif(self.state == 1):
            self.publish_heartbeat_ob_pos_sp()
            self.pos_x = 0.0
            self.pos_y = 0.0
            self.pos_z = self.takeoff_height
            self.pos_yaw = 0
            self.dist = self.get_distance()
            self.publish_position_setpoint(self.pos_x, self.pos_y, self.pos_z, self.pos_yaw)
            print("Pxyz {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.x, 
                                                          self.vehicle_local_position.y, 
                                                          self.vehicle_local_position.z), end=' ')
            print(" / Move to Home")
            if(self.dist < 1):
                self.state = self.state + 1
                
        elif(self.state == 2): # velocity
            self.publish_heartbeat_ob_vel_sp()
            self.pos_x = 1.0
            self.pos_y = 1.0
            self.pos_z = 0.1
            self.pos_yaw = 180.0
            self.publish_velocity_setpoint(self.pos_x, self.pos_y, self.pos_z, self.pos_yaw)
            self.offboard_setpoint_counter += 1
            print("Vxyz {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.vx, 
                                                          self.vehicle_local_position.vy, 
                                                          self.vehicle_local_position.vz), end=' ')
            print(" / Vxyz = 1.0, 1.0, 0.1")
            if self.offboard_setpoint_counter == 50:
                self.state = self.state + 1
                self.offboard_setpoint_counter = 0
                
        elif(self.state == 3): # velocity
            self.publish_heartbeat_ob_vel_sp()
            self.pos_x = -1.0
            self.pos_y = -1.0
            self.pos_z = -0.1
            self.pos_yaw = -180.0
            self.publish_velocity_setpoint(self.pos_x, self.pos_y, self.pos_z, self.pos_yaw)
            self.offboard_setpoint_counter += 1
            print("Vxyz {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.vx, 
                                                          self.vehicle_local_position.vy, 
                                                          self.vehicle_local_position.vz), end=' ')
            print(" / Vxyz = -1.0, -1.0, -0.1")
            if self.offboard_setpoint_counter == 50:
                self.state = self.state + 1
                self.offboard_setpoint_counter = 0
                
        elif(self.state == 4): # acc
            self.publish_heartbeat_ob_acc_sp()
            self.publish_acceleration_setpoint(0.00.00.00.0)
            self.offboard_setpoint_counter += 1
            print("Axyz {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.ax, 
                                                          self.vehicle_local_position.ay, 
                                                          self.vehicle_local_position.az), end=' ')
            print(" / Acc = 0.0, 0.0, 0.0")
            if self.offboard_setpoint_counter == 20:
                self.state = self.state + 1
                self.offboard_setpoint_counter = 0
                
        elif(self.state == 5): # acc
            self.publish_heartbeat_ob_acc_sp()
            self.publish_acceleration_setpoint(0.10.10.1, np.deg2rad(10.0))
            self.offboard_setpoint_counter += 1
            print("Axyz {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.ax,
                                                          self.vehicle_local_position.ay,
                                                          self.vehicle_local_position.az), end=' ')
            print(" / Acc = 0.1, 0.1, 0.1")
            if self.offboard_setpoint_counter == 20:
                self.state = self.state + 1
                self.offboard_setpoint_counter = 0
                
        elif(self.state == 6): # att
            self.publish_heartbeat_ob_att_sp()
            self.publish_attitude_setpoint(5.00.00.00.73# Roll/Pitch/Yaw
            self.offboard_setpoint_counter += 1
            print("Att {:6.2f}, {:6.2f}, {:6.2f} [deg]".format(self._roll_d ,
                                                               self._pitch_d,
                                                               self._yaw_d  ), end=' ')
            print(" / Att = 5.0, 0.0, 0.0")
            if self.offboard_setpoint_counter == 50:
                self.state = self.state + 1
                self.offboard_setpoint_counter = 0
                
        elif(self.state == 7): # att
            self.publish_heartbeat_ob_att_sp()
            self.publish_attitude_setpoint(0.05.00.00.73# Roll/Pitch/Yaw
            self.offboard_setpoint_counter += 1
            print("Att {:6.2f}, {:6.2f}, {:6.2f} [deg]".format(self._roll_d ,
                                                               self._pitch_d,
                                                               self._yaw_d  ), end=' ')
            print(" / Att = 0.0, 5.0, 0.0")
            if self.offboard_setpoint_counter == 50:
                self.state = self.state + 1
                self.offboard_setpoint_counter = 0
                
        elif(self.state == 8): # att
            self.publish_heartbeat_ob_att_sp()
            self.publish_attitude_setpoint(0.00.010.00.71# Roll/Pitch/Yaw
            self.offboard_setpoint_counter += 1
            print("Att {:6.2f}, {:6.2f}, {:6.2f} [deg]".format(self._roll_d ,
                                                               self._pitch_d,
                                                               self._yaw_d  ), end=' ')
            print(" / Att = 0.0, 0.0, 10.0")
            if self.offboard_setpoint_counter == 50:
                self.state = self.state + 1
                self.offboard_setpoint_counter = 0
                
        elif(self.state == 9): # Pos
            self.publish_heartbeat_ob_pos_sp()
            self.pos_x = 0.0
            self.pos_y = 10.0
            self.pos_z = self.takeoff_height
            self.pos_yaw = np.rad2deg(self.vehicle_euler[0])
            self.dist = self.get_distance()
            self.publish_position_setpoint(self.pos_x, self.pos_y, self.pos_z, self.pos_yaw)
            print("Pxyz {:6.2f}, {:6.2f}, {:6.2f}".format(self.vehicle_local_position.x, 
                                                          self.vehicle_local_position.y, 
                                                          self.vehicle_local_position.z), end=' ')
            print(" / Move to Home")
            if(self.dist < 1):
                self.offboard_setpoint_counter += 1
                if self.offboard_setpoint_counter == 50:
                    self.state = self.state + 1
                    self.offboard_setpoint_counter = 0
                
        # Landing
        elif(self.state == 10):
            self.publish_heartbeat_ob_pos_sp()
            self.land()
            exit(0)
        
 
def main(args=None-> None:
    print('Starting offboard control node...')
    rclpy.init(args=args)
    offboard_control = OffboardControl()
    rclpy.spin(offboard_control)
    offboard_control.destroy_node()
    rclpy.shutdown()
 
 
if __name__ == '__main__':
    try:
        main()
    except Exception as e:
        print(e)
cs

 

Offboard로 멀티로터를 제어하기 위한 절차

Offboard 제어를 통해 기본 기능 절차는 아래를 반복한다.

  1. [61 line] ROS2 노드를 주기적으로 실행하도록 타이머를 생성
  2. [251 line...] Offboard 제어하는 노드가 어떤 제어를 한다는 정보를 담은 Heartbeat를 주기적으로 송신한다.(publish_heatbeat)
  3. [264, 150 lines] 어떤 제어를 한다는 것에 대한 제어 명령 값, 위치 제어라면 위치 값을 publish 한다 publish_position_setpoint() -> trajectory_setpoint_publisher.publish(msg)

Offboard 모드 명령 시에 VEHICLE_CMD_DO_SET_MODE를 사용하여 명령을 내리는 구조가 이해하기 힘들 수 있다.

Offboard 모드 명령 함수

 

참고문헌 2를 참조하자면, Set Mode 의 구조는 base, custom_main, custom_sub 모드로 나뉘며 각각 param1,2,3 이 할당된다.

Commander.cpp 코드의 일부 [2]

 

base_mode에 해당하는 param1은 다음과 같다. 1을 입력한다는 것은 Custom 모드를 활성화하는 명령이다.

Commander.cpp 코드의 일부 [4]

 

custom_main_mode에 해당하는 param2는 다음과 같다. 6은 OFFBOARD라는 것이다.

px4_custom_mode.h 코드의 일부 [3]

 

 

정리하면 VEHICLE_CMD_DO_SET_MODE를 통해서 OFFBOARD ENABLE 명령을 내리는 것이다.

 

이륙 절차는 다음과 같다.

  1. [264 line] 이륙 지점을 설정하기 위한 위치 제어 명령을 publish 한다.
  2. [260, 90 lines] 비행 모드를 Offboard 모드로 변경한다.
  3. [262 line] Arm 명령을 하면 위치제어명령의 위치로 이륙하면서 이동한다. 이때 yaw 명령도 추종하기 때문에 현재 yaw 상태를 피드백 해주면 급격한 요잉 운동을 저감할 수 있다.

착륙 절차는 간단하다.

  1. [402 line] heartbeat 를 유지한다.
  2. [403 line] 착륙 명령을 인가한다. 현재 위치에서 착륙을 수행한다. land()

 

나가며

본 글을 통해 PX4에 대해 uXCRE-DDS Client로 붙은 ROS2 노드를 구현했으며, 해당 노드를 통해서 OFFBOARD 모드로 위치, 속도, 가속도, 자세 명령을 인가할 수 있음을 확인했다. 또한 OFFBOARD 모드 일 때, 이륙/운용/착륙 절차에 대해서 알 수 있다.

 

Reference

[1] "I can not understand VEHICLE_CMD_DO_SET_MODE", https://discuss.px4.io/t/i-can-not-understand-vehicle-cmd-do-set-mode/35216

[2] https://github.com/PX4/PX4-Autopilot/blob/30ce560e3a406fd0084ae52e6ce0d9ef36f4cd3d/src/modules/commander/Commander.cpp#L773

[3] https://github.com/PX4/PX4-Autopilot/blob/30ce560e3a406fd0084ae52e6ce0d9ef36f4cd3d/src/modules/commander/px4_custom_mode.h#L44

[4] https://github.com/PX4/PX4-Autopilot/blob/30ce560e3a406fd0084ae52e6ce0d9ef36f4cd3d/src/modules/commander/Commander.cpp#L77

*** EOF ***

728x90

'SW' 카테고리의 다른 글

[MOT] SORT 논문 리뷰  (0) 2024.06.30
ROS2 Offboard으로 고정익 제어해보기  (0) 2024.06.29
Jetson Orin NX Initial Setting  (0) 2024.06.17
~/.bashrc 를 날려먹었다면..  (0) 2024.06.16
ROS2와 연결한 YOLOv5 돌려보기 (Ubuntu 20.04)  (0) 2024.06.16

+ Recent posts