Mavros: Memegang Posisi Penerbangan sambil memberikan Perintah kecepatan 2D drone

Apakah Anda memasukkan data Optitrack ke Pixhawk melalui MAVROS?

Saya memasukkan Data Optitrack sebagai /mavros/mocap dan saya juga telah mengubah parameter Pixhawk sehingga menggunakan informasi ini untuk estimasi posisi lokal, hasilnya sebenarnya cukup bagus.

Namun demikian, saat ini saya hanya mencoba bekerja dengan simulator Gazebo (tidak termasuk mocap). Dalam kode saya, saya membuat penerbit setpoint_position dan setpoint_velcity. Saya mengatur "set_position.pose.position.z = 2" dan "set_velocity.linear.x = 1" Tapi drone hanya melayang di 2 meter, tanpa bergerak ke mana pun.

Dan itu memberi saya kesalahan ini:
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'}

Saya menemukan masalah untuk pesan kesalahan. Saya membuat kesalahan saat mencoba mengirim '/mavros/setpoint_velocity/cmd_vel'. Jenis pesan harus TwistStamped, bukan Twist. Setelah memodifikasi set_velocity saya menjadi 'set_velocity.twist.linear.x = 1' kesalahannya hilang.

Namun, masalah baru muncul. Drone tidak stabil sama sekali, terus bergetar saat bergerak maju (+x arah). Saya tidak yakin apakah pendekatan saya benar. Tolong beri tahu saya jika ada cara formal untuk melakukannya.

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'}

Sebenarnya kesalahan menyatakan apa masalahnya. Perintah kecepatan yang Anda kirim adalah Twist msg, sedangkan node mengharapkan TwistStamped msg.

Namun, masalah baru muncul. Drone tidak stabil sama sekali, terus bergetar saat bergerak maju (+x arah). Saya tidak yakin apakah pendekatan saya benar. Tolong beri tahu saya jika ada cara formal untuk melakukannya.

Tanpa memeriksa kode yang Anda gunakan, agak sulit untuk memahami apa yang salah.

@TSC21 Terima kasih atas bantuannya! Sayangnya, saya tidak yakin apakah saya dapat memposting kode sumber asli saya. Saya akan kembali ke masalah ini setelah saya mendapat izin. Saya mohon maaf atas ketidaknyamanan ini.

Saya juga akan memposting solusi untuk masalah khusus ini setelah saya menemukannya karena menurut saya ini adalah aplikasi/demo yang sangat bagus.

Saya mohon maaf atas ketidaknyamanan ini.

Ketidaknyamanan ini bukan untuk kami yang mencoba membantu Anda. Tetapi menegaskan fungsionalitas sepotong kode tanpa mengetahui kodenya agak sulit.

@TSC21 Ya. Saya benar-benar mengerti.

OKE. Jadi di sini adalah skrip sederhana saya harus mengontrol kecepatan dan posisi.

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()

Anda harus mendasarkan kode Anda pada ini: https://dev.px4.io/en/ros/mavros_offboard.html.

Saya melihat Anda mengirim setpoint posisi hanya sekali sementara Anda harus mengirimnya untuk sementara waktu. Kemudian, ketika memeriksanya di goal_pose , ia harus bergerak ke arah x dengan kecepatan itu. Juga, Anda harus memisahkan verifikasi mode dan status lengan menjadi dua kondisi yang berbeda.

Saya sedang memikirkan pendekatan yang berbeda. Alih-alih mengandalkan '/mavros/setpoint_position/local' untuk mencapai ketinggian yang ditahan, saya mungkin juga menulis pengontrol ketinggian PID saya sendiri yang mengambil perbedaan ketinggian yang diinginkan dan ketinggian saya saat ini (set_z - terukur_z) sebagai input saya, dan kontrol kecepatan keluaran dalam arah z. Oleh karena itu, yang saya butuhkan hanyalah memublikasikan Vz untuk mempertahankan ketinggian, meskipun mungkin memerlukan beberapa penyetelan.

Itu adalah sesuatu yang telah saya lakukan di test_mavros dengan control_toolbox . Periksa apa yang ada dan mungkin Anda tidak harus mengimplementasikannya dari awal.

Terima kasih! Saya akan melihatnya dan saya percaya ini adalah pendekatan yang bagus. Saya pikir masalah ini bisa ditutup sekarang.

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

Saya memodifikasi ini untuk menahan ketinggian sambil mengontrol kecepatan horizontal. Anda juga dapat mencoba setpoint_raw, tetapi ternyata tidak berhasil.

@schmittlema , dapatkah Anda memberi tahu saya bagaimana Anda memodifikasi untuk memberikan kecepatan konstan dengan penahanan ketinggian?

Di bawah. Saya membuat 3 pengontrol PID untuk setiap sumbu. Kemudian jika kecepatan yang diinginkan tidak ada, ia memegang posisi sumbu itu. Z selalu berusaha menahan ketinggian. Semoga membantu.
`"""
menguji kontrol posisi offboard dengan skrip lepas landas sederhana
"""

impor rospy
dari mavros_msgs.msg mengimpor Negara
dari geometri_msgs.msg import PoseStamped, Pose, Point, Quaternion, Vector3, Twist, TwistStamped
impor matematika
impor numpy
dari std_msgs.msg impor Header
dari PID impor PID

kelas VelocityController:
target = PoseStamped()
keluaran = 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)`

Hei di sana. Anda tidak dapat mengontrol Vx, Vy dan Z, hanya kecepatan atau posisi saja. Jika drone bergetar itu karena menjawab setpoint posisi kemudian kecepatan.

@AlexisTM saat ini Anda dapat:
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

Bitmask adalah:
untuk mengontrol vx, vy, z, y -> 3043 ( 0xBE3 )
untuk mengontrol vx, vy, z -> 4067 ( 0xFE3 )
untuk mengontrol vx, vy, z, y_rate -> 2019 ( 0x7E3 )

Memang, tetapi dia menyebutkan menggunakan setpoint_position dan setpoint_velocity . Dia harus menggunakan setpoint_raw dan mengatur type_mask.

Juga, saya tidak yakin mode ini berfungsi karena offboard_mode diubah menjadi vehicule_mode dan offboard_mode.ignore_alt_hold tidak disebutkan sama sekali. Karena itu, saya tidak pernah mengujinya.

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