14 files changed, 1395 insertions, 0 deletions

diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h
new file mode 100755
index 000000000..c7271c157
--- /dev/null
+++ b/src/platforms/ros/eigen_math.h
@@ -0,0 +1,19 @@
+/*
+ * eigen_math.h
+ *
+ *  Created on: Aug 25, 2014
+ *      Author: roman
+ */
+
+#ifndef EIGEN_MATH_H_
+#define EIGEN_MATH_H_
+
+
+struct eigen_matrix_instance {
+	int numRows;
+	int numCols;
+	float *pData;
+};
+
+
+#endif /* EIGEN_MATH_H_ */
diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp
new file mode 100644
index 000000000..6fad681c9
--- /dev/null
+++ b/src/platforms/ros/geo.cpp
@@ -0,0 +1,173 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 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 geo.c
+ *
+ * Geo / math functions to perform geodesic calculations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <geo/geo.h>
+#include <px4.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdbool.h>
+#include <string.h>
+#include <float.h>
+
+__EXPORT float _wrap_pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing >= M_PI_F) {
+		bearing -= M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+
+	while (bearing < -M_PI_F) {
+		bearing += M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_2pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing >= M_TWOPI_F) {
+		bearing -= M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+
+	while (bearing < 0.0f) {
+		bearing += M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_180(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing >= 180.0f) {
+		bearing -= 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+
+	while (bearing < -180.0f) {
+		bearing += 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_360(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing >= 360.0f) {
+		bearing -= 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+
+	while (bearing < 0.0f) {
+		bearing += 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md
new file mode 100644
index 000000000..aafc647ff
--- /dev/null
+++ b/src/platforms/ros/nodes/README.md
@@ -0,0 +1,22 @@
+# PX4 Nodes
+
+This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in
+ROS. They act as a bridge between the PX4 specific topics and the ROS topics.
+
+## Joystick Input
+
+You will need to install the ros joystick packages
+See http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick
+
+### Arch Linux
+```sh
+yaourt -Sy ros-indigo-joystick-drivers --noconfirm
+```
+check joystick
+```sh
+ls /dev/input/
+ls -l /dev/input/js0
+```
+(replace 0 by the number you find with the first command)
+
+make sure the joystick is accessible by the `input` group and that your user is in the `input` group
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
new file mode 100644
index 000000000..bcee0b479
--- /dev/null
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 att_estimator.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+
+#include "attitude_estimator.h"
+
+#include <px4/vehicle_attitude.h>
+#include <mathlib/mathlib.h>
+#include <platforms/px4_defines.h>
+
+AttitudeEstimator::AttitudeEstimator() :
+	_n(),
+	// _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
+	_sub_imu(_n.subscribe("/px4_multicopter/imu", 1, &AttitudeEstimator::ImuCallback, this)),
+	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
+{
+}
+
+void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
+{
+	px4::vehicle_attitude msg_v_att;
+
+	/* Fill px4 attitude topic with contents from modelstates topic */
+
+	/* Convert quaternion to rotation matrix */
+	math::Quaternion quat;
+	//XXX: search for ardrone or other (other than 'plane') vehicle here
+	int index = 1;
+	quat(0) = (float)msg->pose[index].orientation.w;
+	quat(1) = (float)msg->pose[index].orientation.x;
+	quat(2) = (float) - msg->pose[index].orientation.y;
+	quat(3) = (float) - msg->pose[index].orientation.z;
+
+	msg_v_att.q[0] = quat(0);
+	msg_v_att.q[1] = quat(1);
+	msg_v_att.q[2] = quat(2);
+	msg_v_att.q[3] = quat(3);
+
+	math::Matrix<3, 3> rot = quat.to_dcm();
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			PX4_R(msg_v_att.R, i, j) = rot(i, j);
+		}
+	}
+
+	msg_v_att.R_valid = true;
+
+	math::Vector<3> euler = rot.to_euler();
+	msg_v_att.roll = euler(0);
+	msg_v_att.pitch = euler(1);
+	msg_v_att.yaw = euler(2);
+
+	//XXX this is in inertial frame
+	// msg_v_att.rollspeed = (float)msg->twist[index].angular.x;
+	// msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y;
+	// msg_v_att.yawspeed = -(float)msg->twist[index].angular.z;
+
+	_vehicle_attitude_pub.publish(msg_v_att);
+}
+
+void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg)
+{
+	px4::vehicle_attitude msg_v_att;
+
+	/* Fill px4 attitude topic with contents from modelstates topic */
+
+	/* Convert quaternion to rotation matrix */
+	math::Quaternion quat;
+	//XXX: search for ardrone or other (other than 'plane') vehicle here
+	int index = 1;
+	quat(0) = (float)msg->orientation.w;
+	quat(1) = (float)msg->orientation.x;
+	quat(2) = (float) - msg->orientation.y;
+	quat(3) = (float) - msg->orientation.z;
+
+	msg_v_att.q[0] = quat(0);
+	msg_v_att.q[1] = quat(1);
+	msg_v_att.q[2] = quat(2);
+	msg_v_att.q[3] = quat(3);
+
+	math::Matrix<3, 3> rot = quat.to_dcm();
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			PX4_R(msg_v_att.R, i, j) = rot(i, j);
+		}
+	}
+
+	msg_v_att.R_valid = true;
+
+	math::Vector<3> euler = rot.to_euler();
+	msg_v_att.roll = euler(0);
+	msg_v_att.pitch = euler(1);
+	msg_v_att.yaw = euler(2);
+
+	msg_v_att.rollspeed = (float)msg->angular_velocity.x;
+	msg_v_att.pitchspeed = -(float)msg->angular_velocity.y;
+	msg_v_att.yawspeed = -(float)msg->angular_velocity.z;
+
+	_vehicle_attitude_pub.publish(msg_v_att);
+}
+
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "attitude_estimator");
+	AttitudeEstimator m;
+
+	ros::spin();
+
+	return 0;
+}
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
new file mode 100644
index 000000000..f760a39d8
--- /dev/null
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 att_estimator.h
+ * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <gazebo_msgs/ModelStates.h>
+#include <sensor_msgs/Imu.h>
+
+class AttitudeEstimator
+{
+public:
+	AttitudeEstimator();
+
+	~AttitudeEstimator() {}
+
+protected:
+	void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg);
+	void ImuCallback(const sensor_msgs::ImuConstPtr &msg);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _sub_modelstates;
+	ros::Subscriber _sub_imu;
+	ros::Publisher _vehicle_attitude_pub;
+
+
+};
diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
new file mode 100644
index 000000000..b9fc296f9
--- /dev/null
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 commander.cpp
+ * Dummy commander node that publishes the various status topics
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "commander.h"
+
+#include <px4/manual_control_setpoint.h>
+#include <px4/vehicle_control_mode.h>
+#include <px4/vehicle_status.h>
+#include <platforms/px4_middleware.h>
+
+Commander::Commander() :
+	_n(),
+	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, 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()
+{
+}
+
+void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
+{
+	px4::vehicle_control_mode msg_vehicle_control_mode;
+	px4::vehicle_status msg_vehicle_status;
+
+	/* fill vehicle control mode */
+	//XXX hardcoded
+	msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+	msg_vehicle_control_mode.flag_control_manual_enabled = true;
+	msg_vehicle_control_mode.flag_control_rates_enabled = true;
+	msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+
+	/* fill actuator armed */
+	float arm_th = 0.95;
+	_msg_actuator_armed.timestamp = px4::get_time_micros();
+
+	if (_msg_actuator_armed.armed) {
+		/* Check for disarm */
+		if (msg->r < -arm_th && msg->z < (1 - arm_th)) {
+			_msg_actuator_armed.armed = false;
+		}
+
+	} else {
+		/* Check for arm */
+		if (msg->r > arm_th && msg->z < (1 - arm_th)) {
+			_msg_actuator_armed.armed = true;
+		}
+	}
+
+	/* fill vehicle status */
+	//XXX hardcoded
+	msg_vehicle_status.timestamp = px4::get_time_micros();
+	msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL;
+	msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
+	msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
+	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;
+
+	_vehicle_control_mode_pub.publish(msg_vehicle_control_mode);
+	_actuator_armed_pub.publish(_msg_actuator_armed);
+	_vehicle_status_pub.publish(msg_vehicle_status);
+
+	/* Fill parameter update */
+	if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) {
+		_msg_parameter_update.timestamp = px4::get_time_micros();
+		_parameter_update_pub.publish(_msg_parameter_update);
+	}
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "commander");
+	Commander m;
+	ros::spin();
+	return 0;
+}
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
new file mode 100644
index 000000000..bd4092b3a
--- /dev/null
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 commander.h
+ * Dummy commander node that publishes the various status topics
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
+#include <px4/parameter_update.h>
+#include <px4/actuator_armed.h>
+
+class Commander
+{
+public:
+	Commander();
+
+	~Commander() {}
+
+protected:
+	/**
+	 * Based on manual control input the status will be set
+	 */
+	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _man_ctrl_sp_sub;
+	ros::Publisher _vehicle_control_mode_pub;
+	ros::Publisher _actuator_armed_pub;
+	ros::Publisher _vehicle_status_pub;
+	ros::Publisher _parameter_update_pub;
+
+	px4::parameter_update _msg_parameter_update;
+	px4::actuator_armed _msg_actuator_armed;
+
+};
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
new file mode 100644
index 000000000..688df50e0
--- /dev/null
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 manual_input.cpp
+ * Reads from the ros joystick topic and publishes to the px4 manual control input topic.
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "manual_input.h"
+
+#include <px4/manual_control_setpoint.h>
+#include <platforms/px4_middleware.h>
+
+ManualInput::ManualInput() :
+	_n(),
+	_joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)),
+	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1))
+{
+	double dz_default = 0.2;
+	/* Load parameters, default values work for Microsoft XBox Controller */
+	_n.param("map_x", _param_axes_map[0], 4);
+	_n.param("scale_x", _param_axes_scale[0], 1.0);
+	_n.param("off_x", _param_axes_offset[0], 0.0);
+	_n.param("dz_x", _param_axes_dz[0], dz_default);
+
+	_n.param("map_y", _param_axes_map[1], 3);
+	_n.param("scale_y", _param_axes_scale[1], -1.0);
+	_n.param("off_y", _param_axes_offset[1], 0.0);
+	_n.param("dz_y", _param_axes_dz[1], dz_default);
+
+	_n.param("map_z", _param_axes_map[2], 2);
+	_n.param("scale_z", _param_axes_scale[2], -0.5);
+	_n.param("off_z", _param_axes_offset[2], -1.0);
+	_n.param("dz_z", _param_axes_dz[2], dz_default);
+
+	_n.param("map_r", _param_axes_map[3], 0);
+	_n.param("scale_r", _param_axes_scale[3], -1.0);
+	_n.param("off_r", _param_axes_offset[3], 0.0);
+	_n.param("dz_r", _param_axes_dz[3], dz_default);
+
+}
+
+void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg)
+{
+	px4::manual_control_setpoint msg_out;
+
+	/* Fill px4 manual control setpoint topic with contents from ros joystick */
+	/* Map sticks to x, y, z, r */
+	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x);
+	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y);
+	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z);
+	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r);
+	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
+
+	/* Map buttons to switches */
+	//XXX todo
+	/* for now just publish switches in position for manual flight */
+	msg_out.mode_switch = 0;
+	msg_out.return_switch = 0;
+	msg_out.posctl_switch = 0;
+	msg_out.loiter_switch = 0;
+	msg_out.acro_switch = 0;
+	msg_out.offboard_switch = 0;
+
+	msg_out.timestamp = px4::get_time_micros();
+
+	_man_ctrl_sp_pub.publish(msg_out);
+}
+
+void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset,
+			  double deadzone, float &out)
+{
+	double val = msg->axes[map_index];
+
+	if (val + offset > deadzone || val + offset < -deadzone) {
+		out = (float)((val + offset)) * scale;
+	}
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "manual_input");
+	ManualInput m;
+	ros::spin();
+	return 0;
+}
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
new file mode 100644
index 000000000..93e0abe64
--- /dev/null
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 manual_input.h
+ * Reads from the ros joystick topic and publishes to the px4 manual control setpoint topic.
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <sensor_msgs/Joy.h>
+
+class ManualInput
+{
+public:
+	ManualInput();
+
+	~ManualInput() {}
+
+protected:
+	/**
+	 * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic
+	 */
+	void JoyCallback(const sensor_msgs::JoyConstPtr &msg);
+
+	/**
+	 * Helper function to map and scale joystick input
+	 */
+	void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone,
+		     float &out);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _joy_sub;
+	ros::Publisher _man_ctrl_sp_pub;
+
+	/* Parameters */
+	int _param_axes_map[4];
+	double _param_axes_scale[4];
+	double _param_axes_offset[4];
+	double _param_axes_dz[4];
+
+
+};
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
new file mode 100644
index 000000000..54f5fa78b
--- /dev/null
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 mc_mixer.cpp
+ * Dummy multicopter mixer for euroc simulator (gazebo)
+ *
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+#include <ros/ros.h>
+#include <px4.h>
+#include <lib/mathlib/mathlib.h>
+#include <mav_msgs/MotorSpeed.h>
+#include <string>
+
+class MultirotorMixer
+{
+public:
+
+	MultirotorMixer();
+
+	struct Rotor {
+		float roll_scale;
+		float pitch_scale;
+		float yaw_scale;
+	};
+
+	void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg);
+	void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg);
+
+private:
+
+	ros::NodeHandle _n;
+	ros::Subscriber _sub;
+	ros::Publisher _pub;
+
+	const Rotor *_rotors;
+
+	unsigned _rotor_count;
+
+	struct {
+		float control[8];
+	} inputs;
+
+	struct  {
+		float control[8];
+	} outputs;
+
+	bool _armed;
+	ros::Subscriber _sub_actuator_armed;
+
+	void mix();
+};
+
+const MultirotorMixer::Rotor _config_x[] = {
+	{ -0.707107,  0.707107,  1.00 },
+	{  0.707107, -0.707107,  1.00 },
+	{  0.707107,  0.707107, -1.00 },
+	{ -0.707107, -0.707107, -1.00 },
+};
+
+const MultirotorMixer::Rotor _config_quad_plus[] = {
+	{ -1.000000,  0.000000,  1.00 },
+	{  1.000000,  0.000000,  1.00 },
+	{  0.000000,  1.000000, -1.00 },
+	{ -0.000000, -1.000000, -1.00 },
+};
+
+const MultirotorMixer::Rotor _config_quad_plus_euroc[] = {
+	{  0.000000,  1.000000,  1.00 },
+	{ -0.000000, -1.000000,  1.00 },
+	{  1.000000,  0.000000, -1.00 },
+	{ -1.000000,  0.000000, -1.00 },
+};
+const MultirotorMixer::Rotor _config_quad_wide[] = {
+	{ -0.927184,  0.374607,  1.000000 },
+	{  0.777146, -0.629320,  1.000000 },
+	{  0.927184,  0.374607, -1.000000 },
+	{ -0.777146, -0.629320, -1.000000 },
+};
+const MultirotorMixer::Rotor _config_quad_iris[] = {
+	{ -0.876559,  0.481295,  1.000000 },
+	{  0.826590, -0.562805,  1.000000 },
+	{  0.876559,  0.481295, -1.000000 },
+	{ -0.826590, -0.562805, -1.000000 },
+};
+
+const MultirotorMixer::Rotor *_config_index[5] = {
+	&_config_x[0],
+	&_config_quad_plus[0],
+	&_config_quad_plus_euroc[0],
+	&_config_quad_wide[0],
+	&_config_quad_iris[0]
+};
+
+MultirotorMixer::MultirotorMixer():
+	_n(),
+	_rotor_count(4),
+	_rotors(_config_index[0])
+{
+	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this);
+	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10);
+
+	if (!_n.hasParam("motor_scaling_radps")) {
+		_n.setParam("motor_scaling_radps", 150.0);
+	}
+
+	if (!_n.hasParam("motor_offset_radps")) {
+		_n.setParam("motor_offset_radps", 600.0);
+	}
+
+	if (!_n.hasParam("mixer")) {
+		_n.setParam("mixer", "x");
+	}
+
+	_sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this);
+}
+
+void MultirotorMixer::mix()
+{
+	float roll = math::constrain(inputs.control[0], -1.0f, 1.0f);
+	float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f);
+	float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f);
+	float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f);
+	float min_out = 0.0f;
+	float max_out = 0.0f;
+
+	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		float out = roll * _rotors[i].roll_scale
+			    + pitch * _rotors[i].pitch_scale + thrust;
+
+		/* limit yaw if it causes outputs clipping */
+		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
+			yaw = -out / _rotors[i].yaw_scale;
+		}
+
+		/* calculate min and max output values */
+		if (out < min_out) {
+			min_out = out;
+		}
+
+		if (out > max_out) {
+			max_out = out;
+		}
+
+		outputs.control[i] = out;
+	}
+
+	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
+	if (min_out < 0.0f) {
+		float scale_in = thrust / (thrust - min_out);
+
+		/* mix again with adjusted controls */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs.control[i] = scale_in
+					     * (roll * _rotors[i].roll_scale
+						+ pitch * _rotors[i].pitch_scale) + thrust;
+		}
+
+	} else {
+		/* roll/pitch mixed without limiting, add yaw control */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs.control[i] += yaw * _rotors[i].yaw_scale;
+		}
+	}
+
+	/* scale down all outputs if some outputs are too large, reduce total thrust */
+	float scale_out;
+
+	if (max_out > 1.0f) {
+		scale_out = 1.0f / max_out;
+
+	} else {
+		scale_out = 1.0f;
+	}
+
+	/* scale outputs to range _idle_speed..1, and do final limiting */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f);
+	}
+}
+
+void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
+{
+	// read message
+	for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) {
+		inputs.control[i] = msg.control[i];
+	}
+
+	// switch mixer if necessary
+	std::string mixer_name;
+	_n.getParamCached("mixer", mixer_name);
+	if (mixer_name == "x") {
+		_rotors = _config_index[0];
+		ROS_WARN("using x");
+	} else if (mixer_name == "+") {
+		_rotors = _config_index[1];
+	} else if (mixer_name == "e") {
+		_rotors = _config_index[2];
+	} else if (mixer_name == "w") {
+		_rotors = _config_index[3];
+		ROS_WARN("using w");
+	} else if (mixer_name == "i") {
+		_rotors = _config_index[4];
+		ROS_WARN("using i");
+	}
+	ROS_WARN("mixer_name %s", mixer_name.c_str());
+
+	// mix
+	mix();
+
+	// publish message
+	mav_msgs::MotorSpeed rotor_vel_msg;
+	double scaling;
+	double offset;
+	_n.getParamCached("motor_scaling_radps", scaling);
+	_n.getParamCached("motor_offset_radps", offset);
+
+	if (_armed) {
+		for (int i = 0; i < _rotor_count; i++) {
+			rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset);
+		}
+
+	} else {
+		for (int i = 0; i < _rotor_count; i++) {
+			rotor_vel_msg.motor_speed.push_back(0.0);
+		}
+	}
+
+	_pub.publish(rotor_vel_msg);
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "mc_mixer");
+	MultirotorMixer mixer;
+	ros::spin();
+
+	return 0;
+}
+
+void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg)
+{
+	_armed = msg.armed;
+}
diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp
new file mode 100755
index 000000000..a71801397
--- /dev/null
+++ b/src/platforms/ros/perf_counter.cpp
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 - 2014 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 perf_counter.cpp
+ *
+ * Empty function calls for ros compatibility
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+#include <stdlib.h>
+#include <stdio.h>
+#include <systemlib/perf_counter.h>
+
+
+
+perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name)
+{
+	return NULL;
+}
+
+/**
+ * Free a counter.
+ *
+ * @param handle		The performance counter's handle.
+ */
+void perf_free(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_count(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_begin(perf_counter_t handle)
+{
+
+}
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_end(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_cancel(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Reset a performance counter.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_reset(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to stdout
+ *
+ * @param handle		The counter to print.
+ */
+void		perf_print_counter(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to a fd.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ * @param handle		The counter to print.
+ */
+void		perf_print_counter_fd(int fd, perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print all of the performance counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+void		perf_print_all(int fd)
+{
+
+}
+
+/**
+ * Reset all of the performance counters.
+ */
+void		perf_reset_all(void)
+{
+
+}
+
+/**
+ * Return current event_count
+ *
+ * @param handle		The counter returned from perf_alloc.
+ * @return			event_count
+ */
+uint64_t	perf_event_count(perf_counter_t handle)
+{
+
+}
+
+
diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp
new file mode 100644
index 000000000..6ac3c76d3
--- /dev/null
+++ b/src/platforms/ros/px4_nodehandle.cpp
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 px4_nodehandle.cpp
+ *
+ * PX4 Middleware Wrapper Nodehandle
+ */
+#include <px4_nodehandle.h>
+
+namespace px4
+{
+}
+
diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp
new file mode 100644
index 000000000..f02dbe4c9
--- /dev/null
+++ b/src/platforms/ros/px4_publisher.cpp
@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 px4_publisher.cpp
+ *
+ * PX4 Middleware Wrapper for Publisher
+ */
+#include <px4_publisher.h>
+
+
diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp
new file mode 100644
index 000000000..854986a7f
--- /dev/null
+++ b/src/platforms/ros/px4_ros_impl.cpp
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 px4_ros_impl.cpp
+ *
+ * PX4 Middleware Wrapper ROS Implementation
+ */
+
+#include <px4.h>
+
+namespace px4
+{
+
+void init(int argc, char *argv[], const char *process_name)
+{
+	ros::init(argc, argv, process_name);
+}
+
+uint64_t get_time_micros()
+{
+	ros::Time time = ros::Time::now();
+	return time.sec * 1e6 + time.nsec / 1000;
+}
+
+}