MAVSDK-Python set PX4 local position with mocap commands

Themaksiest · May 18, 2020, 7:54pm

Hello.

I am trying to implement a single camera relative position detection for a quadcopter. Let’s say, I get a rotation and translation matrix that describes the necessary rotations and translations required to get from camera coordinate system to quadcopter coordinate frame. I would then like to us MAVSDK-Python, to set the quadcopter local position in relation to that camera (assuming I know the location and orientation of the camera or set it to arbitrary location and orientation), so that I can control the quadcopter without the GPS signal.

Can this be done with the information I have? I looked into the mocap commands, but was unable to understand the message format required to implement them:
System.mocap.set_vision_position_estimate()
System.mocap.set_attitude_position_mocap()
System.mocap.set_odometry()

If I understand correctly, I should set local position with set_attitude_position_mocap() but how are the other 2 used?

If the above stated works, can I then switch my quadcopter to offboard mode and set desired location with offboard.set_position_ned() to set the x,y,z and yaw?

I will be grateful for any advice or recommendations.

JulianOes · May 19, 2020, 6:01am

Do the comments here help in any way?

github.com

mavlink/MAVSDK-Proto/blob/master/protos/mocap/mocap.proto

syntax = "proto3";

package mavsdk.rpc.mocap;

import "mavsdk_options.proto";

option java_package = "io.mavsdk.mocap";
option java_outer_classname = "MocapProto";

/**
 * Allows interfacing a vehicle with a motion capture system in
 * order to allow navigation without global positioning sources available
 * (e.g. indoors, or when flying under a bridge. etc.).
 */
service MocapService {
    // Send Global position/attitude estimate from a vision source.
    rpc SetVisionPositionEstimate(SetVisionPositionEstimateRequest) returns(SetVisionPositionEstimateResponse) { option (mavsdk.options.async_type) = SYNC; }
    // Send motion capture attitude and position.
    rpc SetAttitudePositionMocap(SetAttitudePositionMocapRequest) returns(SetAttitudePositionMocapResponse) { option (mavsdk.options.async_type) = SYNC; }
    // Send odometry information with an external interface.

This file has been truncated. show original

I was planning to generate proper Python docs for the API.

Themaksiest · May 19, 2020, 6:54am

Hi @JulianOes
Thanks for the quick response.

Thanks for hint, it seems that all this documentation can be a bit overwhelming for a newcomer like me.

So if I understand correctly, I can set
q = mavsdk.Quaternion(w, x, y, z)
position_body = mavsdk.PositionBody(x_m, y_m, z_m)
pose_covariance = float('nan')
message = mavsdk.AttitudePositionMocap(time_usec, q, position_body, pose_covariance)
and then
await drone.mocap.set_vision_position_estimate(message)

So the Quaternion and PositionBody I can set to values calculated from rotation and translation matrix.
Given the correct PX4 configuration this would then give quadcopter position information, and it could hold the position I continuously send with drone.offboard.set_position_ned(message2), correct?

What happens when I set pose_covariance = float('nan') can the PX4 operate stably with pose_covariance = NaN? Maybe this is less MAVSDK question and more of a PX4 Autopilot question.