search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
879 stars 990 forks source link

My ground rover don't move with setpoint_velocity/cmd_vel #945

Closed nanane3437 closed 6 years ago

nanane3437 commented 6 years ago

Issue details

Hi,

I try to move my ground robot with /mavros/setpoint_velocity/cmd_vel but when I used it nothing happen. I try to follow this tutorial : https://pixhawk.org/dev/ros/ground_rover I have :

So, step by step :

$ roscore

I run a mavproxy and a mavros

$ sudo mavproxy.py --master=/dev/ttyUSB0 --baudrate 115200 --out udp:127.0.0.1:14540 --out udp:127.0.0.1:14550 --console

$ roslaunch mavros apm.launch fcu_url:=udp://:14550@

Then I rosrun send_to_cmdvel.cpp in my rover_test package

$ rosrun rover_test send_to_cmdvel

my code send_to_cmdvel.cpp : 

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "cmd_vel_fusion");
    ros::NodeHandle nh;
    ros::Publisher send_velocity_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 1000);
    ros::Rate loop_rate(100);

    geometry_msgs::TwistStamped send_velocity_msg;

    double ros_roll=0.0;
    double ros_pitch=0.0;
    double ros_yaw=0.0;
    double ros_throttle=0.0;
    int count = 1;

    while (ros::ok())
    {
        nh.param<double>("ros_roll", ros_roll, 0.0);
        nh.param<double>("ros_pitch", ros_pitch, 0.0);
        nh.param<double>("ros_yaw", ros_yaw, 0.0);
        nh.param<double>("ros_throttle", ros_throttle,0.0);

        send_velocity_msg.header.stamp = ros::Time::now();
        send_velocity_msg.header.seq = count ;
        send_velocity_msg.header.frame_id = 1 ;

        send_velocity_msg.twist.linear.x = ros_throttle;
        send_velocity_msg.twist.linear.y = 0.0;
        send_velocity_msg.twist.linear.z = 0.0;
        send_velocity_msg.twist.angular.x = ros_pitch;
        send_velocity_msg.twist.angular.y = ros_roll;
        send_velocity_msg.twist.angular.z = ros_yaw;

        send_velocity_pub.publish(send_velocity_msg);
        ros::spinOnce();
        count++;
        loop_rate.sleep();
    }
    return 0;
}

Then, I want to send velocity command

$ rosservice call /mavros/set_stream_rate 0 10 1

$ rosrun mavros mavsys mode -c GUIDED

$ rosrun mavros mavsafety arm

$ rosparam set ros_throttle 0.5

And when I run a

$ rostopic echo /mavros/setpoint_velocity/cmd_vel

I can see the values changing but my wheel doesn't move ! And on mission planner my PWM values of ch1out, ch2out, ch3out, ch4out stay in 1500

---
header: 
  seq: 2137
  stamp: 
    secs: 1518687506
    nsecs:  35376796
  frame_id: "\x01"
twist: 
  linear: 
    x: 0.5
    y: 0.0
    z: 0.0
  angular: 
    x: 0.0
    y: 0.0
    z: 0.0
---
header: 
  seq: 2138
  stamp: 
    secs: 1518687506
    nsecs:  46208145
  frame_id: "\x01"
twist: 
  linear: 
    x: 0.5
    y: 0.0
    z: 0.0
  angular: 
    x: 0.0
    y: 0.0
    z: 0.0
---

Any Idea ?

MAVROS version and platform

Mavros: 0.23.0 ROS: Kinetic Ubuntu Mate : 1.12.1

Autopilot type and version

ArduPilot Version: 3.1.2

Node logs

... logging to /home/odroid/.ros/log/380aec3e-1228-11e8-bd2b-7cdd906b1c96/roslaunch-odroid-16513.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://odroid:45321/

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: 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: udp://:14550@
 * /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: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/estimator_type: 3
 * /mavros/odometry/frame_tf/desired_frame: ned
 * /mavros/plugin_blacklist: ['actuator_contro...
 * /mavros/plugin_whitelist: []
 * /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: vision_estimate
 * /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)

ROS_MASTER_URI=http://localhost:11311

process[mavros-1]: started with pid [16531]
[ INFO] [1518688780.584612672]: FCU URL: udp://:14550@
[ INFO] [1518688780.599416164]: udp0: Bind address: 0.0.0.0:14550
[ INFO] [1518688780.600944863]: GCS bridge disabled
[ INFO] [1518688780.655351528]: Plugin 3dr_radio loaded
[ INFO] [1518688780.663921850]: udp0: Remote address: 127.0.0.1:57206
[ INFO] [1518688780.665770714]: Plugin 3dr_radio initialized
[ INFO] [1518688780.665929671]: Plugin actuator_control blacklisted
[ INFO] [1518688780.676319566]: Plugin adsb loaded
[ INFO] [1518688780.689591151]: Plugin adsb initialized
[ INFO] [1518688780.689763858]: Plugin altitude blacklisted
[ INFO] [1518688780.690537020]: Plugin cam_imu_sync loaded
[ INFO] [1518688780.693249170]: Plugin cam_imu_sync initialized
[ INFO] [1518688780.693932582]: Plugin command loaded
[ INFO] [1518688780.715945905]: Plugin command initialized
[ INFO] [1518688780.716087987]: Plugin debug_value blacklisted
[ INFO] [1518688780.716794191]: Plugin distance_sensor loaded
[ INFO] [1518688780.783367490]: Plugin distance_sensor initialized
[ INFO] [1518688780.784618107]: Plugin fake_gps loaded
[ INFO] [1518688780.853811806]: Plugin fake_gps initialized
[ INFO] [1518688780.854074555]: Plugin ftp blacklisted
[ INFO] [1518688780.856131584]: Plugin global_position loaded
[ INFO] [1518688780.936649630]: Plugin global_position initialized
[ INFO] [1518688780.936875545]: Plugin hil blacklisted
[ INFO] [1518688780.938210829]: Plugin home_position loaded
[ INFO] [1518688780.972894199]: Plugin home_position initialized
[ INFO] [1518688780.973550153]: Plugin imu loaded
[ INFO] [1518688781.003237428]: Plugin imu initialized
[ INFO] [1518688781.004203964]: Plugin local_position loaded
[ INFO] [1518688781.029229977]: Plugin local_position initialized
[ INFO] [1518688781.029911889]: Plugin manual_control loaded
[ INFO] [1518688781.043346723]: Plugin manual_control initialized
[ INFO] [1518688781.043618847]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1518688781.044169010]: Plugin obstacle_distance loaded
[ INFO] [1518688781.061949025]: Plugin obstacle_distance initialized
[ INFO] [1518688781.062572147]: Plugin odom loaded
[ INFO] [1518688781.080378620]: Plugin odom initialized
[ INFO] [1518688781.081493655]: Plugin param loaded
[ INFO] [1518688781.093363957]: Plugin param initialized
[ INFO] [1518688781.093497331]: Plugin px4flow blacklisted
[ INFO] [1518688781.094063994]: Plugin rangefinder loaded
[ INFO] [1518688781.096707895]: Plugin rangefinder initialized
[ INFO] [1518688781.097616848]: Plugin rc_io loaded
[ INFO] [1518688781.114646701]: Plugin rc_io initialized
[ INFO] [1518688781.114778117]: Plugin safety_area blacklisted
[ INFO] [1518688781.115362905]: Plugin setpoint_accel loaded
[ INFO] [1518688781.157440104]: Plugin setpoint_accel initialized
[ INFO] [1518688781.158850762]: Plugin setpoint_attitude loaded
[ INFO] [1518688781.208533415]: Plugin setpoint_attitude initialized
[ INFO] [1518688781.209295702]: Plugin setpoint_position loaded
[ INFO] [1518688781.276781912]: Plugin setpoint_position initialized
[ INFO] [1518688781.277715364]: Plugin setpoint_raw loaded
[ INFO] [1518688781.318716320]: Plugin setpoint_raw initialized
[ INFO] [1518688781.319735022]: Plugin setpoint_velocity loaded
[ INFO] [1518688781.354189018]: Plugin setpoint_velocity initialized
[ INFO] [1518688781.355703467]: Plugin sys_status loaded
[ INFO] [1518688781.393995690]: Plugin sys_status initialized
[ INFO] [1518688781.394883143]: Plugin sys_time loaded
[ INFO] [1518688781.408993473]: TM: Timesync mode: NONE
[ INFO] [1518688781.411653123]: Plugin sys_time initialized
[ INFO] [1518688781.412284286]: Plugin vfr_hud loaded
[ INFO] [1518688781.417807377]: Plugin vfr_hud initialized
[ INFO] [1518688781.418047042]: Plugin vibration blacklisted
[ INFO] [1518688781.418149916]: Plugin vision_pose_estimate blacklisted
[ INFO] [1518688781.418335540]: Plugin vision_speed_estimate blacklisted
[ INFO] [1518688781.419431159]: Plugin waypoint loaded
[ INFO] [1518688781.459567328]: Plugin waypoint initialized
[ INFO] [1518688781.460211908]: Built-in SIMD instructions: None
[ INFO] [1518688781.460425073]: Built-in MAVLink package version: 2018.1.1
[ INFO] [1518688781.460707238]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1518688781.460944361]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1518688781.616530487]: IMU: Raw IMU message used.
[ WARN] [1518688781.618659724]: IMU: linear acceleration on RAW_IMU known on APM only.
[ WARN] [1518688781.618829765]: IMU: ~imu/data_raw stores unscaled raw acceleration report.
[ WARN] [1518688781.621797913]: GP: No GPS fix
[ INFO] [1518688781.628069624]: RC_CHANNELS message detected!
[ WARN] [1518688781.672835099]: TM: Wrong FCU time.
[ INFO] [1518688782.154436177]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega / ArduCopter
[ INFO] [1518688782.344564173]: RC_CHANNELS message detected!
[ INFO] [1518688783.213524158]: VER: 1.1: Capabilities         0x000000000000118f
[ INFO] [1518688783.214079280]: VER: 1.1: Flight software:     030102ff (0fa522b856c0c696be6ff61a)
[ INFO] [1518688783.214661776]: VER: 1.1: Middleware software: 00000000 (56c0c696be6ff61a)
[ INFO] [1518688783.215114857]: VER: 1.1: OS software:         00000000 (be6ff61a)
[ INFO] [1518688783.215617562]: VER: 1.1: Board hardware:      00000000
[ INFO] [1518688783.216070393]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1518688783.216544973]: VER: 1.1: UID:                 0000000000000000
[ WARN] [1518688783.218163671]: CMD: Unexpected command 520, result 0
[ INFO] [1518688792.156638451]: HP: requesting home position
[ INFO] [1518688792.222457254]: FCU: APM:Rover v3.1.2 (0fa522b8)
[ INFO] [1518688792.223507997]: FCU: PX4: 56c0c696 NuttX: be6ff61a
[ INFO] [1518688792.224303867]: FCU: PX4v2 00430032 31355118 35333436
[ INFO] [1518688797.232049923]: WP: item #0  F:0 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 0.000000 y: 0.000000 z: 0.000000
[ INFO] [1518688797.281144037]: WP: item #1  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 48.923233 y: 2.352434 z: 100.000000
[ INFO] [1518688797.327871583]: WP: item #2  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 48.923222 y: 2.352374 z: 100.000000
[ INFO] [1518688797.392940141]: WP: item #3  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 48.923206 y: 2.352332 z: 100.000000
[ INFO] [1518688797.436275541]: WP: item #4  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 48.923180 y: 2.352321 z: 100.000000
[ INFO] [1518688797.436737413]: WP: mission received
[ INFO] [1518688802.156912530]: HP: requesting home position
[ INFO] [1518688802.464880968]: PR: parameters list received
[ WARN] [1518688811.829044923]: GP: No GPS fix
[ INFO] [1518688812.156764156]: HP: requesting home position
[ INFO] [1518688822.155639123]: HP: requesting home position
[ INFO] [1518688832.156036833]: HP: requesting home position
[ WARN] [1518688841.866119497]: TM: Wrong FCU time.
[ WARN] [1518688842.058129024]: GP: No GPS fix
[ INFO] [1518688842.156514085]: HP: requesting home position
[ INFO] [1518688852.155899972]: HP: requesting home position

Diagnostics

header: 
  seq: 127
  stamp: 
    secs: 1518688941
    nsecs: 462893337
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://:14550@"
    values: 
      - 
        key: "Received packets:"
        value: "14596"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "22"
      - 
        key: "Tx sequence number:"
        value: "184"
      - 
        key: "Rx total bytes:"
        value: "419527"
      - 
        key: "Tx total bytes:"
        value: "4184"
      - 
        key: "Rx speed:"
        value: "4381.000000"
      - 
        key: "Tx speed:"
        value: "42.000000"
  - 
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "udp://:14550@"
    values: 
      - 
        key: "Satellites visible"
        value: "0"
      - 
        key: "Fix type"
        value: "1"
      - 
        key: "EPH (m)"
        value: "99.99"
      - 
        key: "EPV (m)"
        value: "99.99"
  - 
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://:14550@"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "160"
      - 
        key: "Frequency (Hz)"
        value: "0.999975"
      - 
        key: "Vehicle type"
        value: "Ground rover"
      - 
        key: "Autopilot type"
        value: "ArduPilotMega / ArduCopter"
      - 
        key: "Mode"
        value: "GUIDED"
      - 
        key: "System status"
        value: "Critical"
  - 
    level: 2
    name: "mavros: System"
    message: "Sensor helth"
    hardware_id: "udp://:14550@"
    values: 
      - 
        key: "Sensor present"
        value: "0x0120DD27"
      - 
        key: "Sensor enabled"
        value: "0x0120DD27"
      - 
        key: "Sensor helth"
        value: "0x0100DD03"
      - 
        key: "3D gyro"
        value: "Ok"
      - 
        key: "3D accelerometer"
        value: "Ok"
      - 
        key: "3D magnetometer"
        value: "Fail"
      - 
        key: "GPS"
        value: "Fail"
      - 
        key: "laser based position"
        value: "Ok"
      - 
        key: "3D angular rate control"
        value: "Ok"
      - 
        key: "attitude stabilization"
        value: "Ok"
      - 
        key: "yaw position"
        value: "Ok"
      - 
        key: "x/y position control"
        value: "Ok"
      - 
        key: "motor outputs / control"
        value: "Ok"
      - 
        key: "AHRS subsystem health"
        value: "Fail"
      - 
        key: "Logging"
        value: "Ok"
      - 
        key: "CPU Load (%)"
        value: "13.8"
      - 
        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: 0
    name: "mavros: Battery"
    message: "Normal"
    hardware_id: "udp://:14550@"
    values: 
      - 
        key: "Voltage"
        value: "12.75"
      - 
        key: "Current"
        value: "19.1"
      - 
        key: "Remaining"
        value: "9.0"
---

Check ID

OK. I got messages from 1:1.

---
Received 1358 messages, from 2 addresses
sys:comp   list of messages
 255:190   0
  1:1     0, 129, 2, 22, 1, 152, 150, 24, 27, 30, 193, 33, 35, 36, 165, 42, 173, 62, 65, 74, 77, 163, 241, 116, 125
TSC21 commented 6 years ago

@nanane3437 https://pixhawk.org/dev/ros/ground_rover is probably deprecated already. You should follow https://docs.px4.io/en/frames_rover/ together with https://dev.px4.io/en/ros/mavros_offboard.html. Though I am currently not sure what is the current support of the rover type for OFFBOARD control.

TSC21 commented 6 years ago

Oh wait. Now I see you are using APM. Forget the previous post. You should also set it to GUIDED mode if you want the vehicle to respond at all. https://pixhawk.org/dev/ros/ground_rover is for PX4 Pro autopilots only. For rovers probably you will find better info in http://ardupilot.org/rover/.

nanane3437 commented 6 years ago

In http://ardupilot.org/rover/ there is nothing about using the rover with mavros ... :/

TSC21 commented 6 years ago

In http://ardupilot.org/rover/ there is nothing about using the rover with mavros ... :/

Well then unless someone from APM can help you out, you won't find much of a help here unless you dig in yourself. I suppose APM Rover supports GUIDED mode, so from there you should be able to control the vehicle. But I can't say much more about it since I am not an APM user/dev.