def test_rotate_vector(self):
testing.assert_array_almost_equal(
rotate_vector([1, 0, 0], pi / 6.0, [1, 0, 0]), (1, 0, 0))
testing.assert_array_almost_equal(
rotate_vector([1, 0, 0], pi / 6.0, [0, 1, 0]),
(0.8660254, 0, -0.5))
testing.assert_array_almost_equal(
rotate_vector([1, 0, 0], pi / 6.0, [0, 0, 1]), (0.8660254, 0.5, 0))
def go_pos_unsafe_wait(self, wait_counts=3):
maxvel = 0.295
maxrad = 0.495
ratio = 0.8
counter = 0
if self.go_pos_unsafe_goal_msg is None:
return False
if self.move_base_trajectory_action is None:
rospy.logwarn("go_pos_unsafe_wait is disabled. "
'move_base_trajectory_action is not found')
return True
while counter < wait_counts:
action_ret = self.move_base_trajectory_action.wait_for_result()
if action_ret is False:
return False
goal_position = np.array(self.go_pos_unsafe_goal_msg.goal.
trajectory.points[1].positions,
dtype=np.float32)
odom = self.odom
odom_pos = odom.translation
odom_angle = odom.rpy_angle()[0][0]
diff_position = goal_position - (odom_pos + np.array(
(0, 0, odom_angle)))
v = rotate_vector(
np.array((diff_position[0], diff_position[1], 0.0)),
-odom_angle, 'z') - np.array((0, 0, odom_angle))
x = v[0]
y = v[1]
yaw = diff_position[2]
if yaw > 2 * np.pi:
yaw = yaw - 2 * np.pi
if yaw < -2 * np.pi:
yaw = yaw + 2 * np.pi
sec = max(
np.linalg.norm([x, y]) / (maxvel * ratio),
abs(yaw) / (maxrad * ratio))
sec = max(sec, 1.0)
step = 1.0 / sec
rospy.loginfo(
" diff-pos {} {}, diff-angle {}".format(
x, y, yaw))
if np.sqrt(x * x + y * y) <= 0.025 and \
abs(yaw) <= np.deg2rad(2.5) and \
counter > 0: # try at least 1 time
self.go_pos_unsafe_goal_msg = None
return True
self.go_pos_unsafe_goal_msg = self.move_trajectory(x * step,
y * step,
yaw * step,
sec,
stop=True)
self.move_base_trajectory_action.send_goal(
self.go_pos_unsafe_goal_msg.goal)
counter += 1
self.go_pos_unsafe_goal_msg = None
return True