From 562f4ed1a366345302804d356aa1a041eb10841f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Aug 2019 17:56:47 +1000 Subject: [PATCH] Tools: added Vicon mavlink gateway script --- Tools/Vicon/README.md | 11 +++ Tools/Vicon/vicon_mavlink.py | 174 +++++++++++++++++++++++++++++++++++ 2 files changed, 185 insertions(+) create mode 100644 Tools/Vicon/README.md create mode 100755 Tools/Vicon/vicon_mavlink.py diff --git a/Tools/Vicon/README.md b/Tools/Vicon/README.md new file mode 100644 index 0000000000..b5da6860c0 --- /dev/null +++ b/Tools/Vicon/README.md @@ -0,0 +1,11 @@ +# Vicon MAVLink Gateway + +This script produces GLOBAL_VISION_POSITION_ESTIMATE, GPS_INPUT and +GPS_GLOBAL_ORIGIN messages based on data from a Vicon indoor +positioning system. + +It uses the pyvicon library from here: + + https://github.com/MathGaron/pyvicon + +which is a python wrapper around the Vicon SDK. diff --git a/Tools/Vicon/vicon_mavlink.py b/Tools/Vicon/vicon_mavlink.py new file mode 100755 index 0000000000..64af3b6886 --- /dev/null +++ b/Tools/Vicon/vicon_mavlink.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +''' +connect to a vicon and send mavlink vision data on UDP +Released under GNU GPL v3 or later +''' + +from pyvicon import pyvicon +from pymavlink.quaternion import Quaternion +from pymavlink import mavutil +from pymavlink import mavextra +import math +import time +import sys + +from argparse import ArgumentParser +parser = ArgumentParser(description="vicon to mavlink gateway") + +parser.add_argument("--vicon-host", type=str, + help="vicon host name or IP", default="vicon") +parser.add_argument("--mavlink-dest", type=str, help="mavlink destination", default="udpout:127.0.0.1:14550") +parser.add_argument("--origin", type=str, help="origin in lat,lon,alt,yaw", default="-35.363261,149.165230,584,270") +parser.add_argument("--debug", type=int, default=0, help="debug level") +parser.add_argument("--target-sysid", type=int, default=1, help="target mavlink sysid") +parser.add_argument("--source-sysid", type=int, default=255, help="mavlink source sysid") +parser.add_argument("--source-component", type=int, default=mavutil.mavlink.MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY, help="mavlink source component") +parser.add_argument("--rate", type=float, default=None, help="message rate for GLOBAL_VISION_POSITION_ESTIMATE") +parser.add_argument("--gps-rate", type=int, default=5, help="message rate for GPS_INPUT") +parser.add_argument("--gps-nsats", type=int, default=16, help="GPS satellite count") + +args = parser.parse_args() + +# create a mavlink serial instance +mav = mavutil.mavlink_connection(args.mavlink_dest, + source_system=args.source_sysid, + source_component=args.source_component) + +# create vicon connection +vicon = pyvicon.PyVicon() + +origin_parts = args.origin.split(',') +if len(origin_parts) != 4: + print("Origin should be lat,lon,alt,yaw") + sys.exit(1) +origin = (float(origin_parts[0]), float(origin_parts[1]), float(origin_parts[2])) + +last_origin_send = 0 + +def connect_to_vicon(ip): + '''connect to a vicon with given ip or hostname''' + global vicon + vicon.connect(ip) + vicon.set_stream_mode(pyvicon.StreamMode.ClientPull) + vicon.enable_marker_data() + vicon.enable_segment_data() + vicon.enable_unlabeled_marker_data() + vicon.enable_device_data() + # wait for first subject to appear + while True: + vicon.get_frame() + name = vicon.get_subject_name(0) + if name is not None: + break + print("Connected to subject %s" % name) + +def get_gps_time(tnow): + '''return gps_week and gps_week_ms for current time''' + leapseconds = 18 + SEC_PER_WEEK = 7 * 86400 + MSEC_PER_WEEK = SEC_PER_WEEK * 1000 + + epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - leapseconds + epoch_seconds = int(tnow - epoch) + week = int(epoch_seconds) // SEC_PER_WEEK + t_ms = int(tnow * 1000) % 1000 + week_ms = (epoch_seconds % SEC_PER_WEEK) * 1000 + ((t_ms//200) * 200) + return week, week_ms + + +def main_loop(): + '''loop getting vicon data''' + global vicon + name = vicon.get_subject_name(0) + segname = vicon.get_subject_root_segment_name(name) + last_msg_time = time.time() + last_gps_pos = None + now = time.time() + last_origin_send = now + now_ms = int(now * 1000) + last_gps_send_ms = now_ms + last_gps_send = now + gps_period_ms = 1000 // args.gps_rate + + while True: + vicon.get_frame() + + now = time.time() + now_ms = int(now * 1000) + + if args.rate is not None: + dt = now - last_msg_time + if dt < 1.0 / args.rate: + continue + + last_msg_time = now + + # get position as ENU mm + pos_enu = vicon.get_segment_global_translation(name, segname) + if pos_enu is None: + continue + + # convert to NED meters + pos_ned = [pos_enu[1]*0.001, pos_enu[0]*0.001, -pos_enu[2]*0.001] + + # get orientation in euler, converting to ArduPilot conventions + quat = vicon.get_segment_global_quaternion(name, segname) + q = Quaternion([quat[3], quat[0], quat[1], quat[2]]) + euler = q.euler + roll, pitch, yaw = euler[2], euler[1], euler[0] + yaw -= math.pi*0.5 + + if args.debug > 0: + print("Pose: [%.3f, %.3f, %.3f] [%.2f %.2f %.2f]" % ( + pos_ned[0], pos_ned[1], pos_ned[2], + math.degrees(roll), math.degrees(pitch), math.degrees(yaw))) + + time_us = int(now * 1.0e6) + + if now - last_origin_send > 1: + # send a heartbeat msg + mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0) + + # send origin at 1Hz + mav.mav.set_gps_global_origin_send(args.target_sysid, + int(origin[0]*1.0e7), int(origin[1]*1.0e7), int(origin[2]*1.0e3), + time_us) + last_origin_send = now + + if args.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms: + '''send GPS data at the specified rate, trying to align on the given period''' + if last_gps_pos is None: + last_gps_pos = pos_ned + gps_lat, gps_lon = mavextra.gps_offset(origin[0], origin[1], pos_ned[0], pos_ned[1]) + gps_alt = origin[2] - pos_ned[2] + + gps_dt = now - last_gps_send + gps_vel = [ (pos_ned[0]-last_gps_pos[0])/gps_dt, + (pos_ned[1]-last_gps_pos[1])/gps_dt, + (pos_ned[2]-last_gps_pos[2])/gps_dt ] + + gps_week, gps_week_ms = get_gps_time(now) + + if args.gps_nsats >= 6: + fix_type = 3 + else: + fix_type = 1 + mav.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week, fix_type, + int(gps_lat*1.0e7), int(gps_lon*1.0e7), gps_alt, + 1.0, 1.0, + gps_vel[0], gps_vel[1], gps_vel[2], + 0.2, 1.0, 1.0, + args.gps_nsats) + last_gps_send_ms = (now_ms//gps_period_ms) * gps_period_ms + last_gps_send = now + + + # send VISION_POSITION_ESTIMATE + mav.mav.global_vision_position_estimate_send(time_us, + pos_ned[0], pos_ned[1], pos_ned[2], + roll, pitch, yaw) + + +connect_to_vicon("vicon") +main_loop() +