diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt
index b3cb2aba..c13ff221 100644
--- a/clever/CMakeLists.txt
+++ b/clever/CMakeLists.txt
@@ -7,7 +7,12 @@ project(clever)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  std_msgs
+  message_generation
+)
+
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -50,11 +55,19 @@ find_package(catkin REQUIRED)
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  SetPosition.srv
+  SetPositionYawRate.srv
+  SetPositionGlobal.srv
+  SetPositionGlobalYawRate.srv
+  SetVelocity.srv
+  SetVelocityYawRate.srv
+  SetAttitude.srv
+  SetAttitudeYawRate.srv
+  SetRatesYaw.srv
+  SetRates.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -64,10 +77,10 @@ find_package(catkin REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs  # Or other packages containing msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch
index 011db8e4..94ba7954 100644
--- a/clever/launch/clever.launch
+++ b/clever/launch/clever.launch
@@ -27,8 +27,8 @@
 
     <!-- TODO: marker position estimator -->
 
-    <!-- TODO: simplified offboard control -->
-    <!--<node name="offboard" pkg="clever" type="offboard.py" output="screen"/>-->
+    <!-- simplified offboard control -->
+    <node name="simple_offboard" pkg="clever" type="simple_offboard.py"/>
 
     <!-- bottom camera -->
     <include file="$(find clever)/launch/bottom_camera.launch" if="$(arg camera)"/>
diff --git a/clever/requirements.txt b/clever/requirements.txt
index 484d3c57..6cb6a548 100644
--- a/clever/requirements.txt
+++ b/clever/requirements.txt
@@ -1 +1,2 @@
 flask==0.12.2
+geopy==1.11.0
diff --git a/clever/src/global_local.py b/clever/src/global_local.py
new file mode 100644
index 00000000..a4f8f047
--- /dev/null
+++ b/clever/src/global_local.py
@@ -0,0 +1,36 @@
+import rospy
+import math
+import geopy
+from geometry_msgs.msg import PoseStamped
+from geopy.distance import VincentyDistance, vincenty
+from sensor_msgs.msg import NavSatFix
+
+
+def global_to_local(lat, lon):
+    # TODO: refactor
+
+    position_global = rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=5)
+    pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=5)
+
+    d = math.hypot(pose.pose.position.x, pose.pose.position.y)
+
+    bearing = math.degrees(math.atan2(-pose.pose.position.x, -pose.pose.position.y))
+    if bearing < 0:
+        bearing += 360
+
+    cur = geopy.Point(position_global.latitude, position_global.longitude)
+    origin = VincentyDistance(meters=d).destination(cur, bearing)
+
+    _origin = origin.latitude, origin.longitude
+    olat_tlon = origin.latitude, lon
+    tlat_olon = lat, origin.longitude
+
+    N = vincenty(_origin, tlat_olon)
+    if lat < origin.latitude:
+        N = -N
+
+    E = vincenty(_origin, olat_tlon)
+    if lon < origin.longitude:
+        E = -E
+
+    return E.meters, N.meters
diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py
new file mode 100644
index 00000000..c091c068
--- /dev/null
+++ b/clever/src/simple_offboard.py
@@ -0,0 +1,301 @@
+#!/usr/bin/env python
+from __future__ import division
+
+import rospy
+from geometry_msgs.msg import TransformStamped, PoseStamped, Point, Vector3, Vector3Stamped
+import tf2_ros
+import tf2_geometry_msgs
+from mavros_msgs.msg import PositionTarget, AttitudeTarget, State
+from mavros_msgs.srv import CommandBool, SetMode
+from threading import Lock
+import math
+
+from global_local import global_to_local
+from util import euler_from_orientation, vector3_from_point, orientation_from_euler
+from std_srvs.srv import Trigger
+from clever import srv
+
+
+rospy.init_node('simple_offboard')
+
+
+# TF2 stuff
+tf_broadcaster = tf2_ros.TransformBroadcaster()
+static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
+
+tf_buffer = tf2_ros.Buffer()
+tf_listener = tf2_ros.TransformListener(tf_buffer)
+
+
+def init_fcu_horiz():
+    # `fcu_horiz` frame publishing
+
+    tr = TransformStamped()
+    tr.header.frame_id = 'local_origin'
+    tr.child_frame_id = 'fcu_horiz'
+
+    def update_pose(data):
+        tr.header.stamp = data.header.stamp
+        tr.transform.translation = vector3_from_point(data.pose.position)
+        yaw = euler_from_orientation(data.pose.orientation)[2]
+        tr.transform.rotation = orientation_from_euler(0, 0, yaw)
+        tf_broadcaster.sendTransform(tr)
+
+    rospy.Subscriber('/mavros/local_position/pose', PoseStamped, update_pose)
+
+
+init_fcu_horiz()
+
+
+position_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=1)
+attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
+arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
+set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
+
+
+state = None
+
+
+def state_update(data):
+    global state
+    state = data
+
+
+rospy.Subscriber('/mavros/state', State, state_update)
+
+
+AUTO_OFFBOARD = rospy.get_param('~auto_offboard', True)
+AUTO_ARM = AUTO_OFFBOARD and rospy.get_param('~auto_arm', True)
+OFFBOARD_TIMEOUT = rospy.Duration(rospy.get_param('~offboard_timeout', 3))
+ARM_TIMEOUT = rospy.Duration(rospy.get_param('~arm_timeout', 5))
+TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3))
+SETPOINT_RATE = rospy.get_param('~setpoint_rate', 50)
+
+
+def offboard_and_arm():
+    if AUTO_OFFBOARD and state.mode != 'OFFBOARD':
+        rospy.sleep(.3)
+        rospy.loginfo('Switch mode to OFFBOARD')
+        res = set_mode(base_mode=0, custom_mode='OFFBOARD')
+
+        start = rospy.get_rostime()
+        while True:
+            if state.mode == 'OFFBOARD':
+                break
+            if rospy.get_rostime() - start > OFFBOARD_TIMEOUT:
+                raise Exception('OFFBOARD request timed out')
+
+    if AUTO_ARM and not state.armed:
+        rospy.loginfo('Arming')
+        res = arming(True)
+
+        start = rospy.get_rostime()
+        while True:
+            if state.armed:
+                return True
+
+            if rospy.get_rostime() - start > ARM_TIMEOUT:
+                raise Exception('Arming timed out')
+
+
+ps = PoseStamped()
+vs = Vector3Stamped()
+
+
+def get_publisher_and_message(req, stamp):
+    ps.header.stamp = stamp
+    vs.header.stamp = stamp
+
+    if isinstance(req, srv.SetPositionRequest):
+        ps.header.frame_id = req.frame_id or 'local_origin'
+        ps.pose.position = Point(req.x, req.y, req.z)
+        ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
+        pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
+
+        msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
+                             type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
+                                       PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
+                                       PositionTarget.IGNORE_YAW_RATE,
+                             position=pose_local.pose.position,
+                             yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2)
+        return position_pub, msg
+
+    elif isinstance(req, srv.SetPositionYawRateRequest):
+        ps.header.frame_id = req.frame_id or 'local_origin'
+        ps.pose.position = Point(req.x, req.y, req.z)
+        pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
+        msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
+                             type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
+                                       PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
+                                       PositionTarget.IGNORE_YAW,
+                             position=pose_local.pose.position,
+                             yaw_rate=req.yaw_rate)
+        return position_pub, msg
+
+    elif isinstance(req, srv.SetPositionGlobalRequest):
+        x, y = global_to_local(req.lat, req.lon)
+
+        ps.header.frame_id = req.frame_id or 'local_origin'
+        ps.pose.position = Point(0, 0, req.z)
+        ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
+        pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
+        pose_local.pose.position.x = x
+        pose_local.pose.position.y = y
+
+        msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
+                             type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
+                                       PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
+                                       PositionTarget.IGNORE_YAW_RATE,
+                             position=pose_local.pose.position,
+                             yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2)
+        return position_pub, msg
+
+    elif isinstance(req, srv.SetPositionGlobalYawRateRequest):
+        x, y = global_to_local(req.lat, req.lon)
+
+        ps.header.frame_id = req.frame_id or 'local_origin'
+        ps.pose.position = Point(0, 0, req.z)
+        pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
+        pose_local.pose.position.x = x
+        pose_local.pose.position.y = y
+
+        msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
+                             type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
+                                       PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
+                                       PositionTarget.IGNORE_YAW,
+                             position=pose_local.pose.position,
+                             yaw_rate=req.yaw_rate)
+        return position_pub, msg
+
+    elif isinstance(req, srv.SetVelocityRequest):
+        vs.vector = Vector3(req.vx, req.vy, req.vz)
+        vs.header.frame_id = req.frame_id or 'local_origin'
+        ps.header.frame_id = req.frame_id or 'local_origin'
+        ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
+        pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
+        vector_local = tf_buffer.transform(vs, 'local_origin', TRANSFORM_TIMEOUT)
+        msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
+                             type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ +
+                                       PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
+                                       PositionTarget.IGNORE_YAW_RATE,
+                             velocity=vector_local.vector,
+                             yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2)
+        return position_pub, msg
+
+    elif isinstance(req, srv.SetVelocityYawRateRequest):
+        vs.vector = Vector3(req.vx, req.vy, req.vz)
+        vs.header.frame_id = req.frame_id or 'local_origin'
+        vector_local = tf_buffer.transform(vs, 'local_origin', TRANSFORM_TIMEOUT)
+        msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
+                             type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ +
+                                       PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
+                                       PositionTarget.IGNORE_YAW,
+                             velocity=vector_local.vector,
+                             yaw_rate=req.yaw_rate)
+        return position_pub, msg
+
+    elif isinstance(req, srv.SetAttitudeRequest):
+        ps.header.frame_id = req.frame_id or 'local_origin'
+        ps.pose.orientation = orientation_from_euler(req.roll, req.pitch, req.yaw)
+        pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
+        msg = AttitudeTarget(orientation=pose_local.pose.orientation,
+                             thrust=req.thrust,
+                             type_mask=AttitudeTarget.IGNORE_YAW_RATE + AttitudeTarget.IGNORE_PITCH_RATE +
+                                       AttitudeTarget.IGNORE_ROLL_RATE)
+        return attitude_pub, msg
+
+    elif isinstance(req, srv.SetAttitudeYawRateRequest):
+        msg = AttitudeTarget(orientation=orientation_from_euler(req.roll, req.pitch, 0),
+                             thrust=req.thrust,
+                             type_mask=AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_ROLL_RATE)
+        msg.body_rate.z = req.yaw_rate
+        return attitude_pub, msg
+
+    elif isinstance(req, srv.SetRatesYawRequest):
+        ps.header.frame_id = req.frame_id or 'local_origin'
+        ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
+        pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
+        msg = AttitudeTarget(orientation=pose_local.pose.orientation,
+                             thrust=req.thrust,
+                             type_mask=AttitudeTarget.IGNORE_YAW_RATE,
+                             body_rate=Vector3(req.roll_rate, req.pitch_rate, 0))
+        return attitude_pub, msg
+
+    elif isinstance(req, srv.SetRatesRequest):
+        msg = AttitudeTarget(thrust=req.thrust,
+                             type_mask=AttitudeTarget.IGNORE_ATTITUDE,
+                             body_rate=Vector3(req.roll_rate, req.pitch_rate, req.yaw_rate))
+        return attitude_pub, msg
+
+
+current_pub = None
+current_msg = None
+current_req = None
+handle_lock = Lock()
+
+
+def handle(req):
+    global current_pub, current_msg, current_req
+    with handle_lock:
+        try:
+            stamp = rospy.get_rostime()
+            current_pub, current_msg = get_publisher_and_message(req, stamp)
+            rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg)
+
+            current_msg.header.stamp = stamp
+            current_pub.publish(current_msg)
+
+            offboard_and_arm()
+            return {'success': True}
+
+        except Exception as e:
+            rospy.logerr(str(e))
+            return {'success': False, 'message': str(e)}
+
+
+def release(req):
+    global current_pub
+    current_pub = None
+    rospy.loginfo('simple_offboard: release')
+    return {'success': True}
+
+
+rospy.Service('set_position', srv.SetPosition, handle)
+rospy.Service('set_position/yaw_rate', srv.SetPositionYawRate, handle)
+rospy.Service('set_position_global', srv.SetPositionGlobal, handle)
+rospy.Service('set_position_global/yaw_rate', srv.SetPositionGlobalYawRate, handle)
+rospy.Service('set_velocity', srv.SetVelocity, handle)
+rospy.Service('set_velocity/yaw_rate', srv.SetVelocityYawRate, handle)
+rospy.Service('set_attitude', srv.SetAttitude, handle)
+rospy.Service('set_attitude/yaw_rate', srv.SetAttitudeYawRate, handle)
+rospy.Service('set_rates', srv.SetRates, handle)
+rospy.Service('set_rates/yaw', srv.SetRatesYaw, handle)
+rospy.Service('release', Trigger, release)
+
+
+rospy.loginfo('simple_offboard inited')
+
+
+def start_loop():
+    global current_pub, current_msg, current_req
+    r = rospy.Rate(SETPOINT_RATE)
+
+    while not rospy.is_shutdown():
+        with handle_lock:
+            if current_pub is not None:
+                try:
+                    stamp = rospy.get_rostime()
+
+                    if getattr(current_req, 'update_frame', False):
+                        current_pub, current_msg = get_publisher_and_message(current_req, stamp)
+
+                    current_msg.header.stamp = stamp
+                    current_pub.publish(current_msg)
+
+                except Exception as e:
+                    rospy.logwarn_throttle(10, str(e))
+
+        r.sleep()
+
+
+start_loop()
diff --git a/clever/src/util.py b/clever/src/util.py
new file mode 100644
index 00000000..eafa72fd
--- /dev/null
+++ b/clever/src/util.py
@@ -0,0 +1,28 @@
+from geometry_msgs.msg import Quaternion, Vector3, Point
+import tf.transformations as t
+
+
+def orientation_from_quaternion(q):
+    return Quaternion(*q)
+
+
+def orientation_from_euler(roll, pitch, yaw):
+    q = t.quaternion_from_euler(roll, pitch, yaw)
+    return orientation_from_quaternion(q)
+
+
+def quaternion_from_orientation(o):
+    return o.x, o.y, o.z, o.w
+
+
+def euler_from_orientation(o):
+    q = quaternion_from_orientation(o)
+    return t.euler_from_quaternion(q)
+
+
+def vector3_from_point(p):
+    return Vector3(p.x, p.y, p.z)
+
+
+def point_from_vector3(v):
+    return Point(v.x, v.y, v.z)
diff --git a/clever/srv/SetAttitude.srv b/clever/srv/SetAttitude.srv
new file mode 100644
index 00000000..9c41fff9
--- /dev/null
+++ b/clever/srv/SetAttitude.srv
@@ -0,0 +1,9 @@
+float32 pitch
+float32 roll
+float32 yaw
+float32 thrust
+string frame_id
+bool update_frame
+---
+bool success
+string message
diff --git a/clever/srv/SetAttitudeYawRate.srv b/clever/srv/SetAttitudeYawRate.srv
new file mode 100644
index 00000000..80e68a6a
--- /dev/null
+++ b/clever/srv/SetAttitudeYawRate.srv
@@ -0,0 +1,7 @@
+float32 roll
+float32 pitch
+float32 yaw_rate
+float32 thrust
+---
+bool success
+string message
diff --git a/clever/srv/SetPosition.srv b/clever/srv/SetPosition.srv
new file mode 100644
index 00000000..53e375ed
--- /dev/null
+++ b/clever/srv/SetPosition.srv
@@ -0,0 +1,9 @@
+float32 x
+float32 y
+float32 z
+float32 yaw
+string frame_id
+bool update_frame
+---
+bool success
+string message
diff --git a/clever/srv/SetPositionGlobal.srv b/clever/srv/SetPositionGlobal.srv
new file mode 100644
index 00000000..4bbdfc95
--- /dev/null
+++ b/clever/srv/SetPositionGlobal.srv
@@ -0,0 +1,9 @@
+float32 lat
+float32 lon
+float32 z
+float32 yaw
+string frame_id
+bool update_frame
+---
+bool success
+string message
diff --git a/clever/srv/SetPositionGlobalYawRate.srv b/clever/srv/SetPositionGlobalYawRate.srv
new file mode 100644
index 00000000..a7959e04
--- /dev/null
+++ b/clever/srv/SetPositionGlobalYawRate.srv
@@ -0,0 +1,9 @@
+float32 lat
+float32 lon
+float32 z
+float32 yaw_rate
+string frame_id
+bool update_frame
+---
+bool success
+string message
diff --git a/clever/srv/SetPositionYawRate.srv b/clever/srv/SetPositionYawRate.srv
new file mode 100644
index 00000000..76feda1f
--- /dev/null
+++ b/clever/srv/SetPositionYawRate.srv
@@ -0,0 +1,9 @@
+float32 x
+float32 y
+float32 z
+float32 yaw_rate
+string frame_id
+bool update_frame
+---
+bool success
+string message
diff --git a/clever/srv/SetRates.srv b/clever/srv/SetRates.srv
new file mode 100644
index 00000000..7e7c1cb1
--- /dev/null
+++ b/clever/srv/SetRates.srv
@@ -0,0 +1,7 @@
+float32 pitch_rate
+float32 roll_rate
+float32 yaw_rate
+float32 thrust
+---
+bool success
+string message
diff --git a/clever/srv/SetRatesYaw.srv b/clever/srv/SetRatesYaw.srv
new file mode 100644
index 00000000..fb9dbcc3
--- /dev/null
+++ b/clever/srv/SetRatesYaw.srv
@@ -0,0 +1,9 @@
+float32 pitch_rate
+float32 roll_rate
+float32 yaw
+float32 thrust
+string frame_id
+bool update_frame
+---
+bool success
+string message
diff --git a/clever/srv/SetVelocity.srv b/clever/srv/SetVelocity.srv
new file mode 100644
index 00000000..88efaaa2
--- /dev/null
+++ b/clever/srv/SetVelocity.srv
@@ -0,0 +1,9 @@
+float32 vx
+float32 vy
+float32 vz
+float32 yaw
+string frame_id
+bool update_frame
+---
+bool success
+string message
diff --git a/clever/srv/SetVelocityYawRate.srv b/clever/srv/SetVelocityYawRate.srv
new file mode 100644
index 00000000..965bdf68
--- /dev/null
+++ b/clever/srv/SetVelocityYawRate.srv
@@ -0,0 +1,9 @@
+float32 vx
+float32 vy
+float32 vz
+float32 yaw_rate
+string frame_id
+bool update_frame
+---
+bool success
+string message