extras: mavteleop: Dirty implementation of position control mode.

Issue #133.

mavros_extras/scripts/mavteleop

@@ -26,7 +26,7 @@ import rospy
 from tf.transformations import quaternion_from_euler
 from sensor_msgs.msg import Joy
 from std_msgs.msg import Header, Float64
-from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3, Quaternion
+from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3, Quaternion, Point
 from mavros.msg import OverrideRCIn
 
 
@@ -203,6 +203,47 @@ def velocity_setpoint_control(args):
     rospy.spin()
 
 
+px, py, pz = 0.0, 0.0, 0.0
+
+def position_setpoint_control(args):
+    rospy.init_node("mavteleop")
+    rospy.loginfo("MAV-Teleop: Position setpoint control type.")
+
+    load_map(axes_map, '~axes_map/')
+    load_map(axes_scale, '~axes_scale/')
+    load_map(button_map, '~button_map/')
+
+    pos_pub = rospy.Publisher(args.mavros_ns + "/setpoint/local_position", PoseStamped, queue_size=10)
+
+    def joy_cb(joy):
+        global px, py, pz
+        # get axes normalized to -1.0..+1.0 RPY
+        roll = get_axis(joy, 'roll')
+        pitch = get_axis(joy, 'pitch')
+        yaw = get_axis(joy, 'yaw')
+        throttle = get_axis(joy, 'throttle')
+
+        # TODO: need integrate by time, joy_cb() called with variable frequency
+        px -= pitch
+        py += roll
+        pz += throttle
+
+        rospy.logdebug("RPYT: %f, %f, %f, %f", roll, pitch, yaw, throttle)
+        rospy.logdebug("Point(%f %f %f)", px, py, pz)
+
+        # Based on QGC UAS joystickinput_settargets branch
+        pose = PoseStamped(header=Header(stamp=rospy.get_rostime()))
+        pose.pose.position = Point(x=px, y=py, z=pz)
+        q = quaternion_from_euler(0, 0, yaw)
+        pose.pose.orientation = Quaternion(*q)
+
+        pos_pub.publish(pose)
+
+
+    jsub = rospy.Subscriber("/joy", Joy, joy_cb)
+    rospy.spin()
+
+
 def main():
     parser = argparse.ArgumentParser(description="Teleoperation script for Copter-UAV")
     parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default="/mavros")
@@ -211,6 +252,7 @@ def main():
     mode_group.add_argument('-rc', '--rc-override', action='store_true', help="use rc override control type")
     mode_group.add_argument('-att', '--sp-attitude', action='store_true', help="use attitude setpoint control type")
     mode_group.add_argument('-vel', '--sp-velocity', action='store_true', help="use velocity setpoint control type")
+    mode_group.add_argument('-pos', '--sp-position', action='store_true', help="use position setpoint control type")
 
     args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])
 
@@ -220,8 +262,8 @@ def main():
         attitude_setpoint_control(args)
     elif args.sp_velocity:
         velocity_setpoint_control(args)
-    else:
-        raise NotImplementedError
+    elif args.sp_position:
+        position_setpoint_control(args)
 
 
 if __name__ == '__main__':