fix headers and indentation

4 files changed, 354 insertions, 210 deletions

diff --git a/integrationtests/demo_tests/direct_arm_test.py b/integrationtests/demo_tests/direct_arm_test.py
index 569e0af7c..238f2d7e0 100755
--- a/integrationtests/demo_tests/direct_arm_test.py
+++ b/integrationtests/demo_tests/direct_arm_test.py
@@ -1,4 +1,40 @@
 #!/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
@@ -10,28 +46,28 @@ 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)
+    #
+    # 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()
+        # method to test
+        arm = ManualInput()
+        arm.arm()
 
-		self.assertEquals(self.actuatorStatus.armed, True, "not armed")
+        self.assertEquals(self.actuatorStatus.armed, True, "not armed")
 
 
-	
+    
 
 if __name__ == '__main__':
-	import rostest
-	rostest.rosrun(PKG, 'arm_test', ArmTest)
+    import rostest
+    rostest.rosrun(PKG, 'arm_test', ArmTest)
diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py
index 6b28e0a18..42667757b 100755
--- a/integrationtests/demo_tests/direct_offboard_posctl_test.py
+++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py
@@ -1,4 +1,40 @@
 #!/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
@@ -22,103 +58,103 @@ 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()
-	
+    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()
+    import rostest
+    rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest)
+    #unittest.main()
diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py
index 1d99b7e5a..1f5bf01fc 100644
--- a/integrationtests/demo_tests/flight_path_assertion.py
+++ b/integrationtests/demo_tests/flight_path_assertion.py
@@ -1,4 +1,40 @@
 #!/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
diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py
index eb9144bbc..55911bede 100755
--- a/integrationtests/demo_tests/manual_input.py
+++ b/integrationtests/demo_tests/manual_input.py
@@ -1,4 +1,40 @@
 #!/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
 
@@ -14,97 +50,97 @@ from std_msgs.msg import Header
 #
 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
+    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