Mavros: usando el complemento fake_gps y el complemento vision_pose_estimation con Ardupilot Firmware

Creado en 2 mar. 2018 · 5Comentarios · Fuente: mavlink/mavros

Detalles del problema

Quiero controlar mi dron (
una. use el complemento fake_pgs.
B. utilizar el complemento vision_pose_estimate
Pero tengo problemas con ambos. Quiero describir lo que he hecho con los dos enfoques antes mencionados para que el lector pueda ayudarme, en caso de que me equivoque en alguna parte.

una. usando fake_gps

Se agregó el complemento fake_gps a mi apm_config.yaml.
Se editó apm_pluginlists.yaml y así es como se ve la parte 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

Estoy publicando la pose del robot sobre rostopic / mavros / fake_gps / mocap / tf a aproximadamente 4 Hz (puedo aumentarlo hasta 30 Hz pero creo que el parámetro gps_rate lo limita, por cierto, ¿puedo aumentar gps_rate?)
Me echo / mavros / global_position / global No veo ninguna salida significativa,

¿Alguien puede ayudarme aquí para que esto funcione? ¿Hay algo más que deba hacer? ¿Algún parámetro que deba configurar o desarmar?

B. usando vision_pose

Se agregó el complemento fake_gps a mi apm_config.yaml.
Se editó apm_pluginlists.yaml y así es como se ve la parte 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

Publico la pose del robot sobre el rostopic / mavros / vision_pose / pose
Realizo algunas modificaciones en vision_position_estimate.cpp y uso VISION_POSITION_DELTA lugar de VISION_POSITION_ESTIMATE .
Configuré los indicadores apropiados en el código anterior para ver si este código se ejecuta y esos indicadores se imprimen, por lo que concluyo que mavros está enviando datos a FCU pero probablemente no haya recepción en el extremo de FCU.

Mucho tiempo dedicado a esto, ¡cualquier ayuda es muy apreciada!
PD: pude hacer vision_position_estimate en el firmware PX4, no he probado fake_gps allí, pero debido a algunas limitaciones, quiero que esto funcione en el firmware de arducopter.

Versión y plataforma MAVROS

Mavros: 0,18
ROS: cinético
Ubuntu: 16.04

Tipo y versión de piloto automático

[X] ArduPilot
[] PX4

Versión: 3.4.6

Registros de nodo

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

Diagnósticos

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"
---

Verificar 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

Fuente

SubMishMar

Comentario más útil

Hola @SubMishMar ,

También estaba tratando de usar el complemento fake_gps pero no pude averiguar cómo usarlo, pero esto es lo que hice, tal vez te ayude:

Creé un nodo ros que toma datos de posición del sistema Mocap (UWB en mi caso) y los convierte en valores de latitud y longitud. Luego publiqué estos valores en / mavros / hil / gps topic. Luego cambié el parámetro GPS_TYPE en mavros a 14 (esto hace que la fcu acepte datos gps falsos). Pude alimentar los datos de posición, pero tengo algunos problemas para lograr una buena retención de posición. Estoy tratando de ajustar los valores para obtener un mejor resultado. Espero que esto ayude. Agradecería que alguien me explicara cómo usar correctamente el complemento gps falso también :)

PD. puede cambiar los valores lat0 y long0 a su ubicación.

Aquí está el código del nodo 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 en 6 mar. 2018

👍4

Todos 5 comentarios

Hola @SubMishMar ,

También estaba tratando de usar el complemento fake_gps pero no pude averiguar cómo usarlo, pero esto es lo que hice, tal vez te ayude:

PD. puede cambiar los valores lat0 y long0 a su ubicación.

Aquí está el código del nodo 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 en 6 mar. 2018

👍4

@mellonUAV gracias ... voy a probar esto y les haré saber cómo va.

SubMishMar en 6 mar. 2018

@mellonUAV por fake_gps asegúrese de que APM procese HIL_GPS msgs por defecto sin la necesidad de configurar ningún parámetro o algo así. O tal vez solo tenga que establecer GPS_TYPE en 14 como dijo @SubMishMar . Por VISION_POSITION_DELTA no puedo decir mucho, realmente depende del final del piloto automático. @khancyr ¿ alguna recomendación aquí?
@SubMishMar se agradece su código, pero fake_gps hace exactamente la conversión que necesita. ¿Qué no entiendes sobre su uso?

TSC21 en 4 may. 2018

¡Hola! @ TSC21
Quiero que nuestro PX4 funcione en modo OFFBOARD o POSCTL, ambos son aceptables. Disponemos del sistema de mocap OptiTrack. He leído muchos números en github sobre px4, mavros y mocap.

Pero todavía tengo algunas preguntas:

Muchos usuarios han mencionado el paquete ros 'mavros', pero quiero saber de dónde vino este 'mavros'. ¿Viene de un comando de instalación "sudo apt-get install ros-kinetic-mavros" o viene de git clone? ¿Hay alguna diferencia entre los dos? Al principio, no usé el git one, después de "apt-get install", "roslaunch mavros px4.launch" puede funcionar. Pero la semana pasada obtuve el código fuente por git clone, y en este código fuente, también existe un "px4.launch" !!
Entonces, quiero saber mientras usamos mavros para controlar nuestro PX4 (Set_Point_Position, Set_Point_Attitude, etc.), ¿qué 'mavros' deberíamos usar?
También veo que algunos usuarios mencionaron el complemento "mocap_pose_estimate". Muchos usuarios dijeron que este complemento puede transformar los datos de mocap al sistema de coordenadas que arregla el PX4, pero no sé cómo usarlo. ¿Qué es? ¿Es un nodo? ¿Un tema como '/ mavros / mocap / pose'? ¿O solo algunas funciones que podemos usar directamente?
Entonces, quiero saber qué es y cómo usarlo. ¿Cómo puedo agregar este "complemento" a mi propio proyecto?

¡Muchas gracias!
¡¡Los mejores deseos!!

YiefengYang en 27 may. 2018

Muchos usuarios han mencionado el paquete ros 'mavros', pero quiero saber de dónde vino este 'mavros'.

Es exactamente el paquete del repositorio que está comentando.

¿Viene de un comando de instalación "sudo apt-get install ros-kinetic-mavros" o viene de git clone?

Eso depende de si desea utilizar una versión de lanzamiento (de deb) o la versión ascendente (de git clonando el repositorio).

¿Hay alguna diferencia entre los dos?

Depende de si la versión de lanzamiento está sincronizada con el maestro ascendente, pero dado que esto siempre está en curso en términos de desarrollo, generalmente la versión de lanzamiento está detrás del maestro ascendente.

Al principio, no usé el git one, después de "apt-get install", "roslaunch mavros px4.launch" puede funcionar. Pero la semana pasada obtuve el código fuente por git clone, y en este código fuente, también existe un "px4.launch" !!

Sí, eso es normal. La versión de lanzamiento es este repositorio lanzado en los repositorios de distribución de Ubuntu.

Entonces, quiero saber mientras usamos mavros para controlar nuestro PX4 (Set_Point_Position, Set_Point_Attitude, etc.), ¿qué 'mavros' deberíamos usar?

Cualquiera debería funcionar.

También veo que algunos usuarios mencionaron el complemento "mocap_pose_estimate". Muchos usuarios dijeron que este complemento puede transformar los datos de mocap al sistema de coordenadas que arregla el PX4, pero no sé cómo usarlo. ¿lo que es? ¿Es un nodo? ¿Un tema como '/ mavros / mocap / pose'? ¿O solo algunas funciones que podemos usar directamente?

Quizás si echas un vistazo al código fuente encuentres tu respuesta. Básicamente es un complemento que puede encontrar en mavros_extras que le permite enviar ATT_POS_MOCAP Mavlink msg analizado desde un nodo que tiene que obtiene datos de su sistema mocap. MAVROS tiene funciones para manejar la transformación del marco, por lo que no es específico de ese complemento.

Entonces, quiero saber qué es y cómo usarlo. ¿Cómo puedo agregar este "complemento" a mi propio proyecto?

Este no es un complemento que conecte a su propio proyecto, ya que está estrechamente relacionado con MAVROS. Si desea usar lo que permite este complemento, simplemente use MAVROS y conéctelo a su proyecto.

TSC21 en 29 may. 2018

👍1

¿Fue útil esta página

0 / 5 - 0 calificaciones