Mavros: Ardupilotファームウェアでfake_gpsプラグインとvision_pose_estimationプラグインを使用する

作成日 2018年03月02日 · 5コメント · ソース: mavlink/mavros

問題の詳細

車載カメラでポーズ推定を行い、ドローン（
a。 fake_pgsプラグインを使用します。
b。 vision_pose_estimateプラグインを使用する
しかし、私は両方で問題に直面しています。私がどこかで間違った場合に読者が私を助けることができるように、私が前述の両方のアプローチに対して私がしたことを説明したいと思います。

a。 fake_gpsを使用する

apm_config.yamlにfake_gpsプラグインを追加しました。
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上で約4Hzで公開しています（30 Hzまで増やすことができますが、gps_rateパラメーターで制限されると思いますが、gps_rateを増やすことはできますか？）
/ mavros / global_position / globalをエコーします意味のある出力が表示されません。

誰かがこのことを機能させるためにここで私を助けてくれますか？他に何かする必要がありますか？設定または設定解除する必要のあるパラメータはありますか？

b。 Vision_poseを使用する

apm_config.yamlにfake_gpsプラグインを追加しました。
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側ではおそらく受信がありません。

これに多くの時間を費やしました、どんな助けも大歓迎です!!
PS PX4ファームウェアでvision_position_estimateを実行でき、そこでfake_gpsを試したことはありませんが、いくつかの制約があるため、これをarducopterファームウェアで動作させたいと思います。

MAVROSのバージョンとプラットフォーム

マブロス：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"
---

IDを確認してください

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プラグインを使おうとしていましたが、使い方がわかりませんでしたが、これが私がしたことです。おそらくそれはあなたを助けるでしょう：

モーションキャプチャシステム（私の場合はUWB）から位置データを取得し、それを緯度経度値に変換するrosノードを作成しました。次に、これらの値を/ mavros / hil / gpsトピックに公開しました。次に、mavrosのGPS_TYPEパラメーターを14に変更しました（これにより、fcuは偽のgpsデータを受け入れます）。位置データをフィードすることはできましたが、良好な位置保持を実現するのにいくつか問題があります。より良い結果を得るために値を調整しようとしています。お役に立てれば。誰かが偽のgpsプラグインを適切に使用する方法も説明してくれれば幸いです:)

PS。 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 2018年03月06日

👍4

全てのコメント5件

こんにちは@SubMishMar 、

PS。 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 2018年03月06日

👍4

@mellonUAVありがとう..私はこれを試してみるつもりです、そしてそれがどうなるかをあなたに知らせます。

SubMishMar 2018年03月06日

@mellonUAV for fake_gpsは、パラメータなどを設定することなく、APMがデフォルトでHIL_GPSメッセージを処理することを確認します。または、 @ SubMishMarが言ったように、 GPS_TYPEを14に設定する必要があるかもしれません。 VISION_POSITION_DELTAについては、あまり言えませんが、実際には自動操縦の終了に依存します。 @khancyrここで何かお勧めはありますか？
@SubMishMarあなたのコードは高く評価されていますが、 fake_gpsはあなたが必要とする変換を正確に行います。その使い方がわかりませんか？

TSC21 2018年05月04日

こんにちは！ @ TSC21
PX4をOFFBOARDモードまたはPOSCTLモードで動作させたいのですが、どちらも使用できます。モーションキャプチャシステムOptiTrackがあります。 px4、mavros、mocapに関する多くの問題をgithubで読みました。

しかし、私はまだいくつかの質問があります：

多くのユーザーがrosパッケージ「mavros」について言及していますが、この「mavros」がどこから来たのか知りたいですか？インストールコマンド「sudoapt-getinstall ros-kinetic-mavros」からのものですか、それともgit cloneからのものですか？両者に違いはありますか？最初はgitを使用していませんでしたが、「apt-get install」の後、「roslaunchmavrospx4.launch」が機能します。でも先週、git cloneでソースコードを入手しました。このソースコードには「px4.launch」もあります！
したがって、mavrosを使用してPX4（Set_Point_Position、Set_Point_Attitudeなど......）を制御しているときに、どの「mavros」を使用する必要があるかを知りたいですか？
また、「mocap_pose_estimate」プラグインについて言及しているユーザーもいます。多くのユーザーが、このプラグインはデータをモーションキャプチャからPX4を修正する座標系に変換できると言っていますが、使用方法がわかりません。それは何ですか？それはノードですか？ '/ mavros / mocap / pose'のようなトピック？それとも、直接使用できるいくつかの関数ですか？
それで、私はそれが何であるか、そしてそれをどのように使うか知りたいですか？この「プラグイン」を自分のプロジェクトに追加するにはどうすればよいですか？

どうもありがとうございました！
ご多幸を祈る！！

YiefengYang 2018年05月27日

多くのユーザーがrosパッケージ「mavros」について言及していますが、この「mavros」がどこから来たのか知りたいですか？

それはまさにあなたがコメントしているリポジトリのパッケージです。

インストールコマンド「sudoapt-getinstall ros-kinetic-mavros」からのものですか、それともgit cloneからのものですか？

これは、リリースバージョン（debから）を使用するか、アップストリームバージョン（リポジトリのgitクローン作成から）を使用するかによって異なります。

両者に違いはありますか？

リリースバージョンがアップストリームマスターと同期されているかどうかによって異なりますが、これは開発の観点から常に進行中であるため、通常、リリースバージョンはアップストリームマスターよりも遅れています。

最初はgitを使用していませんでしたが、「apt-get install」の後、「roslaunchmavrospx4.launch」が機能します。でも先週、git cloneでソースコードを入手しました。このソースコードには「px4.launch」もあります！

はい、それは正常です。リリースバージョンは、Ubuntuディストリビューションリポジトリにリリースされたこのリポジトリです。

したがって、mavrosを使用してPX4（Set_Point_Position、Set_Point_Attitudeなど......）を制御しているときに、どの「mavros」を使用する必要があるかを知りたいですか？

それのどれでも動作するはずです。

また、「mocap_pose_estimate」プラグインについて言及しているユーザーもいます。多くのユーザーが、このプラグインはデータをモーションキャプチャからPX4を修正する座標系に変換できると言っていますが、使用方法がわかりません。それは何ですか？それはノードですか？ '/ mavros / mocap / pose'のようなトピック？それとも、直接使用できるいくつかの関数ですか？

たぶん、ソースコードを見ると、答えが見つかります。これは基本的にmavros_extrasにあるプラグインで、モーションキャプチャシステムからデータを取得するノードから解析されたATT_POS_MOCAP Mavlinkメッセージを送信できます。 MAVROSにはフレーム変換を処理する機能があるため、そのプラグインに固有のものではありません。

それで、私はそれが何であるか、そしてそれをどのように使うか知りたいですか？この「プラグイン」を自分のプロジェクトに追加するにはどうすればよいですか？

これはMAVROSと緊密に結合されているため、独自のプロジェクトにプラグインするプラグインではありません。このプラグインで許可されているものを使用する場合は、MAVROSを使用してプロジェクトに結合するだけです。

TSC21 2018年05月29日

👍1

このページは役に立ちましたか？

0 / 5 - 0 評価