def test_rotation_distance(self):
mat1 = np.eye(3)
mat2 = np.eye(3)
diff_theta = rotation_distance(mat1, mat2)
self.assertEqual(diff_theta, 0.0)
mat1 = rpy_matrix(0, 0, np.pi)
mat2 = np.eye(3)
diff_theta = rotation_distance(mat1, mat2)
self.assertEqual(diff_theta, np.pi)
def move_trajectory_sequence(self, trajectory_points, time_list, stop=True,
start_time=None, send_action=None):
"""Move base following the trajectory points at each time points
trajectory-points [ list of #f(x y yaw) ([m] for x, y; [rad] for yaw) ]
time-list [list of time span [sec] ]
stop [ stop after msec moveing ]
start-time [ robot will move at start-time [sec or ros::Time] ]
send-action [ send message to action server, it means robot will move ]
"""
if len(trajectory_points) != len(time_list):
raise ValueError
while self.odom_msg is None:
rospy.sleep(0.01)
odom_coords = geometry_pose_to_coords(self.odom_msg.pose)
goal = control_msgs.msg.FollowJointTrajectoryActionGoal()
msg = trajectory_msgs.msg.JointTrajectory()
msg.joint_names = self.move_base_trajectory_joint_names
if start_time is not None:
msg.header.stamp = start_time
else:
msg.header.stamp = rospy.Time.now()
coords_list = [odom_coords]
for traj_point in trajectory_points:
cds = Coordinates(pos=odom_coords.translation,
rot=odom_coords.rotation)
cds.translate((traj_point[0], traj_point[1], 0.0))
cds.rotate(traj_point[2], 'z')
coords_list.append(cds)
cur_cds = odom_coords
cur_yaw = cur_cds.rpy_angle()[0][0]
cur_time = 0
pts_msg_list = []
for i, (cur_cds, next_cds) in enumerate(zip(
coords_list[:-1],
coords_list[1:])):
next_time = time_list[i]
tra = cur_cds.transformation(next_cds)
rot = cur_cds.rotate_vector(tra.translation)
diff_yaw = rotation_distance(cur_cds.rotation, next_cds.rotation)
pts_msg_list.append(
trajectory_msgs.msg.JointTrajectoryPoint(
positions=[cur_cds.translation[0],
cur_cds.translation[1],
cur_yaw],
velocities=[rot[0] / next_time,
rot[1] / next_time,
tra.rpy_angle()[0][0] / next_time],
time_from_start=rospy.Time(cur_time)))
cur_time += next_time
cur_cds = next_cds
if tra.rpy_angle()[0][0] > 0:
cur_yaw += abs(diff_yaw)
else:
cur_yaw -= abs(diff_yaw)
# append last point
if stop:
velocities = [0, 0, 0]
else:
velocities = [rot[0] / next_time,
rot[1] / next_time,
tra.rpy_angle()[0][0] / next_time]
pts_msg_list.append(
trajectory_msgs.msg.JointTrajectoryPoint(
positions=[cur_cds.translation[0],
cur_cds.translation[1],
cur_yaw],
velocities=velocities,
time_from_start=rospy.Time(cur_time)))
msg.points = pts_msg_list
goal.goal.trajectory = msg
if send_action:
if self.move_base_trajectory_action is None:
rospy.logerror(
'send_action is True, '
'but move_base_trajectory_action is not found')
return False
self.move_base_trajectory_action.send_goal(goal.goal)
if self.move_base_trajectory_action.wait_for_result():
return self.move_base_trajectory_action.get_result()
else:
return False
return goal