From 70855027f30c0403994e2c50d51704f34985aa87 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 1 Apr 2015 09:14:03 -0700
Subject: [PATCH] autotest: added Iris ROS simulator support

use -f IrisRos
---
 Tools/autotest/pysim/iris_ros.py        | 114 ++++++++++++++++++++++++
 Tools/autotest/pysim/sim_multicopter.py |   3 +
 Tools/autotest/sim_vehicle.sh           |   4 +
 3 files changed, 121 insertions(+)
 create mode 100644 Tools/autotest/pysim/iris_ros.py

diff --git a/Tools/autotest/pysim/iris_ros.py b/Tools/autotest/pysim/iris_ros.py
new file mode 100644
index 0000000000..2fdc7cc54e
--- /dev/null
+++ b/Tools/autotest/pysim/iris_ros.py
@@ -0,0 +1,114 @@
+#!/usr/bin/env python
+'''
+Python interface to euroc ROS multirotor simulator
+See https://pixhawk.org/dev/ros/sitl
+'''
+
+from aircraft import Aircraft
+import util, time, math
+from math import degrees, radians
+from rotmat import Vector3, Matrix3
+from pymavlink.quaternion import Quaternion
+import rospy
+import std_msgs.msg as std_msgs
+import mav_msgs.msg as mav_msgs
+import rosgraph_msgs.msg as rosgraph_msgs
+import sensor_msgs.msg as sensor_msgs
+import px4.msg as px4
+
+def quat_to_dcm(q1, q2, q3, q4):
+    '''convert quaternion to DCM'''
+    q3q3 = q3 * q3
+    q3q4 = q3 * q4
+    q2q2 = q2 * q2
+    q2q3 = q2 * q3
+    q2q4 = q2 * q4
+    q1q2 = q1 * q2
+    q1q3 = q1 * q3
+    q1q4 = q1 * q4
+    q4q4 = q4 * q4
+
+    m = Matrix3()
+    m.a.x = 1.0-2.0*(q3q3 + q4q4)
+    m.a.y =   2.0*(q2q3 - q1q4)
+    m.a.z =   2.0*(q2q4 + q1q3)
+    m.b.x =   2.0*(q2q3 + q1q4)
+    m.b.y = 1.0-2.0*(q2q2 + q4q4)
+    m.b.z =   2.0*(q3q4 - q1q2)
+    m.c.x =   2.0*(q2q4 - q1q3)
+    m.c.y =   2.0*(q3q4 + q1q2)
+    m.c.z = 1.0-2.0*(q2q2 + q3q3)
+    return m
+
+
+class IrisRos(Aircraft):
+    '''a IRIS MultiCopter from ROS'''
+    def __init__(self):
+        Aircraft.__init__(self)
+        self.max_rpm = 1200
+        self.have_new_time = False
+        self.have_new_imu = False
+        self.have_new_pos = False
+
+        topics = {
+            "/clock"                       : (self.clock_cb, rosgraph_msgs.Clock),
+            "/iris/imu"                    : (self.imu_cb, sensor_msgs.Imu),
+            "/iris/vehicle_local_position"  : (self.pos_cb, px4.vehicle_local_position),
+            }
+        
+        rospy.init_node('ArduPilot', anonymous=True)
+        for topic in topics.keys():
+            (callback, msgtype) = topics[topic]
+            rospy.Subscriber(topic, msgtype, callback)
+
+        self.motor_pub = rospy.Publisher('/iris/command/motor_speed',
+                                         mav_msgs.CommandMotorSpeed,
+                                         queue_size=1)
+        self.last_time = 0
+   
+        # spin() simply keeps python from exiting until this node is stopped
+        #rospy.spin()
+
+    def clock_cb(self, msg):
+        self.time_now = self.time_base + msg.clock.secs + msg.clock.nsecs*1.0e-9
+        self.have_new_time = True
+
+    def imu_cb(self, msg):
+        self.gyro = Vector3(msg.angular_velocity.x,
+                            -msg.angular_velocity.y,
+                            -msg.angular_velocity.z)
+        self.accel_body = Vector3(msg.linear_acceleration.x,
+                                  -msg.linear_acceleration.y,
+                                  -msg.linear_acceleration.z)
+        self.dcm = quat_to_dcm(msg.orientation.w,
+                               msg.orientation.x,
+                               -msg.orientation.y,
+                               -msg.orientation.z)
+        self.have_new_imu = True
+        
+    def pos_cb(self, msg):
+        self.velocity = Vector3(msg.vx, msg.vy, msg.vz)
+        self.position = Vector3(msg.x, msg.y, msg.z)
+        self.have_new_pos = True
+
+    def update(self, actuators):
+        while self.last_time == self.time_now or not self.have_new_time or not self.have_new_imu or not self.have_new_pos:
+            time.sleep(0.001)
+        self.have_new_time = False
+        self.have_new_pos = False
+        self.have_new_imu = False
+
+        # create motor speed message
+        msg = mav_msgs.CommandMotorSpeed()
+        msg.header.stamp = rospy.get_rostime()
+        motor_speed = []
+        for i in range(len(actuators)):
+            motor_speed.append(actuators[i]*self.max_rpm)
+        msg.motor_speed = motor_speed
+
+        self.last_time = self.time_now
+
+        self.motor_pub.publish(msg)
+        
+        # update lat/lon/altitude
+        self.update_position()
diff --git a/Tools/autotest/pysim/sim_multicopter.py b/Tools/autotest/pysim/sim_multicopter.py
index ef25d4cd13..c091e19b51 100755
--- a/Tools/autotest/pysim/sim_multicopter.py
+++ b/Tools/autotest/pysim/sim_multicopter.py
@@ -2,6 +2,7 @@
 
 from multicopter import MultiCopter
 from helicopter import HeliCopter
+from iris_ros import IrisRos
 import util, time, os, sys, math
 import socket, struct
 import select, errno
@@ -137,6 +138,8 @@ fdm = fgFDM.fgFDM()
 # create the quadcopter model
 if opts.frame == 'heli':
     a = HeliCopter(frame=opts.frame)
+elif opts.frame == 'IrisRos':
+    a = IrisRos()
 else:    
     a = MultiCopter(frame=opts.frame)
 
diff --git a/Tools/autotest/sim_vehicle.sh b/Tools/autotest/sim_vehicle.sh
index 5241b56b9a..26cdbaa7d0 100755
--- a/Tools/autotest/sim_vehicle.sh
+++ b/Tools/autotest/sim_vehicle.sh
@@ -203,6 +203,10 @@ case $FRAME in
 	BUILD_TARGET="sitl-heli"
         EXTRA_SIM="$EXTRA_SIM --frame=heli"
 	;;
+    IrisRos)
+	BUILD_TARGET="sitl"
+        EXTRA_SIM="$EXTRA_SIM --frame=IrisRos"
+	;;
     elevon*)
         EXTRA_PARM="param set ELEVON_OUTPUT 4;"
         EXTRA_SIM="$EXTRA_SIM --elevon"