Send position setpoint via mavros

ROS / MAVROS / MicroDDS
1

Hello,

I am trying to send some setpoints from my computer to my pixhawk4 (they are connected with usb cabel). The mavros connection ist successfully established. I can namly arm the vehicle with the mavros service. I can also send actuator command via the /mavros/actuator_control topic and the vehicle did something. But when It comes to send position set points it doesnt works. At First I tried to send command with rostopic pub command like following:

rostopic pub -r 50 /mavros/setpoint_position/local geometry_msgs/PoseStamped “header:
seq: 0
stamp: now
frame_id: ‘1’
pose:
position:
x: 0.0
y: 0.0
z: 10.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0”

But the vehicle did nothing. I also tried to write a node that publish setpoints and it didnt help:

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include “geometry_msgs/PoseStamped.h”
#include “geometry_msgs/Vector3Stamped.h”

int main(int argc, char **argv)
{
ros::init(argc, argv, “pub_setpoints”);
ros::NodeHandle n;

ros::Publisher chatter_pub = n.advertise<geometry_msgs::PoseStamped>(“/mavros/setpoint_position/local”,100);
ros::Rate loop_rate(100);
ros::spinOnce();

geometry_msgs::PoseStamped msg;
int count = 1;

while(ros::ok()){

   msg.header.stamp = ros::Time::now();
   msg.header.seq=count;
   msg.header.frame_id = 1;
   msg.pose.position.x = 0.0;//0.001*some_object.position_x;
   msg.pose.position.y = 0.0;//0.001*some_object.position_y;
   msg.pose.position.z = 1.0;//0.001*some_object.position_z;
   msg.pose.orientation.x = -0.021;
   msg.pose.orientation.y = 0.047;
   msg.pose.orientation.z = -0.89;
   msg.pose.orientation.w = 0.44;

   chatter_pub.publish(msg);
   ros::spinOnce();
   count++;
   loop_rate.sleep();

}

return 0;
}

I start the mavros node via a launch file und here is the output:

SUMMARY

CLEAR PARAMETERS

  • /mavros/

PARAMETERS

  • /mavros/cmd/use_comp_id_system_control: False
  • /mavros/conn/heartbeat_rate: 1.0
  • /mavros/conn/system_time_rate: 1.0
  • /mavros/conn/timeout: 10.0
  • /mavros/conn/timesync_rate: 10.0
  • /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
  • /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
  • /mavros/distance_sensor/hrlv_ez4_pub/id: 0
  • /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
  • /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
  • /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
  • /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
  • /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
  • /mavros/distance_sensor/laser_1_sub/id: 3
  • /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
  • /mavros/distance_sensor/laser_1_sub/subscriber: True
  • /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
  • /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
  • /mavros/distance_sensor/lidarlite_pub/id: 1
  • /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
  • /mavros/distance_sensor/lidarlite_pub/send_tf: True
  • /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
  • /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
  • /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
  • /mavros/distance_sensor/sonar_1_sub/id: 2
  • /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
  • /mavros/distance_sensor/sonar_1_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: 408.0
  • /mavros/fake_gps/geo_origin/lat: 47.3667
  • /mavros/fake_gps/geo_origin/lon: 8.55
  • /mavros/fake_gps/gps_rate: 5.0
  • /mavros/fake_gps/mocap_transform: True
  • /mavros/fake_gps/satellites_visible: 5
  • /mavros/fake_gps/tf/child_frame_id: fix
  • /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/gps_uere: 1.0
  • /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.0003490659 // 0…
  • /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/landing_target/camera/fov_x: 2.0071286398
  • /mavros/landing_target/camera/fov_y: 2.0071286398
  • /mavros/landing_target/image/height: 480
  • /mavros/landing_target/image/width: 640
  • /mavros/landing_target/land_target_type: VISION_FIDUCIAL
  • /mavros/landing_target/listen_lt: False
  • /mavros/landing_target/mav_frame: LOCAL_NED
  • /mavros/landing_target/target_size/x: 0.3
  • /mavros/landing_target/target_size/y: 0.3
  • /mavros/landing_target/tf/child_frame_id: camera_center
  • /mavros/landing_target/tf/frame_id: landing_target
  • /mavros/landing_target/tf/listen: False
  • /mavros/landing_target/tf/rate_limit: 10.0
  • /mavros/landing_target/tf/send: True
  • /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: True
  • /mavros/mocap/use_tf: False
  • /mavros/odometry/fcu/odom_child_id_des: base_link
  • /mavros/odometry/fcu/odom_parent_id_des: map
  • /mavros/plugin_blacklist: [‘safety_area’, '…
  • /mavros/plugin_whitelist:
  • /mavros/px4flow/frame_id: px4flow
  • /mavros/px4flow/ranger_fov: 0.118682
  • /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_raw/thrust_scaling: 1.0
  • /mavros/setpoint_velocity/mav_frame: LOCAL_NED
  • /mavros/startup_px4_usb_quirk: True
  • /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: MAVLINK
  • /mavros/vibration/frame_id: base_link
  • /mavros/vision_pose/tf/child_frame_id: vision_estimate
  • /mavros/vision_pose/tf/frame_id: odom
  • /mavros/vision_pose/tf/listen: False
  • /mavros/vision_pose/tf/rate_limit: 10.0
  • /mavros/vision_speed/listen_twist: True
  • /mavros/vision_speed/twist_cov: True
  • /mavros/wheel_odometry/child_frame_id: base_link
  • /mavros/wheel_odometry/count: 2
  • /mavros/wheel_odometry/frame_id: odom
  • /mavros/wheel_odometry/send_raw: True
  • /mavros/wheel_odometry/send_twist: False
  • /mavros/wheel_odometry/tf/child_frame_id: base_link
  • /mavros/wheel_odometry/tf/frame_id: odom
  • /mavros/wheel_odometry/tf/send: False
  • /mavros/wheel_odometry/use_rpm: False
  • /mavros/wheel_odometry/vel_error: 0.1
  • /mavros/wheel_odometry/wheel0/radius: 0.05
  • /mavros/wheel_odometry/wheel0/x: 0.0
  • /mavros/wheel_odometry/wheel0/y: -0.15
  • /mavros/wheel_odometry/wheel1/radius: 0.05
  • /mavros/wheel_odometry/wheel1/x: 0.0
  • /mavros/wheel_odometry/wheel1/y: 0.15
  • /rosdistro: kinetic
  • /rosversion: 1.12.14

NODES
/
mavros (mavros/mavros_node)

auto-starting new master
process[master]: started with pid [5053]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to a7ad4db0-8199-11ea-a9de-dc85def86db3
process[rosout-1]: started with pid [5066]
started core service [/rosout]
process[mavros-2]: started with pid [5084]
[ INFO] [1587230754.617274039]: FCU URL: /dev/ttyACM0:57600
[ INFO] [1587230754.620913481]: serial0: device: /dev/ttyACM0 @ 57600 bps
[ INFO] [1587230754.621778125]: GCS bridge disabled
[ INFO] [1587230754.639949832]: Plugin 3dr_radio loaded
[ INFO] [1587230754.642840836]: Plugin 3dr_radio initialized
[ INFO] [1587230754.642986137]: Plugin actuator_control loaded
[ INFO] [1587230754.649998070]: Plugin actuator_control initialized
[ INFO] [1587230754.655602204]: Plugin adsb loaded
[ INFO] [1587230754.661047930]: Plugin adsb initialized
[ INFO] [1587230754.661299325]: Plugin altitude loaded
[ INFO] [1587230754.663016734]: Plugin altitude initialized
[ INFO] [1587230754.663247038]: Plugin cam_imu_sync loaded
[ INFO] [1587230754.664355512]: Plugin cam_imu_sync initialized
[ INFO] [1587230754.664598298]: Plugin command loaded
[ INFO] [1587230754.674940931]: Plugin command initialized
[ INFO] [1587230754.675165442]: Plugin companion_process_status loaded
[ INFO] [1587230754.681536613]: Plugin companion_process_status initialized
[ INFO] [1587230754.681755392]: Plugin debug_value loaded
[ INFO] [1587230754.688794715]: Plugin debug_value initialized
[ INFO] [1587230754.688850375]: Plugin distance_sensor blacklisted
[ INFO] [1587230754.689094639]: Plugin fake_gps loaded
[ INFO] [1587230754.713705324]: Plugin fake_gps initialized
[ INFO] [1587230754.714011920]: Plugin ftp loaded
[ INFO] [1587230754.725618297]: Plugin ftp initialized
[ INFO] [1587230754.725867334]: Plugin global_position loaded
[ INFO] [1587230754.751573413]: Plugin global_position initialized
[ INFO] [1587230754.751811367]: Plugin gps_rtk loaded
[ INFO] [1587230754.755901156]: Plugin gps_rtk initialized
[ INFO] [1587230754.756165899]: Plugin hil loaded
[ INFO] [1587230754.781823847]: Plugin hil initialized
[ INFO] [1587230754.782139713]: Plugin home_position loaded
[ INFO] [1587230754.788699376]: Plugin home_position initialized
[ INFO] [1587230754.789102071]: Plugin imu loaded
[ INFO] [1587230754.804786598]: Plugin imu initialized
[ INFO] [1587230754.805066243]: Plugin landing_target loaded
[ INFO] [1587230754.833368344]: Plugin landing_target initialized
[ INFO] [1587230754.833809332]: Plugin local_position loaded
[ INFO] [1587230754.846106327]: Plugin local_position initialized
[ INFO] [1587230754.846380567]: Plugin log_transfer loaded
[ INFO] [1587230754.852512253]: Plugin log_transfer initialized
[ INFO] [1587230754.852879622]: Plugin manual_control loaded
[ INFO] [1587230754.859846611]: Plugin manual_control initialized
[ INFO] [1587230754.860169211]: Plugin mocap_pose_estimate loaded
[ INFO] [1587230754.870832746]: Plugin mocap_pose_estimate initialized
[ INFO] [1587230754.871117895]: Plugin mount_control loaded
[ INFO] [1587230754.878671776]: Plugin mount_control initialized
[ INFO] [1587230754.878941114]: Plugin obstacle_distance loaded
[ INFO] [1587230754.884405535]: Plugin obstacle_distance initialized
[ INFO] [1587230754.884662026]: Plugin odom loaded
[ INFO] [1587230754.894174953]: Plugin odom initialized
[ INFO] [1587230754.894449672]: Plugin onboard_computer_status loaded
[ INFO] [1587230754.899795615]: Plugin onboard_computer_status initialized
[ INFO] [1587230754.900201466]: Plugin param loaded
[ INFO] [1587230754.905537254]: Plugin param initialized
[ INFO] [1587230754.905762382]: Plugin px4flow loaded
[ INFO] [1587230754.917180185]: Plugin px4flow initialized
[ INFO] [1587230754.917273473]: Plugin rangefinder blacklisted
[ INFO] [1587230754.917722086]: Plugin rc_io loaded
[ INFO] [1587230754.925963592]: Plugin rc_io initialized
[ INFO] [1587230754.926039131]: Plugin safety_area blacklisted
[ INFO] [1587230754.926310443]: Plugin setpoint_accel loaded
[ INFO] [1587230754.932290178]: Plugin setpoint_accel initialized
[ INFO] [1587230754.932699787]: Plugin setpoint_attitude loaded
[ INFO] [1587230754.948817940]: Plugin setpoint_attitude initialized
[ INFO] [1587230754.949134188]: Plugin setpoint_position loaded
[ INFO] [1587230754.977784831]: Plugin setpoint_position initialized
[ INFO] [1587230754.978098074]: Plugin setpoint_raw loaded
[ INFO] [1587230754.994887365]: Plugin setpoint_raw initialized
[ INFO] [1587230754.995191939]: Plugin setpoint_trajectory loaded
[ INFO] [1587230755.003344049]: Plugin setpoint_trajectory initialized
[ INFO] [1587230755.003680917]: Plugin setpoint_velocity loaded
[ INFO] [1587230755.015955031]: Plugin setpoint_velocity initialized
[ INFO] [1587230755.016835415]: Plugin sys_status loaded
[ INFO] [1587230755.041671687]: Plugin sys_status initialized
[ INFO] [1587230755.042017528]: Plugin sys_time loaded
[ INFO] [1587230755.056171261]: TM: Timesync mode: MAVLINK
[ INFO] [1587230755.058675763]: Plugin sys_time initialized
[ INFO] [1587230755.058958963]: Plugin trajectory loaded
[ INFO] [1587230755.069566064]: Plugin trajectory initialized
[ INFO] [1587230755.069908752]: Plugin vfr_hud loaded
[ INFO] [1587230755.071317821]: Plugin vfr_hud initialized
[ INFO] [1587230755.071386214]: Plugin vibration blacklisted
[ INFO] [1587230755.071687638]: Plugin vision_pose_estimate loaded
[ INFO] [1587230755.088142643]: Plugin vision_pose_estimate initialized
[ INFO] [1587230755.088479920]: Plugin vision_speed_estimate loaded
[ INFO] [1587230755.095482271]: Plugin vision_speed_estimate initialized
[ INFO] [1587230755.095818186]: Plugin waypoint loaded
[ INFO] [1587230755.103969036]: Plugin waypoint initialized
[ INFO] [1587230755.104022401]: Plugin wheel_odometry blacklisted
[ INFO] [1587230755.104417505]: Plugin wind_estimation loaded
[ INFO] [1587230755.106645862]: Plugin wind_estimation initialized
[ INFO] [1587230755.106772491]: Autostarting mavlink via USB on PX4
[ INFO] [1587230755.107091248]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1587230755.107132527]: Built-in MAVLink package version: 2020.4.4
[ INFO] [1587230755.107232808]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1587230755.107307175]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1587230755.223903269]: IMU: High resolution IMU detected!
[ INFO] [1587230755.225927511]: IMU: Attitude quaternion IMU detected!
[ INFO] [1587230755.443214204]: RC_CHANNELS message detected!
[ INFO] [1587230755.859208576]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1587230755.864818494]: IMU: High resolution IMU detected!
[ INFO] [1587230755.865256801]: IMU: Attitude quaternion IMU detected!
[ INFO] [1587230755.943907658]: RC_CHANNELS message detected!
[ERROR] [1587230756.029910073]: FCU: Onboard controller lost
[ INFO] [1587230756.081858866]: FCU: Onboard controller regained
[ INFO] [1587230756.865514759]: VER: 1.1: Capabilities 0x000000000000e4ef
[ INFO] [1587230756.865611160]: VER: 1.1: Flight software: 010a01ff (e0f016c2b3000000)
[ INFO] [1587230756.865694507]: VER: 1.1: Middleware software: 010a01ff (e0f016c2b3000000)
[ INFO] [1587230756.865778943]: VER: 1.1: OS software: 071d00ff (427238133be2b0ec)
[ INFO] [1587230756.865855775]: VER: 1.1: Board hardware: 00000032
[ INFO] [1587230756.865930262]: VER: 1.1: VID/PID: 26ac:0032
[ INFO] [1587230756.866003260]: VER: 1.1: UID: 3038510738333933
[ WARN] [1587230756.867597214]: CMD: Unexpected command 520, result 0
[ INFO] [1587230765.860215512]: HP: requesting home position
[ INFO] [1587230770.861342169]: WP: mission received
[ INFO] [1587230775.859518018]: HP: requesting home position
[ INFO] [1587230785.859820618]: HP: requesting home position

So I really don’ t know how to search for the Problem. Does somenone have some tipps und can tell me how to debug and how can I search for the Problem.

Thank you in advance

2

you must change the PX4 to Offboard mode as shown in MAVROS Offboard Example

3

Thank you for your answers. I have tried to run the node of the tutorial but it doesnt help. I have noticed that my tf tree is not connected. there is not a link from map to base_link. This is maybe the Problem right? I have no GPS device I use currently a virtual one. Maybe I should set some parameters to connect the tree.

4

Ah about that, take a look at my response here Need help setting tf links on an iris