def test_cocircular(self):
p1 = model.pose(0, 0, -np.pi/4)
p2 = model.pose(10, 0, np.pi/4)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_cocircular(self):
p1 = model.pose(0, 0, -np.pi / 4)
p2 = model.pose(10, 0, np.pi / 4)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_simple(self):
p1 = model.pose(0, 0, np.pi)
p2 = model.pose(10, 0, 0)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_straightish(self):
p1 = model.pose(0, 0, 0)
p2 = model.pose(10, 0, 0.001)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_straight_diag(self):
p1 = model.pose(0, 0, np.pi / 4)
p2 = model.pose(10, 10, np.pi / 4)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_straightish(self):
p1 = model.pose(0, 0, 0)
p2 = model.pose(10, 0, 0.001)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_parallel(self):
p1 = model.pose(0, 0, 0)
p2 = model.pose(10, 5, 0)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_straight_diag(self):
p1 = model.pose(0, 0, np.pi/4)
p2 = model.pose(10, 10, np.pi/4)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_simple(self):
p1 = model.pose(0, 0, np.pi)
p2 = model.pose(10, 0, 0)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def test_parallel(self):
p1 = model.pose(0, 0, 0)
p2 = model.pose(10, 5, 0)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
def timer_callback(self, timer):
rospy.loginfo("Top Planner pub loop")
try:
trans = self.tf_buffer.lookup_transform(
target_frame=self.map_frame,
source_frame="base_link",
time=timer.current_real,
timeout=rospy.Duration(1)
)
except Exception:
rospy.loginfo("Top Planner pub loop: tf from base link to map not found")
return
at = model.pose_from_ros(trans)
if np.isnan(at.x) or np.isnan(at.y) or np.isnan(at.theta):
return
if self.path is None:
return
while self.path_index < len(self.path):
map_target_pose = self.path[self.path_index]
t = model.pose_from_ros(map_target_pose)
## If already achieved a point or if a point is not ahead and within turning radius, look at future points in the path
if self.is_achieved(at, t) or not self.is_feasible(at, t):
rospy.loginfo("passed index {}".format(self.path_index))
self.path_index += 1
self.pub_path_point = True
else:
break
if self.marker_target_pose is not None and (self.is_achieved(at,
model.pose(
self.marker_target_pose.pose.position.x,
self.marker_target_pose.pose.position.y,
at.theta
)) or not self.is_feasible(at,
model.pose(
self.marker_target_pose.pose.position.x,
self.marker_target_pose.pose.position.y,
at.theta
))):
self.marker_target_pose = None
rospy.loginfo("path index {} publish? {}".format(self.path_index, self.pub_path_point))
if self.marker_target_pose is not None:
self.target_pose_pub.publish(self.marker_target_pose)
self.pub_path_point = True
elif self.path is not None and self.path_index < len(self.path):
if self.pub_path_point:
rospy.loginfo("publishing path target pose")
self.target_pose_pub.publish(self.path[self.path_index])
self.pub_path_point = False
elif self.scan_target_pose is not None:
self.target_pose_pub.publish(self.scan_target_pose)
def test_empty(self):
p1 = model.pose(0, 0, np.pi)
p2 = model.pose(10, 0, 0)
a = AckermannRRT2(None, p1, p2)
ps = np.recarray(0, dtype=model.pose_dtype)
d, b = a._distance_metric(ps, p2)
self.check_result(d, b)
def test_empty(self):
p1 = model.pose(0, 0, np.pi)
p2 = model.pose(10, 0, 0)
a = AckermannRRT2(None, p1, p2)
ps = np.recarray(0, dtype=model.pose_dtype)
d, b = a._distance_metric(ps, p2)
self.check_result(d, b)
def test_vectorized(self):
p1 = model.pose(0, 0, np.pi)
p2 = model.pose(10, 0, 0)
a = AckermannRRT2(None, p1, p2)
ps = np.recarray(2, dtype=model.pose_dtype)
ps[0] = model.pose(0, 0, np.pi)
ps[1] = model.pose(0, 0, -np.pi)
d, b = a._distance_metric(ps, p2)
self.check_result(d, b)
def test_vectorized(self):
p1 = model.pose(0, 0, np.pi)
p2 = model.pose(10, 0, 0)
a = AckermannRRT2(None, p1, p2)
ps = np.recarray(2, dtype=model.pose_dtype)
ps[0] = model.pose(0, 0, np.pi)
ps[1] = model.pose(0, 0, -np.pi)
d, b = a._distance_metric(ps, p2)
self.check_result(d, b)
def test_resize_after_indexerror_recarray(self):
ll = NumpyList(dtype=pose_dtype, atype=np.recarray)
ll.append(pose(1, 2, 3))
ll.append(pose(4, 5, 6))
self.assertEqual(sys.getrefcount(ll._data), 2)
with self.assertRaises(IndexError, msg="indexerrors are raised correctly"):
ll[2]
ll.capacity = 100
self.assertEqual(sys.getrefcount(ll._data), 2)
def timer_callback(self, event):
if self.map is None:
return
try:
tf = self.tf_buffer.lookup_transform(
target_frame=self.map.frame,
source_frame=self.base_frame,
time=event.current_real,
timeout=rospy.Duration(0.05)
).transform
except tf2_ros.LookupException:
return
except tf2_ros.ExtrapolationException:
return
x = tf.translation.x
y = tf.translation.y
_, _, theta = transformations.euler_from_quaternion(
ros_numpy.numpify(tf.rotation)
)
at = model.pose(x, y, theta)
ok = model.robot_fits(self.map, at, center_only=False)
print ok
self.pub_color.publish(ColorRGBA(0, 1, 0, 0.5) if ok else ColorRGBA(1, 0, 0, 0.5))
def test_equilaterateral(self):
p1 = model.pose(0, 0, -np.pi/6)
p2 = model.pose(8, 0, -np.pi/6)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
p1 = model.pose(0, 0, -np.pi/6)
p2 = model.pose(10, -2*np.sqrt(3), -np.pi/2)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
raise NotImplementedError
def test_equilaterateral(self):
p1 = model.pose(0, 0, -np.pi / 6)
p2 = model.pose(8, 0, -np.pi / 6)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
p1 = model.pose(0, 0, -np.pi / 6)
p2 = model.pose(10, -2 * np.sqrt(3), -np.pi / 2)
a = AckermannRRT2(None, p1, p2)
d, b = a._distance_metric(p1, p2)
self.check_result(d, b)
raise NotImplementedError
def test_with_recarray(self):
ll = NumpyList(dtype=pose_dtype, atype=np.recarray)
ll.append(pose(1, 2, 3))
ll.append(pose(4, 5, 6))
ll.append(pose(7, 8, 9))
self.assertEqual(ll[0], pose(1, 2, 3), "element lookup works")
self.assertEqual(ll[1].x, 4, "recarrays class is preserved")
ll.capacity = 100
with self.assertRaises(ValueError):
ll.capacity = 0
# check that pops work
self.assertEqual(ll.pop(0), pose(1, 2, 3))
self.assertEqual(ll.pop(-1), pose(7, 8, 9))
self.assertEqual(ll[0], pose(4, 5, 6))