Merge pull request #1793 from PX4/ros_mavlink

ROS SITL: offboard setpoints

CMakeLists.txt

@@ -3,6 +3,7 @@ project(px4)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 add_definitions(-D__PX4_ROS)
 add_definitions(-D__EXPORT=)
+add_definitions(-DMAVLINK_DIALECT=common)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -11,11 +12,14 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  geometry_msgs
   message_generation
   cmake_modules
   gazebo_msgs
   sensor_msgs
   mav_msgs
+  libmavconn
+  tf
 )
 find_package(Eigen REQUIRED)
 
@@ -74,6 +78,8 @@ add_message_files(
   position_setpoint_triplet.msg
   vehicle_local_position_setpoint.msg
   vehicle_global_velocity_setpoint.msg
+  offboard_control_mode.msg
+  vehicle_force_setpoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -109,7 +115,7 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS src/include
   LIBRARIES px4
-  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
+  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn
   DEPENDS system_lib
 )
 
@@ -128,6 +134,7 @@ include_directories(
   src/
   src/lib
   ${EIGEN_INCLUDE_DIRS}
+  integrationtests
 )
 
 ## generate multiplatform wrapper headers
@@ -231,15 +238,42 @@ target_link_libraries(mc_mixer
   px4
 )
 
-## Commander
+## Commander dummy
 add_executable(commander
 	src/platforms/ros/nodes/commander/commander.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp)
 target_link_libraries(commander
   ${catkin_LIBRARIES}
   px4
 )
 
+## Mavlink dummy
+add_executable(mavlink
+	src/platforms/ros/nodes/mavlink/mavlink.cpp)
+add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mavlink
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Offboard Position Setpoint Demo
+add_executable(demo_offboard_position_setpoints
+	src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp)
+add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(demo_offboard_position_setpoints
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Offboard Attitude Setpoint Demo
+add_executable(demo_offboard_attitude_setpoints
+	src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp)
+add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(demo_offboard_attitude_setpoints
+  ${catkin_LIBRARIES}
+  px4
+)
+
 #############
 ## Install ##
 #############
@@ -287,3 +321,11 @@ install(TARGETS ${PROJECT_NAME}
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(integrationtests/demo_tests/direct_tests.launch)
+  add_rostest(integrationtests/demo_tests/mavros_tests.launch)
+endif()
+
+

integrationtests/demo_tests/direct_arm_test.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+
+from px4.msg import actuator_armed
+from manual_input import ManualInput
+
+class ArmTest(unittest.TestCase):
+
+    #
+    # General callback functions used in tests
+    #
+    def actuator_armed_callback(self, data):
+        self.actuatorStatus = data
+    
+    #
+    # Test arming
+    #
+    def test_arm(self):
+        rospy.init_node('test_node', anonymous=True)
+        sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback)
+
+        # method to test
+        arm = ManualInput()
+        arm.arm()
+
+        self.assertEquals(self.actuatorStatus.armed, True, "not armed")
+
+
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'arm_test', ArmTest)

integrationtests/demo_tests/direct_offboard_posctl_test.py

@@ -0,0 +1,160 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_local_position
+from px4.msg import vehicle_control_mode
+from px4.msg import actuator_armed
+from px4.msg import position_setpoint_triplet
+from px4.msg import position_setpoint
+from sensor_msgs.msg import Joy
+from std_msgs.msg import Header
+
+from manual_input import ManualInput
+from flight_path_assertion import FlightPathAssertion
+
+
+class OffboardPosctlTest(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+        rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback)
+        self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10)
+        self.rate = rospy.Rate(10) # 10hz
+
+    def tearDown(self):
+        if (self.fpa):
+            self.fpa.stop()
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+
+
+    #
+    # Helper methods
+    #
+    def is_at_position(self, x, y, z, offset):
+        rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z))
+        desired = np.array((x, y, z))
+        pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z))
+        return linalg.norm(desired - pos) < offset
+
+    def reach_position(self, x, y, z, timeout):
+        # set a position setpoint
+        pos = position_setpoint()
+        pos.valid = True
+        pos.x = x
+        pos.y = y
+        pos.z = z
+        pos.position_valid = True
+        stp = position_setpoint_triplet()
+        stp.current = pos
+        self.pubSpt.publish(stp)
+
+        # does it reach the position in X seconds?
+        count = 0
+        while(count < timeout):
+            if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, "took too long to get to position")
+
+    #
+    # Test offboard POSCTL
+    #
+    def test_posctl(self):
+        manIn = ManualInput()
+
+        # arm and go into offboard
+        manIn.arm()
+        manIn.offboard()
+        self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set")
+        self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
+        self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set")
+
+        # prepare flight path assertion
+        positions = (
+            (0,0,0),
+            (2,2,-2),
+            (2,-2,-2),
+            (-2,-2,-2),
+            (2,2,-2))
+
+        self.fpa = FlightPathAssertion(positions, 1, 0)
+        self.fpa.start()
+
+        for i in range(0, len(positions)):
+            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120)
+            self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i)
+        
+        # does it hold the position for Y seconds?
+        positionHeld = True
+        count = 0
+        timeout = 50
+        while(count < timeout):
+            if(not self.is_at_position(2, 2, -2, 0.5)):
+                positionHeld = False
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count == timeout, "position could not be held")
+        self.fpa.stop()
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest)
+    #unittest.main()

integrationtests/demo_tests/direct_tests.launch

@@ -0,0 +1,18 @@
+<launch>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+    <arg name="enable_logging" default="false"/>
+    <arg name="enable_ground_truth" default="false"/>
+    <arg name="log_file" default="iris"/>
+
+    <include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+    </include>
+
+    <test test-name="direct_arm" pkg="px4" type="direct_arm_test.py" />
+    <test test-name="direct_offboard_posctl" pkg="px4" type="direct_offboard_posctl_test.py" />
+</launch>

integrationtests/demo_tests/flight_path_assertion.py

@@ -0,0 +1,171 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+import sys
+import rospy
+import threading
+
+from px4.msg import vehicle_local_position
+from gazebo_msgs.srv import SpawnModel
+from gazebo_msgs.srv import SetModelState
+from gazebo_msgs.srv import DeleteModel
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Twist
+
+from numpy import linalg
+import numpy as np
+import math
+
+#
+# Helper to test if vehicle stays in expected flight path.
+#
+class FlightPathAssertion(threading.Thread):
+
+    #
+    # Arguments
+    # - positions: tuple of tuples in the form (x, y, z, heading)
+    #
+    # TODO: yaw validation
+    # TODO: fail main test thread
+    #
+    def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2):
+        threading.Thread.__init__(self)
+        rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback)
+        self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
+        self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)
+        self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
+
+        self.positions = positions
+        self.tunnelRadius = tunnelRadius
+        self.yawOffset = yawOffset
+        self.hasPos = False
+        self.shouldStop = False
+        self.center = positions[0]
+        self.endOfSegment = False
+        self.failed = False
+
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def spawn_indicator(self):
+        self.deleteModel("indicator")
+        xml = "<?xml version='1.0'?><sdf version='1.4'><model name='indicator'><static>true</static><link name='link'><visual name='visual'><transparency>0.7</transparency><geometry><sphere><radius>%f</radius></sphere></geometry><material><ambient>1 0 0 0.5</ambient><diffuse>1 0 0 0.5</diffuse></material></visual></link></model></sdf>" % self.tunnelRadius
+        self.spawnModel("indicator", xml, "", Pose(), "")
+
+    def position_indicator(self):
+        state = SetModelState()
+        state.model_name = "indicator"
+        pose = Pose()
+        pose.position.x = self.center[0]
+        pose.position.y = (-1) * self.center[1]
+        pose.position.z = (-1) * self.center[2]
+        state.pose = pose
+        state.twist = Twist()
+        state.reference_frame = ""
+        self.setModelState(state)
+
+    def distance_to_line(self, a, b, pos):
+        v = b - a
+        w = pos - a
+
+        c1 = np.dot(w, v)
+        if c1 <= 0: # before a
+            self.center = a
+            return linalg.norm(pos - a)
+
+        c2 = np.dot(v, v)
+        if c2 <= c1: # after b
+            self.center = b
+            self.endOfSegment = True
+            return linalg.norm(pos - b)
+
+        x = c1 / c2
+        l = a + x * v
+        self.center = l
+        return linalg.norm(pos - l)
+    
+    def stop(self):
+        self.shouldStop = True
+
+    def run(self):
+        rate = rospy.Rate(10) # 10hz
+        self.spawn_indicator()
+
+        current = 0
+
+        while not self.shouldStop:
+            if (self.hasPos):
+                # calculate distance to line segment between first two points
+                # if distances > tunnelRadius
+                #   exit with error
+                # advance current pos if not on the line anymore or distance to next point < tunnelRadius
+                # exit if current pos is now the last position
+
+                self.position_indicator()
+
+                pos = np.array((self.localPosition.x,
+                    self.localPosition.y, 
+                    self.localPosition.z))
+                aPos = np.array((self.positions[current][0],
+                    self.positions[current][1], 
+                    self.positions[current][2]))
+                bPos = np.array((self.positions[current + 1][0],
+                    self.positions[current + 1][1], 
+                    self.positions[current + 1][2]))
+
+                dist = self.distance_to_line(aPos, bPos, pos)
+                bDist = linalg.norm(pos - bPos)
+
+                rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist))
+
+                if (dist > self.tunnelRadius):
+                    msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)
+                    rospy.logerr(msg)
+                    self.failed = True
+                    break
+
+                if (self.endOfSegment or bDist < self.tunnelRadius):
+                    rospy.loginfo("next segment")
+                    self.endOfSegment = False
+                    current = current + 1
+
+                if (current == len(self.positions) - 1):
+                    rospy.loginfo("no more positions")
+                    break
+
+            rate.sleep()

integrationtests/demo_tests/manual_input.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+import sys
+import rospy
+
+from px4.msg import manual_control_setpoint
+from mav_msgs.msg import CommandAttitudeThrust
+from std_msgs.msg import Header
+
+#
+# Manual input control helper
+#
+# Note: this is not the way to do it. ATM it fakes input to iris/command/attitude because else
+# the simulator does not instantiate our controller.
+#
+class ManualInput:
+
+    def __init__(self):
+        rospy.init_node('test_node', anonymous=True)
+        self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10)
+        self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10)
+
+    def arm(self):
+        rate = rospy.Rate(10) # 10hz
+
+        att = CommandAttitudeThrust()
+        att.header = Header()
+
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 3
+        pos.return_switch = 3
+        pos.posctl_switch = 3
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 3
+
+        count = 0
+        while not rospy.is_shutdown() and count < 10:
+            rospy.loginfo("zeroing")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            # Fake input to iris commander
+            self.pubAtt.publish(att)
+            rate.sleep()
+            count = count + 1 
+
+        pos.r = 1
+        count = 0
+        while not rospy.is_shutdown() and count < 10:
+            rospy.loginfo("arming")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+
+    def posctl(self):
+        rate = rospy.Rate(10) # 10hz
+
+        # triggers posctl
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 2
+        pos.return_switch = 3
+        pos.posctl_switch = 1
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 3
+
+        count = 0
+        while not rospy.is_shutdown() and count < 10:
+            rospy.loginfo("triggering posctl")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+
+    def offboard(self):
+        rate = rospy.Rate(10) # 10hz
+
+        # triggers posctl
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 3
+        pos.return_switch = 3
+        pos.posctl_switch = 3
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 1
+
+        count = 0
+        while not rospy.is_shutdown() and count < 10:
+            rospy.loginfo("triggering posctl")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+

integrationtests/demo_tests/mavros_offboard_posctl_test.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+import math
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_control_mode
+from std_msgs.msg import Header
+from geometry_msgs.msg import PoseStamped, Quaternion
+from tf.transformations import quaternion_from_euler
+
+class OffboardPosctlTest(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+        rospy.Subscriber("px4_multicopter/mavros/position/local", PoseStamped, self.position_callback)
+        self.pubSpt = rospy.Publisher('px4_multicopter/mavros/setpoint/local_position', PoseStamped, queue_size=10)
+        self.rate = rospy.Rate(10) # 10hz
+        self.hasPos = False
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+
+
+    #
+    # Helper methods
+    #
+    def is_at_position(self, x, y, z, offset):
+        if(not self.hasPos):
+            return False
+
+        rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z))
+        desired = np.array((x, y, z))
+        pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z))
+        return linalg.norm(desired - pos) < offset
+
+    def reach_position(self, x, y, z, timeout):
+        # set a position setpoint
+        pos = PoseStamped()
+        pos.header = Header() 
+        pos.header.frame_id = "base_footprint"
+        pos.header.stamp = rospy.Time.now()
+        pos.pose.position.x = x
+        pos.pose.position.y = y
+        pos.pose.position.z = z
+
+        # For demo purposes we will lock yaw/heading to north.
+        yaw_degrees = 0  # North
+        yaw = math.radians(yaw_degrees)
+        quaternion = quaternion_from_euler(0, 0, yaw)
+        pos.pose.orientation = Quaternion(*quaternion)
+
+        # does it reach the position in X seconds?
+        count = 0
+        while(count < timeout):
+            self.pubSpt.publish(pos)
+            
+            if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, "took too long to get to position")
+
+    #
+    # Test offboard POSCTL
+    #
+    def test_posctl(self):
+        # prepare flight path assertion
+        positions = (
+            (0,0,0),
+            (2,2,2),
+            (2,-2,2),
+            (-2,-2,2),
+            (2,2,2))
+
+        for i in range(0, len(positions)):
+            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120)
+        
+        # does it hold the position for Y seconds?
+        positionHeld = True
+        count = 0
+        timeout = 50
+        while(count < timeout):
+            if(not self.is_at_position(2, 2, 2, 0.5)):
+                positionHeld = False
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count == timeout, "position could not be held")
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest)
+    #unittest.main()

integrationtests/demo_tests/mavros_tests.launch

@@ -0,0 +1,18 @@
+<launch>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+    <arg name="enable_logging" default="false"/>
+    <arg name="enable_ground_truth" default="false"/>
+    <arg name="log_file" default="iris"/>
+
+    <include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+    </include>
+    <include file="$(find px4)/launch/mavros_sitl.launch" />
+
+    <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" />
+</launch>

launch/gazebo_ardrone_empty_world.launch

@@ -3,7 +3,7 @@
     <arg name="headless" default="false"/>
     <arg name="gui" default="true"/>
     <arg name="enable_logging" default="false"/>
-    <arg name="enable_ground_truth" default="true"/>
+    <arg name="enable_ground_truth" default="false"/>
     <arg name="log_file" default="ardrone"/>
 
     <include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch">

launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch

@@ -0,0 +1,10 @@
+
+<launch>
+
+<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" />
+<include file="$(find px4)/launch/mavros_sitl.launch" />
+<group ns="px4_multicopter">
+	<node pkg="px4" name="demo_offboard_attitude_setpoints" type="demo_offboard_attitude_setpoints"/>
+</group>
+
+</launch>

launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch

@@ -0,0 +1,10 @@
+
+<launch>
+
+<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" />
+<include file="$(find px4)/launch/mavros_sitl.launch" />
+<group ns="px4_multicopter">
+	<node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_position_setpoints"/>
+</group>
+
+</launch>

launch/mavros_sitl.launch

@@ -0,0 +1,23 @@
+<launch>
+	<!-- vim: set ft=xml noet : -->
+	<!-- example launch script for PX4 based FCU's -->
+
+	<group ns="px4_multicopter">
+		<arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
+		<arg name="gcs_url" default="" />
+		<arg name="tgt_system" default="1" />
+		<arg name="tgt_component" default="50" />
+
+		<param name="mavros/setpoint/attitude/listen_twist" type="bool" value="false" />
+
+		<include file="$(find mavros)/launch/node.launch">
+			<arg name="blacklist_yaml" value="$(find mavros)/launch/px4_blacklist.yaml" />
+			<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
+
+			<arg name="fcu_url" value="$(arg fcu_url)" />
+			<arg name="gcs_url" value="$(arg gcs_url)" />
+			<arg name="tgt_system" value="$(arg tgt_system)" />
+			<arg name="tgt_component" value="$(arg tgt_component)" />
+		</include>
+	</group>
+</launch>

launch/multicopter.launch

@@ -9,6 +9,7 @@
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
+	<node pkg="px4" name="mavlink" type="mavlink"/>
 	<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
 </group>
 

msg/offboard_control_mode.msg

@@ -0,0 +1,9 @@
+# Off-board control mode
+uint64 timestamp
+
+bool ignore_thrust
+bool ignore_attitude
+bool ignore_bodyrate
+bool ignore_position
+bool ignore_velocity
+bool ignore_acceleration_force

msg/vehicle_force_setpoint.msg

@@ -0,0 +1,8 @@
+# Definition of force (NED) setpoint uORB topic. Typically this can be used
+# by a position control app together with an attitude control app.
+
+
+float32 x		# in N NED
+float32 y		# in N NED
+float32 z		# in N NED
+float32 yaw		# right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw)

package.xml

@@ -44,10 +44,15 @@
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>eigen</build_depend>
+  <build_depend>libmavconn</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>rostest</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>eigen</run_depend>
+  <run_depend>libmavconn</run_depend>
+  <run_depend>tf</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/platforms/px4_includes.h

@@ -66,6 +66,8 @@
 #include <px4_vehicle_global_velocity_setpoint.h>
 #include <px4_vehicle_local_position.h>
 #include <px4_position_setpoint_triplet.h>
+#include <px4_offboard_control_mode.h>
+#include <px4_vehicle_force_setpoint.h>
 #endif
 
 #else
@@ -93,6 +95,8 @@
 #include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h>
 #include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h>
+#include <platforms/nuttx/px4_messages/px4_offboard_control_mode.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_force_setpoint.h>
 #endif
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>

src/platforms/ros/nodes/commander/commander.cpp

@@ -45,18 +45,25 @@
 Commander::Commander() :
 	_n(),
 	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
+	_offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, this)),
 	_vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)),
 	_actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
 	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
 	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
 	_msg_parameter_update(),
 	_msg_actuator_armed(),
-	_msg_vehicle_control_mode()
+	_msg_vehicle_control_mode(),
+	_msg_offboard_control_mode(),
+	_got_manual_control(false)
 {
+
+	/* Default to offboard control: when no joystick is connected offboard control should just work */
+
 }
 
 void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
 {
+	_got_manual_control = true;
 	px4::vehicle_status msg_vehicle_status;
 
 	/* fill vehicle control mode based on (faked) stick positions*/
@@ -101,12 +108,51 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	}
 }
 
+void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode)
+{
+	msg_vehicle_control_mode.flag_control_manual_enabled = false;
+	msg_vehicle_control_mode.flag_control_offboard_enabled = true;
+	msg_vehicle_control_mode.flag_control_auto_enabled = false;
+
+	msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate ||
+		!msg_offboard_control_mode.ignore_attitude ||
+		!msg_offboard_control_mode.ignore_position ||
+		!msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_acceleration_force;
+
+	msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude ||
+		!msg_offboard_control_mode.ignore_position ||
+		!msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_acceleration_force;
+
+
+	msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position;
+}
+
 void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 		px4::vehicle_status &msg_vehicle_status,
 		px4::vehicle_control_mode &msg_vehicle_control_mode) {
 	// XXX this is a minimal implementation. If more advanced functionalities are
 	// needed consider a full port of the commander
 
+
+	if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON)
+	{
+		SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode);
+		return;
+	}
+
+	msg_vehicle_control_mode.flag_control_offboard_enabled = false;
+
 	switch (msg->mode_switch) {
 		case px4::manual_control_setpoint::SWITCH_POS_NONE:
 		ROS_WARN("Joystick button mapping error, main mode not set");
@@ -152,6 +198,35 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 
 }
 
+void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg)
+{
+	_msg_offboard_control_mode = *msg;
+
+	/* Force system into offboard control mode */
+	if (!_got_manual_control) {
+		SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode);
+		
+		px4::vehicle_status msg_vehicle_status;
+		msg_vehicle_status.timestamp = px4::get_time_micros();
+		msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF;
+		msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
+		msg_vehicle_status.is_rotary_wing = true;
+		msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
+		
+
+		_msg_actuator_armed.armed = true;
+		_msg_actuator_armed.timestamp = px4::get_time_micros();
+
+		_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+		_msg_vehicle_control_mode.flag_armed = true;
+		
+
+		_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
+		_actuator_armed_pub.publish(_msg_actuator_armed);
+		_vehicle_status_pub.publish(msg_vehicle_status);
+	}
+}
+
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "commander");

src/platforms/ros/nodes/commander/commander.h

@@ -44,6 +44,7 @@
 #include <px4/vehicle_status.h>
 #include <px4/parameter_update.h>
 #include <px4/actuator_armed.h>
+#include <px4/offboard_control_mode.h>
 
 class Commander
 {
@@ -58,6 +59,11 @@ protected:
 	 */
 	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
 
+	/**
+	 * Stores the offboard control mode
+	 */
+	void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg);
+
 	/**
 	 * Set control mode flags based on stick positions (equiv to code in px4 commander)
 	 */
@@ -65,8 +71,15 @@ protected:
 			px4::vehicle_status &msg_vehicle_status,
 			px4::vehicle_control_mode &msg_vehicle_control_mode);
 
+	/**
+	 * Sets offboard controll flags in msg_vehicle_control_mode
+	 */
+	void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode);
+
 	ros::NodeHandle _n;
 	ros::Subscriber _man_ctrl_sp_sub;
+	ros::Subscriber _offboard_control_mode_sub;
 	ros::Publisher _vehicle_control_mode_pub;
 	ros::Publisher _actuator_armed_pub;
 	ros::Publisher _vehicle_status_pub;
@@ -75,5 +88,8 @@ protected:
 	px4::parameter_update _msg_parameter_update;
 	px4::actuator_armed _msg_actuator_armed;
 	px4::vehicle_control_mode _msg_vehicle_control_mode;
+	px4::offboard_control_mode _msg_offboard_control_mode;
+
+	bool _got_manual_control;
 
 };

src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file demo_offboard_position_Setpoints.cpp
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "demo_offboard_attitude_setpoints.h"
+
+#include <platforms/px4_middleware.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Float64.h>
+#include <math.h>
+#include <tf/transform_datatypes.h>
+
+DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
+	_n(),
+	_attitude_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/attitude", 1)),
+	_thrust_sp_pub(_n.advertise<std_msgs::Float64>("mavros/setpoint/att_throttle", 1))
+{
+}
+
+
+int DemoOffboardAttitudeSetpoints::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (ros::ok()) {
+		loop_rate.sleep();
+		ros::spinOnce();
+
+		/* Publish example offboard attitude setpoint */
+		geometry_msgs::PoseStamped pose;
+		tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0);
+		quaternionTFToMsg(q, pose.pose.orientation);
+
+		_attitude_sp_pub.publish(pose);
+
+		std_msgs::Float64 thrust;
+		thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump'
+		_thrust_sp_pub.publish(thrust);
+	}
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "demo_offboard_position_setpoints");
+	DemoOffboardAttitudeSetpoints d;
+	return d.main();
+}

src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h

@@ -1,21 +1,20 @@
 /****************************************************************************
  *
- *	 Copyright (C) 2008-2015 PX4 Development Team. All rights reserved.
- *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  *
  * 1. Redistributions of source code must retain the above copyright
- *		notice, this list of conditions and the following disclaimer.
+ *    notice, this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright
- *		notice, this list of conditions and the following disclaimer in
- *		the documentation and/or other materials provided with the
- *		distribution.
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
  * 3. Neither the name PX4 nor the names of its contributors may be
- *		used to endorse or promote products derived from this software
- *		without specific prior written permission.
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
@@ -33,41 +32,27 @@
  ****************************************************************************/
 
 /**
- * @file offboard_control_mode.h
- * Definition of the manual_control_setpoint uORB topic.
- */
+ * @file demo_offboard_attitude_Setpoints.h
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
 
-#ifndef TOPIC_OFFBOARD_CONTROL_MODE_H_
-#define TOPIC_OFFBOARD_CONTROL_MODE_H_
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
 
-#include <stdint.h>
-#include "../uORB.h"
+class DemoOffboardAttitudeSetpoints
+{
+public:
+	DemoOffboardAttitudeSetpoints();
 
-/**
- * Off-board control mode
- */
+	~DemoOffboardAttitudeSetpoints() {}
 
-/**
- * @addtogroup topics
- * @{
- */
+	int main();
 
-struct offboard_control_mode_s {
-	uint64_t timestamp;
-
-	bool ignore_thrust;
-	bool ignore_attitude;
-	bool ignore_bodyrate;
-	bool ignore_position;
-	bool ignore_velocity;
-	bool ignore_acceleration_force;
-
-}; /**< offboard control inputs */
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(offboard_control_mode);
-
-#endif
+protected:
+	ros::NodeHandle _n;
+	ros::Publisher _attitude_sp_pub;
+	ros::Publisher _thrust_sp_pub;
+};

src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file demo_offboard_position_Setpoints.cpp
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "demo_offboard_position_setpoints.h"
+
+#include <platforms/px4_middleware.h>
+#include <geometry_msgs/PoseStamped.h>
+
+DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() :
+	_n(),
+	_local_position_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/local_position", 1))
+{
+}
+
+
+int DemoOffboardPositionSetpoints::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (ros::ok()) {
+		loop_rate.sleep();
+		ros::spinOnce();
+
+		/* Publish example offboard position setpoint */
+		geometry_msgs::PoseStamped pose;
+		pose.pose.position.x = 0;
+		pose.pose.position.y = 0;
+		pose.pose.position.z = 1;
+		_local_position_sp_pub.publish(pose);
+	}
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "demo_offboard_position_setpoints");
+	DemoOffboardPositionSetpoints d;
+	return d.main();
+}

src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file demo_offboard_position_Setpoints.h
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
+
+class DemoOffboardPositionSetpoints
+{
+public:
+	DemoOffboardPositionSetpoints();
+
+	~DemoOffboardPositionSetpoints() {}
+
+	int main();
+
+protected:
+	ros::NodeHandle _n;
+	ros::Publisher _local_position_sp_pub;
+};

src/platforms/ros/nodes/manual_input/manual_input.cpp

@@ -76,7 +76,8 @@ ManualInput::ManualInput() :
 	_n.param("map_posctl", _param_buttons_map[2], 2);
 	_n.param("map_auto_mission", _param_buttons_map[3], 3);
 	_n.param("map_auto_loiter", _param_buttons_map[4], 4);
-	_n.param("map_auto_rtl", _param_buttons_map[5], 4);
+	_n.param("map_auto_rtl", _param_buttons_map[5], 5);
+	_n.param("map_offboard", _param_buttons_map[6], 6);
 
 	/* Default to manual */
 	_msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
@@ -84,6 +85,8 @@ ManualInput::ManualInput() :
 	_msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 	_msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 	_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
 
 }
 
@@ -120,7 +123,6 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do
 
 void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) {
 	msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
-	msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
 
 	if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) {
 		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
@@ -128,6 +130,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		return;
 
 	} else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) {
@@ -136,6 +139,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		return;
 
 	} else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) {
@@ -144,6 +148,15 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
 		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+	} else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) {
+		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
+		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
 		return;
 	}
 

src/platforms/ros/nodes/manual_input/manual_input.h

@@ -77,6 +77,8 @@ protected:
 		MAIN_STATE_AUTO_MISSION,
 		MAIN_STATE_AUTO_LOITER,
 		MAIN_STATE_AUTO_RTL,
+		// MAIN_STATE_ACRO,
+		MAIN_STATE_OFFBOARD,
 		MAIN_STATE_MAX
 	};
 

src/platforms/ros/nodes/mavlink/mavlink.cpp

@@ -0,0 +1,296 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink.cpp
+ * Dummy mavlink node that interfaces to a mavros node via UDP
+ * This simulates the onboard mavlink app to some degree. It should be possible to
+ * send offboard setpoints via mavros to the SITL setup the same way as on the real system
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "mavlink.h"
+
+#include <platforms/px4_middleware.h>
+
+using namespace px4;
+
+Mavlink::Mavlink() :
+	_n(),
+	_v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)),
+	_v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)),
+	_v_att_sp_pub(_n.advertise<vehicle_attitude_setpoint>("vehicle_attitude_setpoint", 1)),
+	_pos_sp_triplet_pub(_n.advertise<position_setpoint_triplet>("position_setpoint_triplet", 1)),
+	_offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1)),
+	_force_sp_pub(_n.advertise<vehicle_force_setpoint>("vehicle_force_setpoint", 1))
+{
+	_link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560");
+	_link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3));
+
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "mavlink");
+	Mavlink m;
+	ros::spin();
+	return 0;
+}
+
+void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
+{
+	mavlink_message_t msg_m;
+	mavlink_msg_attitude_quaternion_pack_chan(
+			_link->get_system_id(),
+			_link->get_component_id(),
+			_link->get_channel(),
+			&msg_m,
+			get_time_micros() / 1000,
+			msg->q[0],
+			msg->q[1],
+			msg->q[2],
+			msg->q[3],
+			msg->rollspeed,
+			msg->pitchspeed,
+			msg->yawspeed);
+	_link->send_message(&msg_m);
+}
+
+void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg)
+{
+	mavlink_message_t msg_m;
+	mavlink_msg_local_position_ned_pack_chan(
+			_link->get_system_id(),
+			_link->get_component_id(),
+			_link->get_channel(),
+			&msg_m,
+			get_time_micros() / 1000,
+			msg->x,
+			msg->y,
+			msg->z,
+			msg->vx,
+			msg->vy,
+			msg->vz);
+	_link->send_message(&msg_m);
+}
+
+void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
+	(void)sysid;
+	(void)compid;
+
+	switch(mmsg->msgid) {
+		case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+			handle_msg_set_attitude_target(mmsg);
+			break;
+		case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+			handle_msg_set_position_target_local_ned(mmsg);
+			break;
+		default:
+			break;
+	}
+
+}
+
+void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg)
+{
+	mavlink_set_attitude_target_t set_attitude_target;
+	mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target);
+
+	static offboard_control_mode offboard_control_mode;
+
+	/* set correct ignore flags for thrust field: copy from mavlink message */
+	offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
+
+	/*
+	 * if attitude or body rate have been used (not ignored) previously and this message only sends
+	 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
+	 * body rates to keep the controllers running
+	 */
+	bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
+	bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
+
+	if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) {
+		/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
+		offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate;
+		offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude;
+	} else {
+		offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
+		offboard_control_mode.ignore_attitude = ignore_attitude;
+	}
+	offboard_control_mode.ignore_position = true;
+	offboard_control_mode.ignore_velocity = true;
+	offboard_control_mode.ignore_acceleration_force = true;
+
+	offboard_control_mode.timestamp = get_time_micros();
+	_offboard_control_mode_pub.publish(offboard_control_mode);
+
+	static vehicle_attitude_setpoint att_sp = {};
+
+	/* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint
+	 * gets published only if in offboard mode. We leave that out for now.
+	 */
+
+	att_sp.timestamp = get_time_micros();
+	mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body,
+			&att_sp.yaw_body);
+	mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data());
+	att_sp.R_valid = true;
+
+	if (!offboard_control_mode.ignore_thrust) {
+		att_sp.thrust = set_attitude_target.thrust;
+	}
+
+	if (!offboard_control_mode.ignore_attitude) {
+		for (ssize_t i = 0; i < 4; i++) {
+			att_sp.q_d[i] = set_attitude_target.q[i];
+		}
+		att_sp.q_d_valid = true;
+	}
+
+	_v_att_sp_pub.publish(att_sp);
+
+
+	//XXX real mavlink publishes rate sp here
+
+}
+
+void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg)
+{
+
+	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
+	mavlink_msg_set_position_target_local_ned_decode(mmsg, &set_position_target_local_ned);
+
+	offboard_control_mode offboard_control_mode;
+	// memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
+
+	/* Only accept messages which are intended for this system */
+	// XXX removed for sitl, makes maybe sense to re-introduce at some point
+	// if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
+				// set_position_target_local_ned.target_system == 0) &&
+			// (mavlink_system.compid == set_position_target_local_ned.target_component ||
+			 // set_position_target_local_ned.target_component == 0)) {
+
+	/* convert mavlink type (local, NED) to uORB offboard control struct */
+	offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
+	offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
+	offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
+	bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+	/* yaw ignore flag mapps to ignore_attitude */
+	offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
+	/* yawrate ignore flag mapps to ignore_bodyrate */
+	offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
+
+
+
+	offboard_control_mode.timestamp = get_time_micros();
+	_offboard_control_mode_pub.publish(offboard_control_mode);
+
+	/* The real mavlink app has a ckeck at this location which makes sure that the position setpoint triplet
+	 * gets published only if in offboard mode. We leave that out for now.
+	 */
+	if (is_force_sp && offboard_control_mode.ignore_position &&
+			offboard_control_mode.ignore_velocity) {
+		/* The offboard setpoint is a force setpoint only, directly writing to the force
+		 * setpoint topic and not publishing the setpoint triplet topic */
+		vehicle_force_setpoint	force_sp;
+		force_sp.x = set_position_target_local_ned.afx;
+		force_sp.y = set_position_target_local_ned.afy;
+		force_sp.z = set_position_target_local_ned.afz;
+		//XXX: yaw
+
+		_force_sp_pub.publish(force_sp);
+	} else {
+		/* It's not a pure force setpoint: publish to setpoint triplet  topic */
+		position_setpoint_triplet pos_sp_triplet;
+		pos_sp_triplet.previous.valid = false;
+		pos_sp_triplet.next.valid = false;
+		pos_sp_triplet.current.valid = true;
+		pos_sp_triplet.current.type = position_setpoint::SETPOINT_TYPE_POSITION; //XXX support others
+
+		/* set the local pos values */
+		if (!offboard_control_mode.ignore_position) {
+			pos_sp_triplet.current.position_valid = true;
+			pos_sp_triplet.current.x = set_position_target_local_ned.x;
+			pos_sp_triplet.current.y = set_position_target_local_ned.y;
+			pos_sp_triplet.current.z = set_position_target_local_ned.z;
+		} else {
+			pos_sp_triplet.current.position_valid = false;
+		}
+
+		/* set the local vel values */
+		if (!offboard_control_mode.ignore_velocity) {
+			pos_sp_triplet.current.velocity_valid = true;
+			pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
+			pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
+			pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
+		} else {
+			pos_sp_triplet.current.velocity_valid = false;
+		}
+
+		/* set the local acceleration values if the setpoint type is 'local pos' and none
+		 * of the accelerations fields is set to 'ignore' */
+		if (!offboard_control_mode.ignore_acceleration_force) {
+			pos_sp_triplet.current.acceleration_valid = true;
+			pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
+			pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
+			pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
+			pos_sp_triplet.current.acceleration_is_force =
+				is_force_sp;
+
+		} else {
+			pos_sp_triplet.current.acceleration_valid = false;
+		}
+
+		/* set the yaw sp value */
+		if (!offboard_control_mode.ignore_attitude) {
+			pos_sp_triplet.current.yaw_valid = true;
+			pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
+
+		} else {
+			pos_sp_triplet.current.yaw_valid = false;
+		}
+
+		/* set the yawrate sp value */
+		if (!offboard_control_mode.ignore_bodyrate) {
+			pos_sp_triplet.current.yawspeed_valid = true;
+			pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
+
+		} else {
+			pos_sp_triplet.current.yawspeed_valid = false;
+		}
+		//XXX handle global pos setpoints (different MAV frames)
+
+		_pos_sp_triplet_pub.publish(pos_sp_triplet);
+	}
+}

src/platforms/ros/nodes/mavlink/mavlink.h

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink.h
+ * Dummy mavlink node that interfaces to a mavros node via UDP
+ * This simulates the onboard mavlink app to some degree. It should be possible to
+ * send offboard setpoints via mavros to the SITL setup the same way as on the real system
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <mavconn/interface.h>
+#include <px4/vehicle_attitude.h>
+#include <px4/vehicle_local_position.h>
+#include <px4/vehicle_attitude_setpoint.h>
+#include <px4/position_setpoint_triplet.h>
+#include <px4/vehicle_force_setpoint.h>
+#include <px4/offboard_control_mode.h>
+
+namespace px4
+{
+
+class Mavlink
+{
+public:
+	Mavlink();
+
+	~Mavlink() {}
+
+protected:
+
+	ros::NodeHandle _n;
+	mavconn::MAVConnInterface::Ptr _link;
+	ros::Subscriber _v_att_sub;
+	ros::Subscriber _v_local_pos_sub;
+	ros::Publisher _v_att_sp_pub;
+	ros::Publisher _pos_sp_triplet_pub;
+	ros::Publisher _offboard_control_mode_pub;
+	ros::Publisher _force_sp_pub;
+
+	/**
+	 *
+	 * Simulates output of attitude data from the FCU
+	 * Equivalent to the mavlink stream ATTITUDE_QUATERNION
+	 *
+	 * */
+	void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg);
+
+	/**
+	 *
+	 * Simulates output of local position data from the FCU
+	 * Equivalent to the mavlink stream LOCAL_POSITION_NED
+	 *
+	 * */
+	void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg);
+
+
+	/**
+	 *
+	 * Handle incoming mavlink messages ant publish them to ROS ("Mavlink Receiver")
+	 *
+	 * */
+	void handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid);
+
+	/**
+	 *
+	 * Handle SET_ATTITUDE_TARGET mavlink messages
+	 *
+	 * */
+	void handle_msg_set_attitude_target(const mavlink_message_t *mmsg);
+
+	/**
+	 *
+	 * Handle SET_POSITION_TARGET_LOCAL_NED mavlink messages
+	 *
+	 * */
+	void handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg);
+
+};
+
+}