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()
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 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_Time(self):
from genpy.rostime import Time, Duration
self.test_TVal(TVal=Time, test_neg=False)
# #1600 Duration > Time should fail
failed = False
try:
v = Duration.from_sec(0.1) > Time.from_sec(0.5)
failed = True
except:
pass
self.failIf(failed, "should have failed to compare")
try:
v = Time.from_sec(0.4) > Duration.from_sec(0.1)
failed = True
except:
pass
self.failIf(failed, "should have failed to compare")
# TODO: sub
# neg time fails
try:
Time(-1)
failed = True
except:
pass
self.failIf(failed, "negative time not allowed")
try:
Time(1, -1000000001)
failed = True
except:
pass
self.failIf(failed, "negative time not allowed")
# test Time.now() is within 10 seconds of actual time (really generous)
import time
t = time.time()
v = Time.from_sec(t)
self.assertEquals(v.to_sec(), t)
# test from_sec()
self.assertEquals(Time.from_sec(0), Time())
self.assertEquals(Time.from_sec(1.), Time(1))
self.assertEquals(Time.from_sec(v.to_sec()), v)
self.assertEquals(v.from_sec(v.to_sec()), v)
# test to_time()
self.assertEquals(v.to_sec(), v.to_time())
# test addition
# - time + time fails
try:
v = Time(1, 0) + Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Time + Time must fail")
# - time + duration
v = Time(1, 0) + Duration(1, 0)
self.assertEquals(Time(2, 0), v)
v = Duration(1, 0) + Time(1, 0)
self.assertEquals(Time(2, 0), v)
v = Time(1, 1) + Duration(1, 1)
self.assertEquals(Time(2, 2), v)
v = Duration(1, 1) + Time(1, 1)
self.assertEquals(Time(2, 2), v)
v = Time(1) + Duration(0, 1000000000)
self.assertEquals(Time(2), v)
v = Duration(1) + Time(0, 1000000000)
self.assertEquals(Time(2), v)
v = Time(100, 100) + Duration(300)
self.assertEquals(Time(400, 100), v)
v = Duration(300) + Time(100, 100)
self.assertEquals(Time(400, 100), v)
v = Time(100, 100) + Duration(300, 300)
self.assertEquals(Time(400, 400), v)
v = Duration(300, 300) + Time(100, 100)
self.assertEquals(Time(400, 400), v)
v = Time(100, 100) + Duration(300, -101)
self.assertEquals(Time(399, 999999999), v)
v = Duration(300, -101) + Time(100, 100)
self.assertEquals(Time(399, 999999999), v)
# test subtraction
try:
v = Time(1, 0) - 1
failed = True
except:
pass
self.failIf(failed, "Time - non Duration must fail")
class Foob(object):
pass
try:
v = Time(1, 0) - Foob()
failed = True
except:
pass
self.failIf(failed, "Time - non TVal must fail")
# - Time - Duration
v = Time(1, 0) - Duration(1, 0)
self.assertEquals(Time(), v)
v = Time(1, 1) - Duration(-1, -1)
self.assertEquals(Time(2, 2), v)
v = Time(1) - Duration(0, 1000000000)
self.assertEquals(Time(), v)
v = Time(2) - Duration(0, 1000000000)
self.assertEquals(Time(1), v)
v = Time(400, 100) - Duration(300)
self.assertEquals(Time(100, 100), v)
v = Time(100, 100) - Duration(0, 101)
self.assertEquals(Time(99, 999999999), v)
# - Time - Time = Duration
v = Time(100, 100) - Time(100, 100)
self.assertEquals(Duration(), v)
v = Time(100, 100) - Time(100)
self.assertEquals(Duration(0, 100), v)
v = Time(100) - Time(200)
self.assertEquals(Duration(-100), v)
# Time (float secs) vs. Time(int, int)
self.assertEquals(Time.from_sec(0.5), Time(0.5))
t = Time(0.5)
self.assert_(type(t.secs) == int)
self.assertEquals(0, t.secs)
self.assertEquals(500000000, t.nsecs)
try:
Time(0.5, 0.5)
self.fail("should have thrown value error")
except ValueError:
pass
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)
def test_Time(self):
from genpy.rostime import Time, Duration
self.test_TVal(TVal=Time, test_neg=False)
# #1600 Duration > Time should fail
failed = False
try:
v = Duration.from_sec(0.1) > Time.from_sec(0.5)
failed = True
except: pass
self.failIf(failed, "should have failed to compare")
try:
v = Time.from_sec(0.4) > Duration.from_sec(0.1)
failed = True
except: pass
self.failIf(failed, "should have failed to compare")
# TODO: sub
# neg time fails
try:
Time(-1)
failed = True
except: pass
self.failIf(failed, "negative time not allowed")
try:
Time(1, -1000000001)
failed = True
except: pass
self.failIf(failed, "negative time not allowed")
# test Time.now() is within 10 seconds of actual time (really generous)
import time
t = time.time()
v = Time.from_sec(t)
self.assertEquals(v.to_sec(), t)
# test from_sec()
self.assertEquals(Time.from_sec(0), Time())
self.assertEquals(Time.from_sec(1.), Time(1))
self.assertEquals(Time.from_sec(v.to_sec()), v)
self.assertEquals(v.from_sec(v.to_sec()), v)
# test to_time()
self.assertEquals(v.to_sec(), v.to_time())
# test addition
# - time + time fails
try:
v = Time(1,0) + Time(1, 0)
failed = True
except: pass
self.failIf(failed, "Time + Time must fail")
# - time + duration
v = Time(1,0) + Duration(1, 0)
self.assertEquals(Time(2, 0), v)
v = Duration(1, 0) + Time(1,0)
self.assertEquals(Time(2, 0), v)
v = Time(1,1) + Duration(1, 1)
self.assertEquals(Time(2, 2), v)
v = Duration(1, 1) + Time(1,1)
self.assertEquals(Time(2, 2), v)
v = Time(1) + Duration(0, 1000000000)
self.assertEquals(Time(2), v)
v = Duration(1) + Time(0, 1000000000)
self.assertEquals(Time(2), v)
v = Time(100, 100) + Duration(300)
self.assertEquals(Time(400, 100), v)
v = Duration(300) + Time(100, 100)
self.assertEquals(Time(400, 100), v)
v = Time(100, 100) + Duration(300, 300)
self.assertEquals(Time(400, 400), v)
v = Duration(300, 300) + Time(100, 100)
self.assertEquals(Time(400, 400), v)
v = Time(100, 100) + Duration(300, -101)
self.assertEquals(Time(399, 999999999), v)
v = Duration(300, -101) + Time(100, 100)
self.assertEquals(Time(399, 999999999), v)
# test subtraction
try:
v = Time(1,0) - 1
failed = True
except: pass
self.failIf(failed, "Time - non Duration must fail")
class Foob(object): pass
try:
v = Time(1,0) - Foob()
failed = True
except: pass
self.failIf(failed, "Time - non TVal must fail")
# - Time - Duration
v = Time(1,0) - Duration(1, 0)
self.assertEquals(Time(), v)
v = Time(1,1) - Duration(-1, -1)
self.assertEquals(Time(2, 2), v)
v = Time(1) - Duration(0, 1000000000)
self.assertEquals(Time(), v)
v = Time(2) - Duration(0, 1000000000)
self.assertEquals(Time(1), v)
v = Time(400, 100) - Duration(300)
self.assertEquals(Time(100, 100), v)
v = Time(100, 100) - Duration(0, 101)
self.assertEquals(Time(99, 999999999), v)
# - Time - Time = Duration
v = Time(100, 100) - Time(100, 100)
self.assertEquals(Duration(), v)
v = Time(100, 100) - Time(100)
self.assertEquals(Duration(0,100), v)
v = Time(100) - Time(200)
self.assertEquals(Duration(-100), v)
# Time (float secs) vs. Time(int, int)
self.assertEquals(Time.from_sec(0.5), Time(0.5))
t = Time(0.5)
self.assert_(type(t.secs) == int)
self.assertEquals(0, t.secs)
self.assertEquals(500000000, t.nsecs)
try:
Time(0.5, 0.5)
self.fail("should have thrown value error")
except ValueError: pass
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)
t_curr, scale)
keypoints.append(
kpMsg(x=trainKP[m.trainIdx].pt[0],
y=trainKP[m.trainIdx].pt[1],
scale=scale,
class_id=clsid,
detects=kpHist[clsid].detects,
trainSize=trainKP[m.trainIdx].size,
querySize=queryKP[m.queryIdx].size))
if opts.publish:
keypoints = sorted(keypoints,
key=op.attrgetter('scale', 'detects'),
reverse=True)[:10]
datalog.write(frame_id=frmbuf.frameNum,
timestep=Duration(int((t_curr - t_last) / 1000),
((t_curr - t_last) % 1000) * 1e6),
keypoints=keypoints)
# Update the keypoint history for previously expanding keypoint that were
# not detected/matched in this frame
detected = set(map(op.attrgetter('class_id'), trainKP))
# get rid of old matches
kpHist = OrderedDict(
filter(lambda kv: kv[1].downdate().age < LAST_DAY, kpHist.iteritems()))
# keep matches that were missed in this frame
missed = filter(lambda k: (kpHist[k].age > 0) and (k not in detected),
kpHist.iterkeys())
missed = [(kpHist[k].keypoint, kpHist[clsid].descriptor.reshape(1, -1))
for k in missed]
if __name__ == '__main__':
rospy.init_node('sample_planning_node')
star = AStar()
star.getMap()
rospy.sleep(Duration.from_sec(0.5))
star.clear_cells()
# if not '--clear' in sys.argv():
rospy.sleep(Duration.from_sec(0.1))
path = star.find_path((3, 3), (9, 15))
star.pub_cells(path, star.path_cells_pub)
# rospy.spin()
