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_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)
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()
