class RosNode: def __init__(self): ''' Constructor ''' rospy.init_node('imu2tf', anonymous=True) rospy.Subscriber("imu", Imu, self.callbackIMU) self.br = tf.TransformBroadcaster() self.pose = PoseWithCovarianceStamped() self.measurment_last = Imu() self.imu_callback_counter = np.uint64(0) self.twist = Twist() self.duration = Duration() rospy.spin() def callbackIMU(self, msg): # rospy.loginfo("ServoCommand received!") avr_samples = 20.0 if (self.imu_callback_counter < avr_samples): if (self.imu_callback_counter == 0): self.linear_acceleration_offest = Vector3() self.linear_acceleration_offest.x = 0 self.linear_acceleration_offest.y = 0 self.linear_acceleration_offest.z = 0 self.measurment_last = copy.copy(msg) self.linear_acceleration_offest.x += msg.linear_acceleration.x / avr_samples self.linear_acceleration_offest.y += msg.linear_acceleration.y / avr_samples self.linear_acceleration_offest.z += msg.linear_acceleration.z / avr_samples else: self.duration = (msg.header.stamp - self.measurment_last.header.stamp) dt = self.duration.to_sec() self.twist.linear.x = self.twist.linear.x + ( msg.linear_acceleration.x - self.linear_acceleration_offest.x) * dt self.twist.linear.y = self.twist.linear.y + ( msg.linear_acceleration.y - self.linear_acceleration_offest.y) * dt self.twist.linear.z = self.twist.linear.z + ( msg.linear_acceleration.z - self.linear_acceleration_offest.z) * dt self.pose.pose.pose.position.x = self.pose.pose.pose.position.x + self.twist.linear.x * dt self.pose.pose.pose.position.y = self.pose.pose.pose.position.y + self.twist.linear.y * dt self.pose.pose.pose.position.z = self.pose.pose.pose.position.z + self.twist.linear.z * dt self.br.sendTransform( (self.pose.pose.pose.position.x, self.pose.pose.pose.position.y, self.pose.pose.pose.position.z), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "base", msg.header.frame_id) self.measurment_last = copy.copy(msg) self.imu_callback_counter = self.imu_callback_counter + 1
def test_Duration(self): from genpy.rostime import Duration # See #3667 as well as PEP 238 d = Duration(13, 500000000) to_sec = d.to_sec() self.assertEquals(Duration(to_sec / 2.), d / 2) # Test div self.assertEquals(Duration(4), Duration(8) / 2) self.assertEquals(Duration(4), Duration(8) / 2.) self.assertEquals(Duration(4), Duration(8) // 2) self.assertEquals(Duration(4), Duration(8) // 2.) self.assertEquals(Duration(4), Duration(9) // 2) self.assertEquals(Duration(4), Duration(9) // 2.) self.assertEquals(Duration(4, 2), Duration(8, 4) / 2) v = Duration(4, 2) - (Duration(8, 4) / 2.) self.assert_(abs(v.to_nsec()) < 100) self.assertEquals(Duration(4, 0), Duration(8, 4) // 2) self.assertEquals(Duration(4, 0), Duration(9, 5) // 2) v = Duration(4, 2) - (Duration(9, 5) // 2.) self.assert_(abs(v.to_nsec()) < 100)
def test_Duration(self): from genpy.rostime import Duration # See #3667 as well as PEP 238 d = Duration(13, 500000000) to_sec = d.to_sec() self.assertEquals(Duration(to_sec / 2.), d/2) # Test div self.assertEquals(Duration(4), Duration(8) / 2) self.assertEquals(Duration(4), Duration(8) / 2.) self.assertEquals(Duration(4), Duration(8) // 2) self.assertEquals(Duration(4), Duration(8) // 2.) self.assertEquals(Duration(4), Duration(9) // 2) self.assertEquals(Duration(4), Duration(9) // 2.) self.assertEquals(Duration(4, 2), Duration(8, 4) / 2) v = Duration(4, 2) - (Duration(8, 4) / 2.) self.assert_(abs(v.to_nsec()) < 100) self.assertEquals(Duration(4, 0), Duration(8, 4) // 2) self.assertEquals(Duration(4, 0), Duration(9, 5) // 2) v = Duration(4, 2) - (Duration(9, 5) // 2.) self.assert_(abs(v.to_nsec()) < 100)
def test_Duration(self): from genpy.rostime import Time, Duration self.test_TVal(TVal=Duration, test_neg=True) # test from_sec v = Duration(1000) self.assertEquals(v, Duration.from_sec(v.to_sec())) self.assertEquals(v, v.from_sec(v.to_sec())) v = Duration(0, 1000) self.assertEquals(v, Duration.from_sec(v.to_sec())) self.assertEquals(v, v.from_sec(v.to_sec())) # test neg v = -Duration(1, -1) self.assertEquals(-1, v.secs) self.assertEquals(1, v.nsecs) v = -Duration(-1, -1) self.assertEquals(1, v.secs) self.assertEquals(1, v.nsecs) v = -Duration(-1, 1) self.assertEquals(0, v.secs) self.assertEquals(999999999, v.nsecs) # test addition self.assertEquals(Duration(1, 0) + Time(1, 0), Time(2, 0)) failed = False try: v = Duration(1, 0) + 1 failed = True except: pass self.failIf(failed, "Duration + int must fail") v = Duration(1, 0) + Duration(1, 0) self.assertEquals(2, v.secs) self.assertEquals(0, v.nsecs) self.assertEquals(Duration(2, 0), v) v = Duration(-1, -1) + Duration(1, 1) self.assertEquals(0, v.secs) self.assertEquals(0, v.nsecs) self.assertEquals(Duration(), v) v = Duration(1) + Duration(0, 1000000000) self.assertEquals(2, v.secs) self.assertEquals(0, v.nsecs) self.assertEquals(Duration(2), v) v = Duration(100, 100) + Duration(300) self.assertEquals(Duration(400, 100), v) v = Duration(100, 100) + Duration(300, 300) self.assertEquals(Duration(400, 400), v) v = Duration(100, 100) + Duration(300, -101) self.assertEquals(Duration(399, 999999999), v) # test subtraction try: v = Duration(1, 0) - 1 failed = True except: pass self.failIf(failed, "Duration - non duration must fail") try: v = Duration(1, 0) - Time(1, 0) failed = True except: pass self.failIf(failed, "Duration - Time must fail") v = Duration(1, 0) - Duration(1, 0) self.assertEquals(Duration(), v) v = Duration(-1, -1) - Duration(1, 1) self.assertEquals(Duration(-3, 999999998), v) v = Duration(1) - Duration(0, 1000000000) self.assertEquals(Duration(), v) v = Duration(2) - Duration(0, 1000000000) self.assertEquals(Duration(1), v) v = Duration(100, 100) - Duration(300) self.assertEquals(Duration(-200, 100), v) v = Duration(100, 100) - Duration(300, 101) self.assertEquals(Duration(-201, 999999999), v) # test abs self.assertEquals(abs(Duration()), Duration()) self.assertEquals(abs(Duration(1)), Duration(1)) self.assertEquals(abs(Duration(-1)), Duration(1)) self.assertEquals(abs(Duration(0, -1)), Duration(0, 1)) self.assertEquals(abs(Duration(-1, -1)), Duration(1, 1)) self.assertEquals(abs(Duration(0, 1)), Duration(0, 1)) # Duration (float secs) vs. Duration(int, int) self.assertEquals(Duration.from_sec(0.5), Duration(0.5)) t = Duration(0.5) self.assert_(type(t.secs) == int) self.assertEquals(0, t.secs) self.assertEquals(500000000, t.nsecs) try: Duration(0.5, 0.5) self.fail("should have thrown value error") except ValueError: pass # Test mul self.assertEquals(Duration(4), Duration(2) * 2) self.assertEquals(Duration(4), Duration(2) * 2.) self.assertEquals(Duration(10), Duration(4) * 2.5) self.assertEquals(Duration(4, 8), Duration(2, 4) * 2) v = Duration(4, 8) - (Duration(2, 4) * 2.) self.assert_(abs(v.to_nsec()) < 100) v = Duration(5, 10) - (Duration(2, 4) * 2.5) self.assert_(abs(v.to_nsec()) < 100) # Test div self.assertEquals(Duration(4), Duration(8) / 2) self.assertEquals(Duration(4), Duration(8) / 2.) self.assertEquals(Duration(4), Duration(8) // 2) self.assertEquals(Duration(4), Duration(8) // 2.) self.assertEquals(Duration(4), Duration(9) // 2) self.assertEquals(Duration(4), Duration(9) // 2.) self.assertEquals(Duration(4, 2), Duration(8, 4) / 2) v = Duration(4, 2) - (Duration(8, 4) / 2.) self.assert_(abs(v.to_nsec()) < 100) self.assertEquals(Duration(4, 2), Duration(8, 4) // 2) self.assertEquals(Duration(4, 2), Duration(9, 5) // 2) v = Duration(4, 2) - (Duration(9, 5) // 2.) self.assert_(abs(v.to_nsec()) < 100)
def test_Duration(self): from genpy.rostime import Time, Duration self.test_TVal(TVal=Duration, test_neg=True) # test from_sec v = Duration(1000) self.assertEquals(v, Duration.from_sec(v.to_sec())) self.assertEquals(v, v.from_sec(v.to_sec())) v = Duration(0,1000) self.assertEquals(v, Duration.from_sec(v.to_sec())) self.assertEquals(v, v.from_sec(v.to_sec())) # test neg v = -Duration(1, -1) self.assertEquals(-1, v.secs) self.assertEquals(1, v.nsecs) v = -Duration(-1, -1) self.assertEquals(1, v.secs) self.assertEquals(1, v.nsecs) v = -Duration(-1, 1) self.assertEquals(0, v.secs) self.assertEquals(999999999, v.nsecs) # test addition self.assertEquals(Duration(1,0) + Time(1, 0), Time(2, 0)) failed = False try: v = Duration(1,0) + 1 failed = True except: pass self.failIf(failed, "Duration + int must fail") v = Duration(1,0) + Duration(1, 0) self.assertEquals(2, v.secs) self.assertEquals(0, v.nsecs) self.assertEquals(Duration(2, 0), v) v = Duration(-1,-1) + Duration(1, 1) self.assertEquals(0, v.secs) self.assertEquals(0, v.nsecs) self.assertEquals(Duration(), v) v = Duration(1) + Duration(0, 1000000000) self.assertEquals(2, v.secs) self.assertEquals(0, v.nsecs) self.assertEquals(Duration(2), v) v = Duration(100, 100) + Duration(300) self.assertEquals(Duration(400, 100), v) v = Duration(100, 100) + Duration(300, 300) self.assertEquals(Duration(400, 400), v) v = Duration(100, 100) + Duration(300, -101) self.assertEquals(Duration(399, 999999999), v) # test subtraction try: v = Duration(1,0) - 1 failed = True except: pass self.failIf(failed, "Duration - non duration must fail") try: v = Duration(1, 0) - Time(1,0) failed = True except: pass self.failIf(failed, "Duration - Time must fail") v = Duration(1,0) - Duration(1, 0) self.assertEquals(Duration(), v) v = Duration(-1,-1) - Duration(1, 1) self.assertEquals(Duration(-3, 999999998), v) v = Duration(1) - Duration(0, 1000000000) self.assertEquals(Duration(), v) v = Duration(2) - Duration(0, 1000000000) self.assertEquals(Duration(1), v) v = Duration(100, 100) - Duration(300) self.assertEquals(Duration(-200, 100), v) v = Duration(100, 100) - Duration(300, 101) self.assertEquals(Duration(-201, 999999999), v) # test abs self.assertEquals(abs(Duration()), Duration()) self.assertEquals(abs(Duration(1)), Duration(1)) self.assertEquals(abs(Duration(-1)), Duration(1)) self.assertEquals(abs(Duration(0,-1)), Duration(0,1)) self.assertEquals(abs(Duration(-1,-1)), Duration(1,1)) self.assertEquals(abs(Duration(0,1)), Duration(0,1)) # Duration (float secs) vs. Duration(int, int) self.assertEquals(Duration.from_sec(0.5), Duration(0.5)) t = Duration(0.5) self.assert_(type(t.secs) == int) self.assertEquals(0, t.secs) self.assertEquals(500000000, t.nsecs) try: Duration(0.5, 0.5) self.fail("should have thrown value error") except ValueError: pass # Test mul self.assertEquals(Duration(4), Duration(2) * 2) self.assertEquals(Duration(4), Duration(2) * 2.) self.assertEquals(Duration(4), 2 * Duration(2)) self.assertEquals(Duration(4), 2. * Duration(2)) self.assertEquals(Duration(10), Duration(4) * 2.5) self.assertEquals(Duration(4, 8), Duration(2, 4) * 2) v = Duration(4, 8) - (Duration(2, 4) * 2.) self.assert_(abs(v.to_nsec()) < 100) v = Duration(5, 10) - (Duration(2, 4) * 2.5) self.assert_(abs(v.to_nsec()) < 100) # Test div self.assertEquals(Duration(4), Duration(8) / 2) self.assertEquals(Duration(4), Duration(8) / 2.) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') self.assertEqual(Duration(4), Duration(8) // 2) self.assertEqual(Duration(4), Duration(8) // 2.) self.assertEqual(Duration(4), Duration(9) // 2) self.assertEqual(Duration(4), Duration(9) // 2.) self.assertEqual(len(w), 0) self.assertEquals(Duration(4, 2), Duration(8, 4) / 2) v = Duration(4, 2) - (Duration(8, 4) / 2.) self.assert_(abs(v.to_nsec()) < 100) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') self.assertEqual(Duration(4, 0), Duration(8, 4) // 2) self.assertEqual(Duration(4, 0), Duration(9, 5) // 2) v = Duration(4, 2) - (Duration(9, 5) // 2.) self.assertTrue(abs(v.to_nsec()) < 100) self.assertEqual(len(w), 0)