0,0,1 not going where I think it will

Patrick Hinchey

Hello,

I have an fixed-type AprilTag located at 0,0,0. I have a rospy routine that commands the vehicle to 0,0,1 over MAVROS with the script below.

When I switch to offboard mode, the drone very deliberately flies to the same place every time, but it's not above the tag. It can see the tag in its FoV and voxl-portal draws a little square around it and labels it "0". Am I publishing to the right topic? Did I miss something in the AprilTag setup? I notice in MAVSDK you specify the reference frame, but I don't see anything about that in rospy MAVROS.

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest



current_state = State()

def state_cb(msg):
    global current_state
    current_state = msg


if __name__ == "__main__":
    rospy.init_node("offb_node_py")

    state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)

    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
    

    # Setpoint publishing MUST be faster than 2Hz
    rate = rospy.Rate(20)

    # Wait for Flight Controller connection
    while(not rospy.is_shutdown() and not current_state.connected):
        rate.sleep()
    rospy.loginfo("Connected to MAVROS")

    target_pose = PoseStamped()

    # Initial pose 1m above tag - go here as soon as mode is switched to OFFBOARD.
    target_pose.pose.position.x = 0
    target_pose.pose.position.y = 0
    target_pose.pose.position.z = 1


    while(not rospy.is_shutdown()):
        local_pos_pub.publish(target_pose)

        rate.sleep()

Here is a snippet from tag_locations.conf:

"locations":	[{
			"id":	0,
			"name":	"nest-roof",
			"loc_type":	"fixed",
			"size_m":	0.226,
			"T_tag_wrt_fixed":	[0, 0, 0],
			"R_tag_to_fixed":	[[0, -1, 0], [1, 0, 0], [0, 0, 1]]
		},

Here is a snippet from voxl-vision-px4.conf

{
	"config_file_version":	1,
	"qgc_ip":	"192.168.8.60",
	"en_secondary_qgc":	false,
	"secondary_qgc_ip":	"192.168.1.214",
	"qgc_udp_port_number":	14550,
	"udp_mtu":	512,
	"en_localhost_mavlink_udp":	true,
	"localhost_udp_port_number":	14551,
	"vio_pipe":	"qvio",
	"en_vio":	true,
	"en_send_vio_to_qgc":	true,
	"en_reset_vio_if_initialized_inverted":	true,
	"vio_warmup_s":	3,
	"send_odom_while_failed":	false,
	"horizon_cal_tolerance":	0.5,
	"offboard_mode":	"off",
	"follow_tag_id":	0,
	"figure_eight_move_home":	true,
	"robot_radius":	0.300000011920929,
	"collision_sampling_dt":	0.1,
	"max_lookahead_distance":	1,
	"en_tag_fixed_frame":	true,
	"fixed_frame_filter_len":	5,
	"en_transform_mavlink_pos_setpoints_from_fixed_frame":	true,
	"en_voa":	false,
	"en_send_voa_to_qgc":	false,
	"voa_upper_bound_m":	-0.15000000596046448,
	"voa_lower_bound_m":	0.15000000596046448,
	"voa_memory_s":	1,
	"voa_inputs":	[{

Thanks in advance!

張為超

It's been a while since I use apriltag, so I'm not 100% sure.....

But I think you can try to use /mavros/setpoint_raw/local as publisher topic and give it with coordinates frame as FRAME_LOCAL_NED, mask with 1991 and give it a try.

Again I'm not 100% sure so I'm not responsibility any tragedy that happened.

Patrick Hinchey

Thanks for the tip!
I tried your suggestion and it didn't fix the problem, but I did find something else out:
If I send it to 0,0,1, then fly away in POSITION mode, then send it back to 0,0,1, and repeat, it always goes back to the same place, but that place slowly drifts as you would expect VIO to do. Even though a fixed AprilTag is in view. I can see that it identifies the tag in voxl-portal.
So from that I deduce that it's not relocalising using the AprilTag.
What setting an I missing? I've set en_fixed_frame, and my tag is set as fixed.

Patrick Hinchey

No tragedy happened though

張為超

@Patrick-Hinchey looks like your pose didn't fixed according to apriltag this time. Maybe you can try to handhold the drone and inspect voxl-vision-px4 -f or inspect voxl-pose -f to see if the apriltag fix your position or not.

If not maybe you can follow https://docs.modalai.com/voxl-tag-detector/ to setup. If it does change pose, check the value to make sure it's correct according to apirltag