ν¬μ¦ μΆμ μ μν΄ μ¨λ³΄λ μΉ΄λ©λΌλ₯Ό μ¬μ©νμ¬ μ€λ΄μμ λλ‘ ( SkyViper )μ μ μ΄νκ³ μΆμ΅λλ€. μ¬μ€ μ λ λ§μ ArUco λ§μ»€κ°μλ μ₯λ©΄μ μ°Ύκ³ opencvμ aruco νμ§ λ° ν¬μ¦ μΆμ μκ³ λ¦¬μ¦μ μ¬μ©νμ¬ λλ‘ μ ν¬μ¦λ₯Ό μΆμ νκ³ μμ΅λλ€. κ°λ¨ν λ§ν΄μ λΉμ μ μ¬μ©νμ¬ λλ‘ μ 6D ν¬μ¦λ₯Ό μΆμ νκ³ μμ΅λλ€. μ΄μ μ΄ λ°μ΄ν°λ₯Ό FCUλ‘ μ μ‘νμ¬ μ€λ΄μ κ°μ΄λ λͺ¨λμμ λλ‘ μ μ μ΄νλ €κ³ ν©λλ€. arducopter νμ¨μ΄λ₯Ό μ¬μ©νλ©΄ λΆλͺ
ν λ κ°μ§ μμ
μ μν ν μ μμ΅λλ€.
γ
. fake_pgs νλ¬κ·ΈμΈμ μ¬μ©νμμμ€.
λΉ. vision_pose_estimate νλ¬κ·ΈμΈ μ¬μ©
κ·Έλ¬λ λλ λ κ°μ§ λͺ¨λμ λ¬Έμ κ° μμ΅λλ€. λλ λ΄κ° μ΄λκ°μμ μλͺ»λμμ λ λ
μκ° λλ₯Ό λμΈ μ μλλ‘ μμ μΈκΈ ν λ κ°μ§ μ κ·Όλ²μ λν΄ λ΄κ° ν μΌμ μ€λͺ
νκ³ μΆλ€.
γ . fake_gps μ¬μ©
# fake_gps
fake_gps:
# select data source
use_mocap: true # ~mocap/pose
mocap_transform: true # ~mocap/tf instead of pose
use_vision: false # ~vision (pose)
# origin (default: ZΓΌrich)
geo_origin:
lat: 30.6197674 # latitude [degrees]
lon: -96.3414215 # longitude [degrees]
alt: 99.9674829152 # altitude (height over the WGS-84 ellipsoid) [meters]
eph: 2.0
epv: 2.0
satellites_visible: 10 # virtual number of visible satellites
fix_type: 3 # type of GPS fix (default: 3D)
tf:
listen: false
send: false # send TF?
frame_id: "map" # TF frame_id
child_frame_id: "Quadrotor_pose" # TF child_frame_id
rate_limit: 10.0 # TF rate
gps_rate: 5.0 # GPS data publishing rate
λꡬλ μ§μ΄ μΌμ΄ μλνλλ‘ μ¬κΈ°μμ λλ₯Ό λμΈ μ μμ΅λκΉ? λ€λ₯Έ μ‘°μΉκ° νμν©λκΉ? μ€μ νκ±°λ μ€μ ν΄μ ν΄μΌνλ λ§€κ° λ³μκ° μμ΅λκΉ?
λΉ. vision_pose μ¬μ©
# vision_pose_estimate
vision_pose:
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "map"
child_frame_id: "vision_estimate"
rate_limit: 10.0
VISION_POSITION_DELTA
λμ VISION_POSITION_ESTIMATE
ν©λλ€.μ΄κ²μ λ§μ μκ°μ 보λμ΅λλ€, μ΄λ€ λμμ΄λΌλ λλ¨ν κ°μ¬ν©λλ€!
μΆμ : PX4 νμ¨μ΄μμ vision_position_estimateλ₯Ό μν ν μ μμκ³ κ±°κΈ°μμ fake_gpsλ₯Ό μλνμ§ μμμ§λ§ λͺ κ°μ§ μ μ½μΌλ‘ μΈν΄ arducopter νμ¨μ΄ μμμ΄ μμ
μνκ³ μΆμ΅λλ€.
λ§ λΈλ‘μ€ : 0.18
ROS : ν€λ€ν±
Ubuntu : 16.04
[X] ArduPilot
[] PX4
λ²μ : 3.4.6
started roslaunch server http://subodh-desktop:41065/
SUMMARY
========
CLEAR PARAMETERS
* /mavros/
PARAMETERS
* /mavros/cmd/use_comp_id_system_control: False
* /mavros/conn/heartbeat_rate: 1.0
* /mavros/conn/system_time_rate: 0.0
* /mavros/conn/timeout: 10.0
* /mavros/conn/timesync_rate: 0.0
* /mavros/distance_sensor/rangefinder_pub/field_of_view: 0.0
* /mavros/distance_sensor/rangefinder_pub/frame_id: lidar
* /mavros/distance_sensor/rangefinder_pub/id: 0
* /mavros/distance_sensor/rangefinder_pub/send_tf: False
* /mavros/distance_sensor/rangefinder_pub/sensor_position/x: 0.0
* /mavros/distance_sensor/rangefinder_pub/sensor_position/y: 0.0
* /mavros/distance_sensor/rangefinder_pub/sensor_position/z: -0.1
* /mavros/distance_sensor/rangefinder_sub/id: 1
* /mavros/distance_sensor/rangefinder_sub/orientation: PITCH_270
* /mavros/distance_sensor/rangefinder_sub/subscriber: True
* /mavros/fake_gps/eph: 2.0
* /mavros/fake_gps/epv: 2.0
* /mavros/fake_gps/fix_type: 3
* /mavros/fake_gps/geo_origin/alt: 99.9674829152
* /mavros/fake_gps/geo_origin/lat: 30.6197674
* /mavros/fake_gps/geo_origin/lon: -96.3414215
* /mavros/fake_gps/gps_rate: 5.0
* /mavros/fake_gps/mocap_transform: True
* /mavros/fake_gps/satellites_visible: 10
* /mavros/fake_gps/tf/child_frame_id: Quadrotor_pose
* /mavros/fake_gps/tf/frame_id: map
* /mavros/fake_gps/tf/listen: False
* /mavros/fake_gps/tf/rate_limit: 10.0
* /mavros/fake_gps/tf/send: False
* /mavros/fake_gps/use_mocap: True
* /mavros/fake_gps/use_vision: False
* /mavros/fcu_protocol: v2.0
* /mavros/fcu_url: /dev/ttyACM0:57600
* /mavros/gcs_url:
* /mavros/global_position/child_frame_id: base_link
* /mavros/global_position/frame_id: map
* /mavros/global_position/rot_covariance: 99999.0
* /mavros/global_position/tf/child_frame_id: base_link
* /mavros/global_position/tf/frame_id: map
* /mavros/global_position/tf/global_frame_id: earth
* /mavros/global_position/tf/send: False
* /mavros/global_position/use_relative_alt: True
* /mavros/image/frame_id: px4flow
* /mavros/imu/angular_velocity_stdev: 0.000349065850399
* /mavros/imu/frame_id: base_link
* /mavros/imu/linear_acceleration_stdev: 0.0003
* /mavros/imu/magnetic_stdev: 0.0
* /mavros/imu/orientation_stdev: 1.0
* /mavros/local_position/frame_id: map
* /mavros/local_position/tf/child_frame_id: base_link
* /mavros/local_position/tf/frame_id: map
* /mavros/local_position/tf/send: False
* /mavros/local_position/tf/send_fcu: False
* /mavros/mission/pull_after_gcs: True
* /mavros/mocap/use_pose: False
* /mavros/mocap/use_tf: True
* /mavros/odometry/estimator_type: 3
* /mavros/odometry/frame_tf/desired_frame: ned
* /mavros/plugin_blacklist: ['actuator_contro...
* /mavros/plugin_whitelist: ['fake_gps', 'vis...
* /mavros/px4flow/frame_id: px4flow
* /mavros/px4flow/ranger_fov: 0.118682389136
* /mavros/px4flow/ranger_max_range: 5.0
* /mavros/px4flow/ranger_min_range: 0.3
* /mavros/safety_area/p1/x: 1.0
* /mavros/safety_area/p1/y: 1.0
* /mavros/safety_area/p1/z: 1.0
* /mavros/safety_area/p2/x: -1.0
* /mavros/safety_area/p2/y: -1.0
* /mavros/safety_area/p2/z: -1.0
* /mavros/setpoint_accel/send_force: False
* /mavros/setpoint_attitude/reverse_thrust: False
* /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
* /mavros/setpoint_attitude/tf/frame_id: map
* /mavros/setpoint_attitude/tf/listen: False
* /mavros/setpoint_attitude/tf/rate_limit: 50.0
* /mavros/setpoint_attitude/use_quaternion: False
* /mavros/setpoint_position/mav_frame: LOCAL_NED
* /mavros/setpoint_position/tf/child_frame_id: target_position
* /mavros/setpoint_position/tf/frame_id: map
* /mavros/setpoint_position/tf/listen: False
* /mavros/setpoint_position/tf/rate_limit: 50.0
* /mavros/setpoint_velocity/mav_frame: LOCAL_NED
* /mavros/startup_px4_usb_quirk: False
* /mavros/sys/disable_diag: False
* /mavros/sys/min_voltage: 10.0
* /mavros/target_component_id: 1
* /mavros/target_system_id: 1
* /mavros/tdr_radio/low_rssi: 40
* /mavros/time/time_ref_source: fcu
* /mavros/time/timesync_avg_alpha: 0.6
* /mavros/time/timesync_mode: NONE
* /mavros/vibration/frame_id: base_link
* /mavros/vision_pose/tf/child_frame_id: Quadrotor_pose
* /mavros/vision_pose/tf/frame_id: map
* /mavros/vision_pose/tf/listen: False
* /mavros/vision_pose/tf/rate_limit: 10.0
* /mavros/vision_speed/listen_twist: False
* /rosdistro: kinetic
* /rosversion: 1.12.12
NODES
/
mavros (mavros/mavros_node)
auto-starting new master
process[master]: started with pid [26930]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to af2a9730-1e56-11e8-86d2-001fc69c99e7
process[rosout-1]: started with pid [26943]
started core service [/rosout]
process[mavros-2]: started with pid [26961]
[ INFO] [1520021829.553095960]: FCU URL: /dev/ttyACM0:57600
[ INFO] [1520021829.554915572]: serial0: device: /dev/ttyACM0 @ 57600 bps
[ INFO] [1520021829.555552991]: GCS bridge disabled
[ INFO] [1520021829.567142748]: Plugin 3dr_radio loaded
[ INFO] [1520021829.569320914]: Plugin 3dr_radio initialized
[ INFO] [1520021829.569349388]: Plugin actuator_control blacklisted
[ INFO] [1520021829.571809744]: Plugin adsb loaded
[ INFO] [1520021829.575769648]: Plugin adsb initialized
[ INFO] [1520021829.575794473]: Plugin altitude blacklisted
[ INFO] [1520021829.575892237]: Plugin cam_imu_sync loaded
[ INFO] [1520021829.576626653]: Plugin cam_imu_sync initialized
[ INFO] [1520021829.576772377]: Plugin command loaded
[ INFO] [1520021829.582684066]: Plugin command initialized
[ INFO] [1520021829.582712464]: Plugin debug_value blacklisted
[ INFO] [1520021829.582886299]: Plugin distance_sensor loaded
[ INFO] [1520021829.592604674]: Plugin distance_sensor initialized
[ INFO] [1520021829.592748090]: Plugin fake_gps loaded
[ INFO] [1520021829.609103603]: Plugin fake_gps initialized
[ INFO] [1520021829.609128115]: Plugin ftp blacklisted
[ INFO] [1520021829.609259829]: Plugin global_position loaded
[ INFO] [1520021829.628580110]: Plugin global_position initialized
[ INFO] [1520021829.628612736]: Plugin hil blacklisted
[ INFO] [1520021829.628766952]: Plugin home_position loaded
[ INFO] [1520021829.633001155]: Plugin home_position initialized
[ INFO] [1520021829.633140461]: Plugin imu loaded
[ INFO] [1520021829.640533947]: Plugin imu initialized
[ INFO] [1520021829.640671494]: Plugin local_position loaded
[ INFO] [1520021829.646783905]: Plugin local_position initialized
[ INFO] [1520021829.646948100]: Plugin manual_control loaded
[ INFO] [1520021829.650807807]: Plugin manual_control initialized
[ INFO] [1520021829.650943083]: Plugin mocap_pose_estimate loaded
[ INFO] [1520021829.655518964]: Plugin mocap_pose_estimate initialized
[ INFO] [1520021829.655642737]: Plugin obstacle_distance loaded
[ INFO] [1520021829.659014223]: Plugin obstacle_distance initialized
[ INFO] [1520021829.659130864]: Plugin odom loaded
[ INFO] [1520021829.663794739]: Plugin odom initialized
[ INFO] [1520021829.664012214]: Plugin param loaded
[ INFO] [1520021829.667288846]: Plugin param initialized
[ INFO] [1520021829.667312025]: Plugin px4flow blacklisted
[ INFO] [1520021829.667423654]: Plugin rangefinder loaded
[ INFO] [1520021829.668137088]: Plugin rangefinder initialized
[ INFO] [1520021829.668297342]: Plugin rc_io loaded
[ INFO] [1520021829.672569622]: Plugin rc_io initialized
[ INFO] [1520021829.672594510]: Plugin safety_area blacklisted
[ INFO] [1520021829.672725799]: Plugin setpoint_accel loaded
[ INFO] [1520021829.676462007]: Plugin setpoint_accel initialized
[ INFO] [1520021829.676710869]: Plugin setpoint_attitude loaded
[ INFO] [1520021829.689281332]: Plugin setpoint_attitude initialized
[ INFO] [1520021829.689452779]: Plugin setpoint_position loaded
[ INFO] [1520021829.705853198]: Plugin setpoint_position initialized
[ INFO] [1520021829.706010896]: Plugin setpoint_raw loaded
[ INFO] [1520021829.716862466]: Plugin setpoint_raw initialized
[ INFO] [1520021829.717029302]: Plugin setpoint_velocity loaded
[ INFO] [1520021829.723754613]: Plugin setpoint_velocity initialized
[ INFO] [1520021829.723998780]: Plugin sys_status loaded
[ INFO] [1520021829.730640327]: Plugin sys_status initialized
[ INFO] [1520021829.730800888]: Plugin sys_time loaded
[ INFO] [1520021829.734651825]: TM: Timesync mode: NONE
[ INFO] [1520021829.735319038]: Plugin sys_time initialized
[ INFO] [1520021829.735450495]: Plugin vfr_hud loaded
[ INFO] [1520021829.736911489]: Plugin vfr_hud initialized
[ INFO] [1520021829.736935514]: Plugin vibration blacklisted
[ INFO] [1520021829.737033502]: Plugin vision_pose_estimate loaded
[ INFO] [1520021829.746346315]: Plugin vision_pose_estimate initialized
[ INFO] [1520021829.746515765]: Plugin vision_speed_estimate loaded
[ INFO] [1520021829.750239950]: Plugin vision_speed_estimate initialized
[ INFO] [1520021829.750419761]: Plugin waypoint loaded
[ INFO] [1520021829.755638602]: Plugin waypoint initialized
[ INFO] [1520021829.755726057]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1520021829.755751539]: Built-in MAVLink package version: 2018.2.2
[ INFO] [1520021829.755767668]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1520021829.755795346]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1520021829.849907257]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega / ArduCopter
[ INFO] [1520021830.072218261]: IMU: Raw IMU message used.
[ INFO] [1520021830.075098601]: RC_CHANNELS message detected!
[ WARN] [1520021830.075901836]: TM: Wrong FCU time.
[ WARN] [1520021830.572875238]: GP: No GPS fix
[ INFO] [1520021830.859501557]: VER: 1.1: Capabilities 0x0000000000003bcf
[ INFO] [1520021830.859610755]: VER: 1.1: Flight software: 030406ff (e707341bde6b667d8c965992)
[ INFO] [1520021830.859692086]: VER: 1.1: Middleware software: 00000000 (de6b667d8c965992)
[ INFO] [1520021830.859811288]: VER: 1.1: OS software: 00000000 (8c965992)
[ INFO] [1520021830.859894423]: VER: 1.1: Board hardware: 00000000
[ INFO] [1520021830.859991754]: VER: 1.1: VID/PID: 0000:0000
[ INFO] [1520021830.860091685]: VER: 1.1: UID: 0000000000000000
[ WARN] [1520021830.860457816]: CMD: Unexpected command 520, result 0
[ERROR] [1520021833.850124648]: FCU: PreArm: RC not calibrated
[ERROR] [1520021833.850282817]: FCU: PreArm: Compass offsets too high
[ INFO] [1520021839.850783561]: HP: requesting home position
[ INFO] [1520021839.853771718]: FCU: APM:Copter V3.4.6 (e707341b)
[ INFO] [1520021839.854040516]: FCU: PX4: de6b667d NuttX: 8c965992
[ INFO] [1520021839.854183948]: FCU: Frame: QUAD
[ INFO] [1520021839.854286161]: FCU: PX4v2 0034002F 31334705 39343031
[ INFO] [1520021840.469965330]: PR: parameters list received
[ INFO] [1520021844.857490824]: WP: mission received
[ INFO] [1520021849.851115121]: HP: requesting home position
[ INFO] [1520021859.850746668]: HP: requesting home position
[ WARN] [1520021860.684726116]: GP: No GPS fix
[ERROR] [1520021863.861805572]: FCU: PreArm: RC not calibrated
[ERROR] [1520021863.861953300]: FCU: PreArm: Compass offsets too high
header:
seq: 59
stamp:
secs: 1520021901
nsecs: 257080897
frame_id: ''
status:
-
level: 0
name: "mavros: FCU connection"
message: "connected"
hardware_id: "/dev/ttyACM0:57600"
values:
-
key: "Received packets:"
value: "6240"
-
key: "Dropped packets:"
value: "0"
-
key: "Buffer overruns:"
value: "0"
-
key: "Parse errors:"
value: "0"
-
key: "Rx sequence number:"
value: "211"
-
key: "Tx sequence number:"
value: "82"
-
key: "Rx total bytes:"
value: "182353"
-
key: "Tx total bytes:"
value: "1883"
-
key: "Rx speed:"
value: "2106.000000"
-
key: "Tx speed:"
value: "21.000000"
-
level: 2
name: "mavros: GPS"
message: "No satellites"
hardware_id: "/dev/ttyACM0:57600"
values:
-
key: "Satellites visible"
value: "0"
-
key: "Fix type"
value: "0"
-
key: "EPH (m)"
value: "0.00"
-
key: "EPV (m)"
value: "0.00"
-
level: 0
name: "mavros: Heartbeat"
message: "Normal"
hardware_id: "/dev/ttyACM0:57600"
values:
-
key: "Heartbeats since startup"
value: "72"
-
key: "Frequency (Hz)"
value: "1.037047"
-
key: "Vehicle type"
value: "Quadrotor"
-
key: "Autopilot type"
value: "ArduPilotMega / ArduCopter"
-
key: "Mode"
value: "STABILIZE"
-
key: "System status"
value: "Standby"
-
level: 0
name: "mavros: System"
message: "Normal"
hardware_id: "/dev/ttyACM0:57600"
values:
-
key: "Sensor present"
value: "0x0160FC0F"
-
key: "Sensor enabled"
value: "0x00601C0F"
-
key: "Sensor helth"
value: "0x0160FC0F"
-
key: "3D gyro"
value: "Ok"
-
key: "3D accelerometer"
value: "Ok"
-
key: "3D magnetometer"
value: "Ok"
-
key: "absolute pressure"
value: "Ok"
-
key: "3D angular rate control"
value: "Ok"
-
key: "attitude stabilization"
value: "Ok"
-
key: "yaw position"
value: "Ok"
-
key: "AHRS subsystem health"
value: "Ok"
-
key: "Terrain subsystem health"
value: "Ok"
-
key: "CPU Load (%)"
value: "30.1"
-
key: "Drop rate (%)"
value: "0.0"
-
key: "Errors comm"
value: "0"
-
key: "Errors count #1"
value: "0"
-
key: "Errors count #2"
value: "0"
-
key: "Errors count #3"
value: "0"
-
key: "Errors count #4"
value: "0"
-
level: 1
name: "mavros: Battery"
message: "Low voltage"
hardware_id: "/dev/ttyACM0:57600"
values:
-
key: "Voltage"
value: "0.00"
-
key: "Current"
value: "-0.0"
-
key: "Remaining"
value: "-1.0"
---
OK. I got messages from 1:1.
---
Received 1131 messages, from 1 addresses
sys:comp list of messages
1:1 0, 1, 2, 193, 152, 150, 24, 27, 29, 30, 33, 163, 36, 165, 42, 178, 182, 62, 65, 74, 77, 241, 116, 125
μλ νμΈμ @SubMishMar ,
λν fake_gps νλ¬κ·ΈμΈμ μ¬μ©νλ €κ³ νμ§λ§ μ¬μ© λ°©λ²μ μ μ μμμ§λ§ μ¬κΈ°μ λ΄κ° ν μΌμ΄ μμ΅λλ€. μλ§λ λμμ΄ λ κ²μ λλ€.
Mocap μμ€ν (λ΄ κ²½μ°μλ UWB)μμ μμΉ λ°μ΄ν°λ₯Ό κ°μ Έμ μλ κ²½λ κ°μΌλ‘ λ³ννλ ros λ Έλλ₯Ό λ§λ€μμ΅λλ€. κ·Έλ° λ€μμ΄ κ°μ / mavros / hil / gps μ£Όμ μ κ²μνμ΅λλ€. κ·Έλ° λ€μ mavrosμ GPS_TYPE λ§€κ° λ³μλ₯Ό 14λ‘ λ³κ²½νμ΅λλ€ (μ΄λ‘ μΈν΄ fcuκ° κ°μ§ gps λ°μ΄ν°λ₯Ό νμ© ν¨). μμΉ λ°μ΄ν°λ₯Ό μ 곡 ν μ μμμ§λ§ μ’μ μμΉ μ μ§λ₯Ό λ¬μ±νλ λ° λͺ κ°μ§ λ¬Έμ κ° μμ΅λλ€. λ λμ κ²°κ³Όλ₯Ό μ»κΈ° μν΄ κ°μ μ‘°μ νλ €κ³ ν©λλ€. λμμ΄ λμκΈ°λ₯Ό λ°λλλ€. λκ΅°κ°κ° κ°μ§ gps νλ¬κ·ΈμΈμ μ¬λ°λ₯΄κ² μ¬μ©νλ λ°©λ²μ μ€λͺ νλ©΄ κ°μ¬νκ² μ΅λλ€ :)
μΆμ . lat0 λ° long0 κ°μ μ¬μ©μ μμΉλ‘ λ³κ²½ν μ μμ΅λλ€.
λ€μμ ros λ Έλ μ½λμ λλ€.
// adding x, y, z to long lat transform
// v3 - taking data from UWB and sending to APM
#include <ros/ros.h>
//#include <geometry_msgs/PoseStamped.h>
#include <cstdlib>
#include <mavros_msgs/HilGPS.h>
//#include <mavros_msgs/OverrideRCIn.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <array>
#include <geometry_msgs/PoseStamped.h>
std::vector <float> GPS = {0, 0}; // {long, lat}
float xPos = 0;
float yPos = 0;
float zPos = 0;
std::vector <float> CartToGPS (float x, float y, float z){ // x, y, z in [mm]
std::vector <float> GPS = {0, 0}; // {long, lat}
//Some constants
float pi = 3.14159265359; // pi
float alpha = pi / 2; // angle between x axis and North
float r = 6371000000; // mean radius of Earh in [mm]
float lat0 = (39.894470 * pi) / 180; // origin latitude
float long0 = (32.777973 * pi) / 180; // origin longitude
float dlat = (x * cos(alpha) + y * sin(alpha)) / r; // [rad]
float dlong = (x * sin(alpha) - y * cos(alpha)) / r; // [rad]
GPS = {((long0 + dlong) * 180) / pi, ((lat0 + dlat) * 180) / pi};
return GPS;
}
void poseMessageReceived(const geometry_msgs::PoseStamped &msg) // callback function for pose Message
{
xPos = msg.pose.position.x;
yPos = msg.pose.position.y;
zPos = msg.pose.position.z;
//ROS_INFO_STREAM("callback");
}
int main(int argc, char **argv) {
// if(argc<4){
// printf("Usage: rosrun send_pose position_sender lat long alt\n");
// return -1;
// }
//int rate = 10;
ros::init(argc, argv, "position_sender");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<mavros_msgs::HilGPS>("mavros/hil/gps", 1);
ros::Subscriber subPose = n.subscribe("UWB_Pos",5,&poseMessageReceived);
ros::Rate rate(5);
while(ros::ok()){
mavros_msgs::HilGPS msg;
//msg.header.frame_id = "1";
msg.header.stamp=ros::Time::now();
// float x = double(atoi(argv[1]));
// float y = double(atoi(argv[2]));
// float z = double(atoi(argv[3]));
GPS = CartToGPS(xPos, yPos, zPos);
//msg.header.stamp=ros::Time::now();
msg.fix_type = 3;
msg.geo.longitude = GPS[0];
msg.geo.latitude = GPS[1];
msg.geo.altitude = zPos / 10000;
// msg.eph = 65535;
// msg.epv = 65535;
// msg.vel = 65535;
// msg.vn = 0;
// msg.ve = 0;
// msg.vd = 0;
// msg.cog = 65535;
// msg.satellites_visible = 255;
msg.eph = 2.0;
msg.epv = 2.0;
msg.vel = 65535;
msg.vn = 0;
msg.ve = 0;
msg.vd = 0;
msg.cog = 65535;
msg.satellites_visible = 255;
//ROS_INFO("Position data sent %f, %f, %f", msg.geo.latitude, msg.geo.longitude, msg.geo.altitude);
pub.publish(msg);
// rc_override_pub.publish(msg_override);
ros::spinOnce();
rate.sleep();
}
}
@mellonUAV κ°μ¬ν©λλ€ .. λλ μ΄κ²μ μλνκ³ μ΄λ»κ² μ§νλλμ§ μλ €μ€ κ²μ λλ€.
fake_gps
λν @mellonUAV λ APMμ΄ λ§€κ° λ³μ λ±μ μ€μ ν νμμμ΄ κΈ°λ³Έμ μΌλ‘ HIL_GPS
λ©μμ§λ₯Ό μ²λ¦¬νλμ§ νμΈν©λλ€. λλ @SubMishMarκ° λ§νλ―μ΄ GPS_TYPE
μ 14
λ‘ μ€μ ν΄μΌ ν μλ μμ΅λλ€. VISION_POSITION_DELTA
λ§μ΄ λ§ν μλ μμ§λ§ μ€μ λ‘λ μλ μ‘°μ’
μ₯μΉ μ’
λ£μ λ°λΌ λ€λ¦
λλ€. @khancyr μ¬κΈ°μ κΆμ₯ μ¬νμ΄ μμ΅λκΉ?
@SubMishMar κ·νμ μ½λλ κ°μ¬νμ§λ§ fake_gps
λ νμν λ³νμ μ ννκ² μνν©λλ€. μ¬μ©λ²μ λν΄ μ΄ν΄νμ§ λͺ»νλ κ²μ 무μμ
λκΉ?
μλ
νμΈμ! ν
PX4κ° OFFBOARD λλ POSCTL λͺ¨λμμ μλνκΈ°λ₯Ό μν©λλ€. λ λ€ νμ©λ©λλ€. mocap μμ€ν
OptiTrackμ΄ μμ΅λλ€. px4, mavros λ° mocapμ λν githubμμ λ§μ λ¬Έμ λ₯Ό μ½μμ΅λλ€.
νμ§λ§ μ¬μ ν λͺ κ°μ§ μ§λ¬Έμ΄ μμ΅λλ€.
λλ¨ν κ°μ¬ν©λλ€!
μ΅κ³ μ μμ !!
λ§μ μ¬μ©μκ° ros ν¨ν€μ§ 'mavros'λ₯Ό μΈκΈνμ§λ§μ΄ 'mavros'κ° μ΄λμμ μλμ§ μκ³ μΆμ΅λλ€.?
λκΈμ λ¬κ³ μλ μ μ₯μμ ν¨ν€μ§μ λλ€.
μ€μΉ λͺ λ Ή "sudo apt-get install ros-kinetic-mavros"μμ μ€λ κ²μ λκΉ, μλλ©΄ git cloneμμ μ€λ κ²μ λκΉ?
λ¦΄λ¦¬μ€ λ²μ (debμ) λλ μ μ€νΈλ¦Ό λ²μ (μ μ₯μλ₯Ό 볡μ νλ gitμμ)μ μ¬μ© νλ €λμ§ μ¬λΆμ λ°λΌ λ€λ¦ λλ€.
λ μ¬μ΄μ μ°¨μ΄μ μ΄ μμ΅λκΉ?
λ¦΄λ¦¬μ€ λ²μ μ΄ μ μ€νΈλ¦Ό λ§μ€ν°μ λκΈ°νλλμ§ μ¬λΆμ λ°λΌ λ€λ₯΄μ§λ§ μ΄λ κ°λ° μΈ‘λ©΄μμ νμ μ§ν μ€μ΄λ―λ‘ μΌλ°μ μΌλ‘ λ¦΄λ¦¬μ€ λ²μ μ μ μ€νΈλ¦Ό λ§μ€ν° λ€μ μμ΅λλ€.
μ²μμλ "apt-get install", "roslaunch mavros px4.launch"κ° μλ ν νμ gitμ μ¬μ©νμ§ μμμ΅λλ€. νμ§λ§ μ§λμ£Όμ μ λ git cloneμΌλ‘ μμ€ μ½λλ₯Ό μ»μμ΅λλ€.μ΄ μμ€ μ½λμλ "px4.launch"λ μμ΅λλ€ !!
λ€, μ μμ λλ€. λ¦΄λ¦¬μ€ λ²μ μ Ubuntu λ°°ν¬ μ μ₯μμ λ¦΄λ¦¬μ€ λμ΄ μ μ₯μμ λλ€.
κ·Έλμ μ°λ¦¬κ° PX4 (Set_Point_Position, Set_Point_Attitude λ± ......)λ₯Ό μ μ΄νκΈ° μν΄ mavrosλ₯Ό μ¬μ©νλ λμ μ΄λ€ 'mavros'λ₯Ό μ¬μ©ν΄μΌνλμ§ μκ³ μΆμ΅λλ€.
λͺ¨λ μλν©λλ€.
λν μΌλΆ μ¬μ©μκ° "mocap_pose_estimate"νλ¬κ·ΈμΈμ μΈκΈ ν κ²μ 보μμ΅λλ€. λ§μ μ¬μ©μκ°μ΄ νλ¬κ·ΈμΈμ΄ mocapμμ PX4λ₯Ό μμ νλ μ’νκ³λ‘ λ°μ΄ν°λ₯Ό λ³ν ν μ μλ€κ³ λ§νμ§λ§ μ¬μ© λ°©λ²μ λͺ¨λ₯΄κ² μ΅λλ€. λμΌ? λ Έλμ λκΉ? '/ mavros / mocap / pose'μ κ°μ μ£Όμ ? μλλ©΄ μ°λ¦¬κ° μ§μ μ¬μ©ν μμλ λͺ κ°μ§ κΈ°λ₯μΌκΉμ?
μμ€ μ½λ λ₯Ό μ΄ν΄λ³΄λ©΄ λ΅μ μ°Ύμ μ μμ΅λλ€. κΈ°λ³Έμ μΌλ‘ mavros_extras
μμ μ°Ύμ μμλ νλ¬κ·ΈμΈμΌλ‘ mocap μμ€ν
μμ λ°μ΄ν°λ₯Ό κ°μ Έ μ€λ λ
Έλμμ ꡬ문 λΆμ λ ATT_POS_MOCAP
Mavlink λ©μμ§λ₯Ό λ³΄λΌ μ μμ΅λλ€. MAVROSμλ νλ μ λ³νμ μ²λ¦¬νλ κΈ°λ₯μ΄ μμΌλ―λ‘ ν΄λΉ νλ¬κ·ΈμΈμλ§ κ΅νλμ§ μμ΅λλ€.
κ·Έλμ λλ κ·Έκ²μ΄ 무μμ΄λ©° μ΄λ»κ² μ¬μ©νλμ§ μκ³ μΆμ΅λλ€. μ΄ "νλ¬κ·ΈμΈ"μ λ΄ νλ‘μ νΈμ μ΄λ»κ² μΆκ° ν μ μμ΅λκΉ ??
μ΄κ²μ MAVROSμ λ°μ νκ² κ²°ν©λμ΄ μκΈ° λλ¬Έμ μμ μ νλ‘μ νΈμ μ°κ²°νλ νλ¬κ·ΈμΈμ΄ μλλλ€. μ΄ νλ¬κ·ΈμΈμ΄ νμ©νλ κ²μ μ¬μ©νλ €λ©΄ MAVROSλ₯Ό μ¬μ©νκ³ νλ‘μ νΈμ μ°κ²°νμμμ€.
κ°μ₯ μ μ©ν λκΈ
μλ νμΈμ @SubMishMar ,
λν fake_gps νλ¬κ·ΈμΈμ μ¬μ©νλ €κ³ νμ§λ§ μ¬μ© λ°©λ²μ μ μ μμμ§λ§ μ¬κΈ°μ λ΄κ° ν μΌμ΄ μμ΅λλ€. μλ§λ λμμ΄ λ κ²μ λλ€.
Mocap μμ€ν (λ΄ κ²½μ°μλ UWB)μμ μμΉ λ°μ΄ν°λ₯Ό κ°μ Έμ μλ κ²½λ κ°μΌλ‘ λ³ννλ ros λ Έλλ₯Ό λ§λ€μμ΅λλ€. κ·Έλ° λ€μμ΄ κ°μ / mavros / hil / gps μ£Όμ μ κ²μνμ΅λλ€. κ·Έλ° λ€μ mavrosμ GPS_TYPE λ§€κ° λ³μλ₯Ό 14λ‘ λ³κ²½νμ΅λλ€ (μ΄λ‘ μΈν΄ fcuκ° κ°μ§ gps λ°μ΄ν°λ₯Ό νμ© ν¨). μμΉ λ°μ΄ν°λ₯Ό μ 곡 ν μ μμμ§λ§ μ’μ μμΉ μ μ§λ₯Ό λ¬μ±νλ λ° λͺ κ°μ§ λ¬Έμ κ° μμ΅λλ€. λ λμ κ²°κ³Όλ₯Ό μ»κΈ° μν΄ κ°μ μ‘°μ νλ €κ³ ν©λλ€. λμμ΄ λμκΈ°λ₯Ό λ°λλλ€. λκ΅°κ°κ° κ°μ§ gps νλ¬κ·ΈμΈμ μ¬λ°λ₯΄κ² μ¬μ©νλ λ°©λ²μ μ€λͺ νλ©΄ κ°μ¬νκ² μ΅λλ€ :)
μΆμ . lat0 λ° long0 κ°μ μ¬μ©μ μμΉλ‘ λ³κ²½ν μ μμ΅λλ€.
λ€μμ ros λ Έλ μ½λμ λλ€.