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)