Mavros: Holding Flight Position while giving the drone 2D velocity Command

[gy2256]

I am a PhD student currently working on an experiment that requires me to control drones with 2D Velocity Command (Vx, Vy). Essentially I want the drone to hold at a constant altitude and move around on the x,y plane.

My current setup is the following: DJI F450 frame, Odroid XU4, Pixhawk, PX4 Firmware with LPE (I am also using Optitrack). At this point, I am just trying to get my algorithm working in the Gazebo Simulator.
So far, I have got the waypoints working but I am having trouble to let the drone hold its position while sending Vx, Vy. Do anyone have experience with this? Any help would be appreciated!

[TSC21]

Are you feeding Optitrack data into the Pixhawk through MAVROS?

[gy2256]

I am feeding Optitrack Data as /mavros/mocap and I have also changed the Pixhawk parameters such that it uses this information for local position estimation, the result is actually pretty good.

But that being said, I am currently just trying to work with Gazebo simulator (not including mocap). In my code, I create both setpoint_position and setpoint_velcity publishers. I set "set_position.pose.position.z = 2" and "set_velocity.linear.x = 1" But the drone just hover at 2 meters, without moving anywhere.

And it gives me this error:
Could not process inbound connection: topic types do not match: [geometry_msgs/TwistStamped] vs. [geometry_msgs/Twist]{'topic': '/mavros/setpoint_velocity/cmd_vel', 'tcp_nodelay': '0', 'md5sum': '98d34b0043a2093cf9d9345ab6eef12e', 'type': 'geometry_msgs/TwistStamped', 'callerid': '/mavros'}

[gy2256]

I found the issue for the error message. I made a mistake when I try to send '/mavros/setpoint_velocity/cmd_vel'. The message type should be TwistStamped, instead of Twist. After modifying my set_velocity to 'set_velocity.twist.linear.x = 1' the error disappears.

However, a new issue appears. The drone is not stable at all, it keeps shaking as it moves forward (+x direction). I am not sure if my approach is correct. Please let me know if there is a formal way to do it.

[TSC21]

Could not process inbound connection: topic types do not match: [geometry_msgs/TwistStamped] vs. [geometry_msgs/Twist]{'topic': '/mavros/setpoint_velocity/cmd_vel', 'tcp_nodelay': '0', 'md5sum': '98d34b0043a2093cf9d9345ab6eef12e', 'type': 'geometry_msgs/TwistStamped', 'callerid': '/mavros'}

Actually the error states what is the problem. The velocity command you are sending is a Twist msg, while the node is expecting a TwistStamped msg.

[TSC21]

However, a new issue appears. The drone is not stable at all, it keeps shaking as it moves forward (+x direction). I am not sure if my approach is correct. Please let me know if there is a formal way to do it.

Without checking the code you are using, is kinda difficult to understand what's wrong.

[gy2256]

@TSC21 Thanks for the help! Unfortunately , I am not sure if I can post my original source code. I will get back to this issue once I have the permission. I apologize for the inconvenience.

I will also post the solution for this particular problem once I find it because I think it is a really good application/demo.

[TSC21]

I apologize for the inconvenience.

The inconvenience is not for us that are trying to help you. But asserting over the functionality of a piece of code without knowing the code is kinda difficult.

[gy2256]

@TSC21 Yes. I completely understand.

[gy2256]

OK. So here is a simple script I have to control both velocity and position.

import rospy
import numpy as np
import math
import mavros_msgs

from geometry_msgs.msg import PoseStamped, TwistStamped
from mavros_msgs import srv
from mavros_msgs.msg import State

#=================Parameter Initializaiton========================
goal_pose = PoseStamped()
current_pose = PoseStamped()
set_velocity = TwistStamped()
current_state = State()


def altitude_hold():
    global goal_pose
    goal_pose.pose.position.z = 2

#==============Call Back Functions=====================
def pos_sub_callback(pose_sub_data):
    global current_pose
    current_pose = pose_sub_data

def state_callback(state_data):
    global current_state
    current_state = state_data

#============Intialize Node, Publishers and Subscribers=================
rospy.init_node('Vel_Control_Node', anonymous = True)
rate = rospy.Rate(10) #publish at 10 Hz
local_position_subscribe = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pos_sub_callback)
local_position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size = 10)
setpoint_velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',TwistStamped, queue_size = 10)
state_status_subscribe = rospy.Subscriber('/mavros/state',State,state_callback)
altitude_hold()


#============Define Velocity==============================================
set_velocity.twist.linear.x = 1 #moving 1m/s at x direction


while not rospy.is_shutdown():
    local_position_pub.publish(goal_pose)

    if current_state.mode != "OFFBOARD" or not current_state.armed:
        arm = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
        arm(True)
        set_mode = rospy.ServiceProxy('/mavros/set_mode',mavros_msgs.srv.SetMode)
        mode = set_mode(custom_mode = 'OFFBOARD')

        if mode.success:
            rospy.loginfo('Switched to OFFBOARD mode!')

    setpoint_velocity_pub.publish(set_velocity)


    rate.sleep()

[TSC21]

You should base your code in this: https://dev.px4.io/en/ros/mavros_offboard.html.

I see you send the position setpoint only once while you should send it for a while. Then, when it check it's in the goal_pose, it should move on x direction with that velocity. Also, you should separate the verification of the mode and arm state into two different conditions.

[gy2256]

I am thinking about a different approach. Instead of rely on '/mavros/setpoint_position/local' to achieve the altitude hold, I might as well write my own PID altitude controller that takes the difference of the desired altitude and my current altitude (set_z - measured_z) as my input, and output velocity control in the z direction. Therefore, all I need is to publish Vz to maintain the altitude, it might require some tuning though.

[TSC21]

That is something I have already done in test_mavros with control_toolbox. Check what is there and maybe you don't have to implement it from scratch.

[gy2256]

Thanks! I will take a look at it and I believe it is a great approach. I think this issue can be closed now.

[schmittlema]

https://github.com/darknight-007/docking/blob/master/testVelocityControl.py

I modified this to get altitude hold while controling horizontal velocity. You can also try setpoint_raw, but I found that it didn't work.

[visrinivasan]

@schmittlema , can you tell me how did you modify to give a constant velocity with altitude hold?

[schmittlema]

Below. I made a 3 PID controllers for each axis. Then if the desired velocity is none it holds that axis position. Z is always trying to hold altitude. Hopefully it helps.
`"""
testing offboard positon control with a simple takeoff script
"""

import rospy
from mavros_msgs.msg import State
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Vector3, Twist, TwistStamped
import math
import numpy
from std_msgs.msg import Header
from PID import PID

class VelocityController:
target = PoseStamped()
output = TwistStamped()

def __init__(self):
    self.X = PID()
    self.Y = PID()
    self.Z = PID()
    self.lastTime = rospy.get_time()
    self.target = None

def setTarget(self, target):
    self.target = target

def update(self, state,x_vel,y_vel,hold_state):
    if (self.target is None):
        rospy.logwarn("Target position for velocity controller is none.")
        return None
    # simplify variables a bit
    time = state.header.stamp.to_sec()
    position = state.pose.position
    orientation = state.pose.orientation
    # create output structure
    output = TwistStamped()
    output.header = state.header
    # output velocities
    linear = Vector3()
    angular = Vector3()
    if x_vel == 0:
        self.target.position.x = hold_state.pose.position.x
        linear.x = self.X.update(self.target.position.x, position.x, time)
    else:
        linear.x = x_vel
    if y_vel == 0:
        self.target.position.y = hold_state.pose.position.y
        linear.y = self.Y.update(self.target.position.y, position.y, time)
    else:
        linear.y = y_vel
    # Control in Z vel
    linear.z = self.Z.update(self.target.position.z, position.z, time)
    # Control yaw (no x, y angular)
    # TODO
    output.twist = Twist()
    output.twist.linear = linear
    return output

def stop(self):
    setTarget(self.current)
    update(self.current)`

[AlexisTM]

Hey there. You cannot control Vx, Vy and Z, only velocities or only position. If the drone is shaking it's because it answer to the position setpoint then the velocity.

[TSC21]

@AlexisTM currently you can:
https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L902
https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L1005-L1008
https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L1612

Bitmasks are:
to control vx, vy, z, y -> 3043 (0xBE3)
to control vx, vy, z -> 4067 (0xFE3)
to control vx, vy, z, y_rate -> 2019 (0x7E3)

[AlexisTM]

Indeed, but he mentioned using setpoint_position and setpoint_velocity. He has to use setpoint_raw and set the type_mask.

Also, I am not certain the mode is working as the offboard_mode is converted to a vehicule_mode and the offboard_mode.ignore_alt_hold is not mentioned at all. Therefore, I never tested it.

https://github.com/PX4/Firmware/blob/bd84061ea54cd41289a9faf37c1cac5868c11773/src/modules/commander/commander.cpp#L4001-L4039