def _compare_odometry(self, odom, world_x_exp, world_y_exp,
world_theta_exp, start_world_x, start_world_y,
start_world_theta):
"""Compares the expected world coordinates with an Odometry message
Parameters:
:param Odometry odom: The Odometry message
:param float world_x_exp: The expected world X coordinate
:param float world_y_exp: The expected world Y coordinate
:param float world_theta_exp: The expected world Theta orientation
:param float start_world_x: The staring world X coordinate
:param float start_world_y: The staring world Y coordinate
:param float start_world_theta: The staring world Theta coordinate
"""
# print("Staring World: ({}, {}, {})".format(
# start_world_x, start_world_y, start_world_theta))
# print("Expected World: ({}, {}, {})".format(world_x_exp, world_y_exp, world_theta_exp))
x_actual = odom.pose.pose.position.x
y_actual = odom.pose.pose.position.y
th_actual = tf.transformations.euler_from_quaternion([
odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w,
])[2]
print("Actual World/Odom: ({}, {}, {})".format(x_actual, y_actual,
th_actual))
# Adjust expected pose to accomodate the starting pose using a 2D transformation
x_exp_adj, y_exp_adj, theta_exp_adj = transform_pose_2d(
world_x_exp, world_y_exp, world_theta_exp, start_world_x,
start_world_y, start_world_theta)
print("Actual: ({}, {}, {}), Expected: ({}, {}, {})".format(
x_actual, y_actual, th_actual, x_exp_adj, y_exp_adj,
theta_exp_adj))
self.assertAlmostEqual(x_actual, x_exp_adj, places=2)
self.assertAlmostEqual(y_actual, y_exp_adj, places=2)
self.assertAlmostEqual(normalize_theta(th_actual),
normalize_theta(theta_exp_adj),
places=1)
def run(self):
looprate = rospy.Rate(self._loophz)
try:
while not rospy.is_shutdown():
# Update the heading state
with self._state_lock:
self._current_heading = heading_from_odometry(self._odom)
rospy.logdebug(
"Current heading: {} deg (goal: {} deg)".format(
round(
math.degrees(
normalize_theta(self._current_heading)),
2), round(math.degrees(self._heading_goal)),
2))
self._decide()
looprate.sleep()
except rospy.ROSInterruptException:
rospy.logwarn("ROSInterruptException received in main loop")
def test_normalize_theta(self):
# (input_theta, exp_normal_theta)
tests = [
# Positive & Negatives
(pi, pi), # 180 = 180
(-pi, pi), # -180 = 180
(twopi, 0.0), # 360 = 0
(-twopi, 0.0), # -360 = 0
(0, 0), # 0 = 0
(pi / 2 - pi, pi * 1.5), # -90 = 270
# More that 2 Pi cases
(twopi + pi, pi), # 3 pi = pi
(-twopi - pi, pi), # -3 pi = pi
(pi + pi * 1.5, pi / 2) # 2.5 pi = 0.5 pi
]
for input_theta, exp_normal_theta in tests:
normal_theta = normalize_theta(input_theta)
print("Input theta: {}, actual: {}, expected: {}".format(
input_theta, normal_theta, exp_normal_theta))
self.assertAlmostEqual(normal_theta, exp_normal_theta, 3)
def _process_obstacle_plan(self):
"""
Note, the logic here assumes that if self._obstacle_XXX is None
then we haven't yet been in position to test it. Once we test that
position, we set the value to either True (obstacle) or False (clear)
then calculate the turn and switch into TURN mode.
Therefore, if we are in PLAN mode, we can determine which sides we need
to test still by examiming the self._obstacle_XXX state.
Example:
If in PLAN mode and self._obstacle_forward is NONE, we need to
test the front position, and if TRUE, turn to the right side.
If in PLAN mode and self._obstacle_forward is TRUE,
and self._obstacle_right is NONE: we have turned to the right
but have not yet tested the right side for an obstacle. So test
the position and if TRUE, we need to turn to the left side.
"""
if self._obstacle_forward in (None, False):
if self._prox_sensor is True:
# Calculate the turn to check the right side
self._obstacle_forward = True
rospy.logdebug("(Planner) Forward is blocked")
self._heading_goal = normalize_theta(self._current_heading -
self._turn_radians)
rospy.logdebug(
"(Planner) Turning to check right side. New heading: {}".
format(math.degrees(self._heading_goal)))
self._mode = MODE_OBSTACLE_TURN
else:
self._set_forward_mode()
elif self._obstacle_right is None:
if self._prox_sensor is True:
# Calculate the turn to check the left side
# We've already turned to the right, so we need to turn 180 to test
# the left side
self._obstacle_right = True
rospy.logdebug("(Planner) Right side is blocked")
self._heading_goal = normalize_theta(self._current_heading +
self._turn_radians * 2)
rospy.logdebug(
"(Planner) Turning to check left side. New heading: {}".
format(math.degrees(self._heading_goal)))
self._mode = MODE_OBSTACLE_TURN
else:
self._set_forward_mode()
elif self._obstacle_left is None:
if self._prox_sensor is True:
# All three of fwd, right, left are blocked
self._obstacle_left = True
rospy.logdebug("(Planner) left is blocked")
self._heading_goal = normalize_theta(self._current_heading +
self._turn_radians)
rospy.logdebug(
"(Planner) Turning to rear to backtrack. New heading: {}".
format(math.degrees(self._heading_goal)))
self._mode = MODE_OBSTACLE_TURN
self._reverse_plan = True
else:
self._set_forward_mode()
elif self._reverse_plan is True:
# We were performing a turn to reverse. Since we're in plan mode
# again, this means the turn is complete
rospy.logdebug("(Planner) Turn to rear complete, moving forward")
self._set_forward_mode()
else:
# This should not be possible
message = "Obstacle plan logic reached else block that should not be possible"
rospy.logerr(message)
raise RuntimeError(message)