Wrong transformation for raw_setpoint

AlexisTM commented 7 years ago

Issue details

The transformation of the raw_setpoint plugin is correct for the FRAME_LOCAL_NED but not correct for the FRAME_BODY_NED.

MAVROS version and platform

Mavros:0.17.5, 0.20.1 ROS: Indigo, Kinetic Ubuntu: 14.04, 16.04

Autopilot type and version

[x] PX4 1.5.5

Code

The current implementation:

https://github.com/mavlink/mavros/blob/ca852ac0418b8e0a380c3086c75cfa2ba9302010/mavros/src/plugins/setpoint_raw.cpp#L145-L170

This is correct when using the FRAME_LOCAL_NED. Yet, on simulation and intuition, it seems to be wrong for the FRAME_BODY_NED (that we can set from the message).

With the following (@7Hz) :

    setpoint_msg.coordinate_frame =  mavros_msgs::PositionTarget::FRAME_BODY_NED;
    // Velocity(x,y,z) and yawrate not ignored
    setpoint_msg.type_mask =  0b0000011111000111;
    setpoint_msg.position.x = 0;
    setpoint_msg.position.y = 0;
    setpoint_msg.position.z = 0;
    setpoint_msg.velocity.x = 1;
    setpoint_msg.velocity.y = 0;
    setpoint_msg.velocity.z = 0;
    setpoint_msg.yaw = 0.0;
    setpoint_msg.yaw_rate = 1;

Indeed, sending Velocity (1,0,0) and yawrate(1) to the Pixhawk (in simulation, Firmware 1.5.5) with the FRAME_BODY_NED make the copter going along the x axis of the map frame but not of the copter. (While, as it is rotating, I expect the drone to 'make a circle' in the map frame)

NOTE: I did not check if it is implemented in the PX4 Firmware. That could be the issue.

TSC21 commented 7 years ago

@AlexisTM on the Firmware side: https://github.com/PX4/Firmware/commit/e01eaf172a50727ba241513303df0212b83e3558. We should though change the name of set_position_target_local_ned to set_position_target_ned since it it supports both frames of reference. But Firmware 1.5.5?? That's too old already. The above commit is already supported on 1.6.5 and upstream master. Though there's a fix I'm pushing in https://github.com/mavlink/mavros/pull/753 so to fix the yaw rotations and also taking into consideration the body frame rotations. Though I still have to test it (it's not complete though).

AlexisTM commented 7 years ago

Indeed, 1.5.5 is already old, yet, companies are not prone to changes. I do try to update it whenever possible. "If it is working, why would we change?".

I will close this for now as it seems to come from an old version issue.

mzahana commented 4 years ago

@TSC21 @vooon I still see a similar issue here. For example, when I send body velocity setpoint to /mavros/setpoint_raw/local with type_mask=FRAME_BODY_NED, I expect the convention (ROS) to be, x=forward, y=left, z=up (see REP-103). However, the result I get is x=right, y=front, z=up.

On the other hand, the /mavros/local_position/velocity_body topic shows correct ROS convention i.e. when the drone moves forward with respect to its body frame, I see changes in the +x coordinate, and similarly for the other axes. So, I think the body -frame-related transformations for setpoint_raw are not as expected. What do you think?

BTW, I tested this on Ubuntu 18 + ROS Melodic + PX4 v1.10.1 + MAVROS from apt-get

mavlink / mavros