• Categories
  • Recent
  • Tags
  • Popular
  • Users
  • Groups
  • Register
  • Login
ModalAI Forum
  • Categories
  • Recent
  • Tags
  • Popular
  • Users
  • Groups
    • Register
    • Login

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

    Ask your questions right here!
    2
    5
    493
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • P
      Patrick Hinchey
      last edited by Patrick Hinchey 8 Dec 2022, 00:10 7 Dec 2022, 23:11

      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!

      1 Reply Last reply Reply Quote 0
      • 張
        張為超
        last edited by 8 Dec 2022, 02:23

        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. 👻

        1 Reply Last reply Reply Quote 0
        • P
          Patrick Hinchey
          last edited by 8 Dec 2022, 21:44

          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.

          張 1 Reply Last reply 9 Dec 2022, 02:14 Reply Quote 0
          • P
            Patrick Hinchey
            last edited by 8 Dec 2022, 21:45

            No tragedy happened though 🙂

            1 Reply Last reply Reply Quote 0
            • 張
              張為超 @Patrick Hinchey
              last edited by 張為超 9 Dec 2022, 02:20 9 Dec 2022, 02:14

              @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

              1 Reply Last reply Reply Quote 0
              1 out of 5
              • First post
                1/5
                Last post
              Powered by NodeBB | Contributors