px4_sim

움직이기

정지홍 2025. 9. 19. 14:15

ssh -i ~/.ssh/id_ed25519 -Y quad@192.167.1.31

 

 

map  --(rtabmap)-->  odom  --(rtabmap_odom)-->  base_link  --(static)-->  base_link_frd  --(static)-->  camera_link  --(static)-->  camera_color_optical_frame

 

1.

ros2 launch mavros px4.launch   fcu_url:=serial:///dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0:921600   fcu_protocol:=v2.0

 

 

2. 

ros2 launch realsense2_camera rs_launch.py   align_depth:=true   enable_gyro:=true enable_accel:=true   unite_imu_method:=2   enable_sync:=true   publish_tf:=false
ros2 launch realsense2_camera rs_launch.py   align_depth:=true enable_gyro:=true enable_accel:=true   unite_imu_method:=linear_interpolation enable_sync:=true publish_tf:=false

 

 

3.

ros2 run tf2_ros static_transform_publisher \
  0.10 -0.02 0.05  0 0 0 1  base_link  camera_link

 

4.

ros2 run tf2_ros static_transform_publisher \
  0 0 0  -0.5 0.5 -0.5 0.5  camera_link  camera_color_optical_frame

 

3.

ros2 run tf2_ros static_transform_publisher 0 0 0 1 0 0 0 base_link base_link_frd

 

 

4.

ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 1 base_link_frd camera_link

 

 

5.

 ros2 run tf2_ros static_transform_publisher 0 0 0 -0.5 0.5 -0.5 0.5 camera_link camera_color_optical_frame

 

 

 

6. 위에꺼

ros2 run rtabmap_odom rgbd_odometry --ros-args -p frame_id:=base_link -p odom_frame_id:=odom -p publish_tf:=true -p wait_for_transform:=0.8 -p approx_sync:=true -p approx_sync_max_interval:=0.02 -p qos:=2 -p qos_camera_info:=2 -r rgb/image:=/camera/camera/color/image_raw -r depth/image:=/camera/camera/depth/image_rect_raw -r rgb/camera_info:=/camera/camera/color/camera_info -r depth/camera_info:=/camera/camera/depth/camera_info -r imu:=/mavros/imu/data
ros2 run rtabmap_odom rgbd_odometry --ros-args   -p frame_id:=base_link   -p odom_frame_id:=odom   -p publish_tf:=true   -p wait_for_transform:=0.8   -p approx_sync:=true -p approx_sync_max_interval:=0.02   -p qos:=2 -p qos_camera_info:=2   -r rgb/image:=/camera/camera/color/image_raw   -r depth/image:=/camera/camera/depth/image_rect_raw   -r rgb/camera_info:=/camera/camera/color/camera_info   -r odom:=/rtabmap/odom   -r imu:=/mavros/imu/data

 

 

7.위에꺼

ros2 run rtabmap_slam rtabmap --ros-args   -p frame_id:=base_link   -p odom_frame_id:=odom   -p map_frame_id:=map   -p publish_tf:=true   -p tf_delay:=0.12   -p tf_tolerance:=0.5   -p wait_for_transform:=0.8   -p subscribe_depth:=true -p subscribe_rgb:=true   -p approx_sync:=true -p approx_sync_max_interval:=0.12   -p qos_image:=2 -p qos_camera_info:=2   -r rgb/image:=/camera/camera/color/image_raw   -r depth/image:=/camera/camera/depth/image_rect_raw   -r rgb/camera_info:=/camera/camera/color/camera_info   -r odom:=/rtabmap/odom   -r imu:=/mavros/imu/data -r depth/camera_info:=/camera/camera/depth/camera_info -p subscribe_imu:=true

ros2 run rtabmap_slam rtabmap --ros-args   -p frame_id:=base_link   -p odom_frame_id:=odom   -p map_frame_id:=map   -p publish_tf:=true   -p tf_delay:=0.12   -p tf_tolerance:=0.5   -p wait_for_transform:=0.8   -p subscribe_depth:=true -p subscribe_rgb:=true   -p approx_sync:=true -p approx_sync_max_interval:=0.12   -p qos_image:=2 -p qos_camera_info:=2   -r rgb/image:=/camera/camera/color/image_raw   -r depth/image:=/camera/camera/depth/image_rect_raw   -r rgb/camera_info:=/camera/camera/color/camera_info   -r odom:=/rtabmap/odom   -r imu:=/mavros/imu/data

 

 

8. px4 드론의 파라미터 설정... 각각의 명령어는 ros2 service call을 통해서 한다.

  • /mavros/param/set
    • MAVROS에서 파라미터를 설정하기 위한 서비스이다.
  • mavros_msgs/srv/ParamSetV2
    • 사용하는 서비스 타입
  • param_id:
    • 설정할 파라미터 이름
  • value
    • 설정할 값
    • type: 2  ==> 설정할 값을 integer타입으로 설정하겠다.
ros2 service call /mavros/param/set mavros_msgs/srv/ParamSetV2 \
"{force_set: true, param_id: 'COM_ARM_WO_GPS',  value: {type: 2, integer_value: 1}}"

ros2 service call /mavros/param/set mavros_msgs/srv/ParamSetV2 \
"{force_set: true, param_id: 'CBRK_IO_SAFETY', value: {type: 2, integer_value: 22027}}"

ros2 service call /mavros/param/set mavros_msgs/srv/ParamSetV2 \
"{force_set: true, param_id: 'CBRK_USB_CHK',  value: {type: 2, integer_value: 197848}}"


ros2 service call /mavros/param/set mavros_msgs/srv/ParamSetV2 \
"{force_set: true, param_id: 'COM_RC_IN_MODE', value: {type: 2, integer_value: 1}}"
  • COM_ARM_NO_GPS
    • gps없이 드론을 arm할수있게 허용합니다.
    • 0 ==> GPS없다면, arm불가능
    • 1 ==> GPS가 없어도, ARM가능 ( 실내에서 GPS없이 드론을 arm하고 싶을때 사용 )
  • CBRK_IO_SAFETY
    • IO safety Breaker 파라미터이다. 하드웨어 안전장치를 비활성화한다.
      • 22027 ==> 안전 브레이커 해제
      • 0 ==> 안전기능 활성화
  • CBRK_USB_CHK
    • USB연결 상태 검사 우회
    • 197848 ==> USB연결 상태 무시 ==> 즉, USB가 연결되어 있어도 비행을 가능하게 한다.
  • COM_RC_IN_MODE
    • RC입력 방식을 설정
    • 0 ==> RC 수신기를 자동으로 탐지
    • 1 ==> MaVLink로 RC입력 받기.  ==> 즉, 실제 RC수신기가 없고, GCS or simulator로 RC를 제어할때 설정 

 

 

 

 

ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong \
"{broadcast: false, command: 246, confirmation: 0, \
param1: 1.0, param2: 0.0, param3: 0.0, param4: 0.0, param5: 0.0, param6: 0.0, param7: 0.0}"

 

 

 

 

9. 드론에 "속도0으로 가만히 있어라"라는 명령어 보내기 

----> 유사시에 이거를 종료

ros2 topic pub -r 20 /mavros/setpoint_velocity/cmd_vel_unstamped \
geometry_msgs/msg/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
  • ros2 topic pub
    • ROS2에서 topic에 message를 publish한다.
    • -r 20
      • 초당 20Hz로 반복해서 message를 보냄. ( OFFBOARD 모드 유지에 필수이다. )
    • / mavros / setpoint_velocity / cmd_vel_unstamped
      • MAVROS가 수신하는 velocity 명령 토픽
    • geometry_msgs / msg / Twist
      • 메시지 타입 ( 선속도 + 각속도 벡터 포함 )
    • {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}
      • 드론을 정지 상태로 유지하라는 의미.

 

10. OFFBOARD 모드로 전환 

ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode \
"{base_mode: 0, custom_mode: 'OFFBOARD'}"

 

 

 

 

11. 단순 arming 테스트

ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong \
"{broadcast: false, command: 400, confirmation: 0, \
param1: 1.0, param2: 0.0, param3: 0.0, param4: 0.0, param5: 0.0, param6: 0.0, param7: 0.0}"

 

 

 

12. arming 옵션을 true로 바꿔줌.

ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"

ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: false}"

 

 

 

ros2 topic pub -r 20 /mavros/setpoint_velocity/cmd_vel_unstamped \
geometry_msgs/msg/Twist '{}'

ros2 topic pub --times 60 -r 20 /mavros/setpoint_velocity/cmd_vel_unstamped \
geometry_msgs/msg/Twist '{linear: {x: 0.0, y: 0.0, z: 0.1}, angular: {x: 0.0, y: 0.0, z: 0.0}}'

 

 

13. 안전 하강 ( 착륙 )

ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{base_mode: 0, custom_mode: 'AUTO.LAND'}"