Mavros: استخدام plugin_gps plugin و vision_pose_estimation plugin مع Ardupilot Firmware

تم إنشاؤها على ٢ مارس ٢٠١٨ · 5تعليقات · مصدر: mavlink/mavros

تفاصيل المشكلة

أريد التحكم في الطائرة بدون طيار ( SkyViper ) في الداخل باستخدام الكاميرا الموجودة على متن الطائرة لتقدير الوضع. في الواقع ، أنا أبحث عن مشهد به الكثير من علامات ArUco وأقدر وضع طائرتى بدون طيار باستخدام كشف aruco وخوارزمية تقدير الوضع. قصة قصيرة طويلة ، أنا أستخدم الرؤية لتقدير وضع الطائرة بدون طيار سداسي الأبعاد. الآن أنوي إرسال هذه البيانات إلى FCU للتحكم في الطائرة بدون طيار في الوضع الموجه ، في الداخل. مع برنامج arducopter الثابت ، يبدو أن هناك شيئين يمكنني القيام بهما:
أ. استخدام البرنامج المساعد fake_pgs.
ب. استخدم vision_pose_estimate plugin
لكنني أواجه مشاكل مع كليهما. أريد أن أصف ما قمت به لكلا النهجين المذكورين أعلاه حتى يتمكن القارئ من مساعدتي ، في حال أخطأت في مكان ما.

أ. باستخدام fake_gps

تمت إضافة المكون الإضافي fake_gps إلى apm_config.yaml الخاص بي.
تم تحرير apm_pluginlists.yaml وإليكم كيف يبدو الجزء 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

أقوم بنشر الروبوت على rostopic / mavros / fake_gps / mocap / tf عند حوالي 4 هرتز (يمكنني زيادته حتى 30 هرتز ولكني أعتقد أن معلمة gps_rate تحدها ، راجع للشغل هل يمكنني زيادة GPS_rate؟)
أنا صدى / mavros / global_position / global لا أرى أي مخرجات ذات مغزى ،

هل يمكن لأي شخص مساعدتي هنا للحصول على هذا الشيء؟ هل هناك أي شيء آخر يجب أن أفعله؟ أي معلمات يجب علي تعيينها أو إلغاء تعيينها؟

ب. باستخدام Vision_pose

تمت إضافة المكون الإضافي fake_gps إلى apm_config.yaml الخاص بي.
تم تحرير apm_pluginlists.yaml وإليكم كيف يبدو جزء 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

أنشر وضع الروبوت على rostopic / mavros / vision_pose / pose
لقد أجريت بعض التعديلات في Vision_position_estimate.cpp واستخدم VISION_POSITION_DELTA بدلاً من VISION_POSITION_ESTIMATE .
لقد قمت بتعيين العلامات المناسبة في الكود أعلاه لمعرفة ما إذا تم تنفيذ هذا الرمز وطباعة تلك العلامات ، لذلك استنتج أن mavros يرسل البيانات إلى FCU ولكن ربما لا يوجد استقبال في نهاية FCU.

الكثير من الوقت الذي يقضيه في هذا ، أي مساعدة موضع تقدير كبير !!
ملاحظة: لقد كنت قادرًا على القيام بـ Vision_position_estimate في البرامج الثابتة PX4 ، ولم أجرب fake_gps هناك ولكن نظرًا لبعض القيود ، أريد أن أجعل هذا العمل على برنامج arducopter الثابت.

إصدار MAVROS والنظام الأساسي

مافروس: 0.18
روس: حركي
أوبونتو: 16.04

نوع الطيار الآلي والإصدار

[X] ArduPilot
[] PX4

الإصدار: 3.4.6.0

سجلات العقدة

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

APM extras mocap question vision

مصدر

SubMishMar

التعليق الأكثر فائدة

مرحبا SubMishMar ،

كنت أحاول أيضًا استخدام المكون الإضافي fake_gps لكنني لم أتمكن من معرفة كيفية استخدامه ولكن هذا ما فعلته ، ربما سيساعدك:

لقد أنشأت عقدة روس تأخذ بيانات الموقع من نظام Mocap (UWB في حالتي) وتحولها إلى قيم خطوط الطول والعرض. ثم قمت بنشر هذه القيم على موضوع / mavros / hil / gps. ثم قمت بتغيير معلمة GPS_TYPE في mavros إلى 14 (وهذا يجعل fcu يقبل بيانات GPS المزيفة). لقد تمكنت من تغذية بيانات المركز ، لكن لدي بعض المشاكل في تحقيق مركز جيد. أحاول تعديل القيم للحصول على نتيجة أفضل. أتمنى أن يساعدك هذا. سأكون ممتنًا إذا قام شخص ما بشرح كيفية استخدام مكوّن GPS المزيف بشكل صحيح :)

ملاحظة. يمكنك تغيير قيم lat0 و long0 إلى موقعك.

هذا هو رمز عقدة روس:

// 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 في ٦ مارس ٢٠١٨

👍4

ال 5 كومينتر

مرحبا SubMishMar ،

ملاحظة. يمكنك تغيير قيم lat0 و long0 إلى موقعك.

هذا هو رمز عقدة روس:

// 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 في ٦ مارس ٢٠١٨

👍4

mellonUAV شكرا .. سأحاول هذا

SubMishMar في ٦ مارس ٢٠١٨

mellonUAV لـ fake_gps تأكد من أن APM تعالج HIL_GPS الرسائل بشكل افتراضي دون الحاجة إلى تعيين أي معلمة أو نحو ذلك. أو ربما عليك فقط تعيين GPS_TYPE إلى 14 كما قال SubMishMar . بالنسبة إلى VISION_POSITION_DELTA لا أستطيع أن أقول الكثير ، فهذا يعتمد حقًا على نهاية الطيار الآلي. @ khancyr أي توصيات هنا؟
SubMishMar شفرتك محل تقدير ولكن fake_gps يقوم بالتحويل الذي تحتاجه بالضبط. ما الذي لا تفهمه من استخدامه؟

TSC21 في ٤ مايو ٢٠١٨

أهلا! @ TSC21
أريد أن يعمل جهاز PX4 في وضع OFFBOARD أو POSCTL ، فكلاهما مقبول. لدينا نظام mocap OptiTrack. لقد قرأت العديد من المشكلات على github حول px4 و mavros و mocap.

لكن لا يزال لدي بعض الأسئلة:

ذكر العديد من المستخدمين حزمة روس "مافروس" ، لكني أريد أن أعرف من أين أتت هذه "مافروس" ؟؟ هل يأتي من أمر التثبيت "sudo apt-get install ros-kinetic-mavros" أم أنه يأتي من git clone؟ هل هناك فرق بين الاثنين؟ في البداية ، لم أستخدم git one ، بعد "apt-get install" ، يمكن أن يعمل "roslaunch mavros px4.launch". لكن في الأسبوع الماضي حصلت على الكود المصدري عن طريق git clone ، وفي كود المصدر هذا ، يوجد أيضًا "px4.lunch" !!
لذا ، أريد أن أعرف أثناء استخدامنا mavros للتحكم في PX4 (Set_Point_Position ، Set_Point_Attitude وما إلى ذلك ...) ، ما هي "mavros" التي يجب أن نستخدمها ؟؟
أرى أيضًا بعض المستخدمين ذكروا المكون الإضافي "mocap_pose_estimate". قال العديد من المستخدمين أن هذا البرنامج المساعد يمكنه تحويل البيانات من mocap إلى نظام الإحداثيات الذي يعمل على إصلاح PX4 ، لكنني لا أعرف كيفية استخدامه ، فما هو؟ هل هي عقدة؟ موضوع مثل "/ mavros / mocap / pose"؟ أو فقط بعض الوظائف التي يمكننا استخدامها مباشرة؟
لذا ، أريد أن أعرف ما هو وكيف أستخدمه؟ كيف يمكنني إضافة هذا "البرنامج المساعد" إلى مشروعي الخاص ؟؟

شكرا جزيلا!
افضل الأمنيات!!

YiefengYang في ٢٧ مايو ٢٠١٨

ذكر العديد من المستخدمين حزمة روس "مافروس" ، لكني أريد أن أعرف من أين أتت هذه "مافروس" ؟؟

إنها بالضبط حزمة الريبو التي تعلق عليها.

هل يأتي من أمر التثبيت "sudo apt-get install ros-kinetic-mavros" أم أنه يأتي من git clone؟

هذا يعتمد على ما إذا كنت تريد استخدام نسخة إصدار (من deb) أو نسخة أولية (من git استنساخ الريبو).

هل هناك فرق بين الاثنين؟

يعتمد ذلك على ما إذا كان إصدار الإصدار متزامنًا مع الإصدار الرئيسي من المنبع ، ولكن نظرًا لأن هذا مستمر دائمًا من حيث التطوير ، فعادةً ما يكون إصدار الإصدار وراء الإصدار الرئيسي الرئيسي.

في البداية ، لم أستخدم git one ، بعد "apt-get install" ، يمكن أن يعمل "roslaunch mavros px4.launch". لكن في الأسبوع الماضي حصلت على الكود المصدري عن طريق git clone ، وفي كود المصدر هذا ، يوجد أيضًا "px4.lunch" !!

نعم هذا طبيعي. نسخة الإصدار هي هذا الريبو الذي تم إصداره في مستودعات Ubuntu distro.

لذا ، أريد أن أعرف أثناء استخدامنا mavros للتحكم في PX4 (Set_Point_Position ، Set_Point_Attitude وما إلى ذلك ...) ، ما هي "mavros" التي يجب أن نستخدمها ؟؟

يجب أن يعمل أي منها.

أرى أيضًا بعض المستخدمين ذكروا المكون الإضافي "mocap_pose_estimate". قال العديد من المستخدمين أن هذا المكون الإضافي يمكنه تحويل البيانات من mocap إلى نظام الإحداثيات الذي يعمل على إصلاح PX4 ، لكنني لا أعرف كيفية استخدامه. ما هو هل هي عقدة؟ موضوع مثل "/ mavros / mocap / pose"؟ أو فقط بعض الوظائف التي يمكننا استخدامها مباشرة؟

ربما إذا ألقيت نظرة على شفرة المصدر تجد إجابتك. إنه في الأساس مكون إضافي يمكنك العثور عليه في mavros_extras والذي يسمح لك بإرسال رسالة Mavlink ATT_POS_MOCAP Mavlink التي تم تحليلها من عقدة لديك والتي تحصل على بيانات من نظام mocap الخاص بك. لدى MAVROS وظائف للتعامل مع تحويل الإطار ، لذلك لا يقتصر الأمر على هذا المكون الإضافي.

لذا ، أريد أن أعرف ما هو وكيف أستخدمه؟ كيف يمكنني إضافة هذا "البرنامج المساعد" إلى مشروعي الخاص ؟؟

هذا ليس مكونًا إضافيًا تقوم بتوصيله بمشروعك الخاص لأن هذا مقترن بإحكام بـ MAVROS. إذا كنت تريد استخدام ما يسمح به هذا المكون الإضافي ، فما عليك سوى استخدام MAVROS وإقرانه بمشروعك.

TSC21 في ٢٩ مايو ٢٠١٨

👍1

هل كانت هذه الصفحة مفيدة؟

0 / 5 - 0 التقييمات