TEST_F(InitTrajectoryTest, WrappingSpec)
{
// Modify trajectory message created in the fixture to wrap around
const double wrap = 4 * M_PI;
for (unsigned int i = 0; i < trajectory_msg.points.size(); ++i)
{
trajectory_msg.points[i].positions[0] += wrap; // Add wrapping value to all points
}
// Before first point: Return 4 segments: Last of current + bridge + full message
const ros::Time time(curr_traj[0][0].startTime());
std::vector<bool> angle_wraparound(1, true);
InitJointTrajectoryOptions<Trajectory> options;
options.current_trajectory = &curr_traj;
options.angle_wraparound = &angle_wraparound;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
ASSERT_EQ(points.size() + 1, trajectory[0].size());
// Check current trajectory segment start/end times and states (only one)
{
const Segment& ref_segment = curr_traj[0][0];
const Segment& segment = trajectory[0][0];
// Check start/end times
{
EXPECT_EQ(ref_segment.startTime(), segment.startTime());
EXPECT_EQ(ref_segment.endTime(), segment.endTime());
}
// Check start state
{
typename Segment::State ref_state, state;
ref_segment.sample(ref_segment.startTime(), ref_state);
segment.sample(segment.startTime(), state);
EXPECT_EQ(ref_state.position[0], state.position[0]);
EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
}
// Check end state
{
typename Segment::State ref_state, state;
ref_segment.sample(ref_segment.endTime(), ref_state);
segment.sample(segment.endTime(), state);
EXPECT_EQ(ref_state.position[0], state.position[0]);
EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
}
}
// Check bridge trajectory segment start/end times and states (only one)
{
const Segment& ref_segment = curr_traj[0][0];
const Segment& segment = trajectory[0][1];
const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
// Segment start time should correspond to message start time
// Segment end time should correspond to first trajectory message point
const ros::Time msg_start_time = trajectory_msg.header.stamp;
const typename Segment::Time start_time = msg_start_time.toSec();
const typename Segment::Time end_time = (msg_start_time + msg_points[0].time_from_start).toSec();
// Check start/end times
{
EXPECT_EQ(start_time, segment.startTime());
EXPECT_EQ(end_time, segment.endTime());
}
// Check start state. Corresponds to current trajectory sampled at start_time
{
typename Segment::State ref_state, state;
ref_segment.sample(start_time, ref_state);
segment.sample(segment.startTime(), state);
EXPECT_EQ(ref_state.position[0], state.position[0]);
EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
}
// Check end state. Corresponds to first trajectory message point
{
typename Segment::State state;
segment.sample(segment.endTime(), state);
EXPECT_EQ(msg_points[0].positions[0] - wrap, state.position[0]); // NOTE: Wrap used
EXPECT_EQ(msg_points[0].velocities[0], state.velocity[0]);
EXPECT_EQ(msg_points[0].accelerations[0], state.acceleration[0]);
}
}
// Check all segment start/end times and states (2 segments)
for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it)
{
const ros::Time msg_start_time = trajectory_msg.header.stamp;
const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
const Segment& segment = trajectory[0][traj_it];
const JointTrajectoryPoint& p_start = msg_points[msg_it];
const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
// Check start/end times
{
const typename Segment::Time start_time = (msg_start_time + p_start.time_from_start).toSec();
const typename Segment::Time end_time = (msg_start_time + p_end.time_from_start).toSec();
EXPECT_EQ(start_time, segment.startTime());
EXPECT_EQ(end_time, segment.endTime());
}
// Check start state
{
typename Segment::State state;
segment.sample(segment.startTime(), state);
EXPECT_EQ(p_start.positions[0] - wrap, state.position[0]); // NOTE: Wrap used
EXPECT_EQ(p_start.velocities[0], state.velocity[0]);
EXPECT_EQ(p_start.accelerations[0], state.acceleration[0]);
}
// Check end state
{
typename Segment::State state;
segment.sample(segment.endTime(), state);
EXPECT_EQ(p_end.positions[0] - wrap, state.position[0]); // NOTE: Wrap used
EXPECT_EQ(p_end.velocities[0], state.velocity[0]);
EXPECT_EQ(p_end.accelerations[0], state.acceleration[0]);
}
}
// Reference joint names size mismatch
{
std::vector<bool> angle_wraparound(2, true);
InitJointTrajectoryOptions<Trajectory> options;
options.current_trajectory = &curr_traj;
options.angle_wraparound = &angle_wraparound;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp, options);
EXPECT_TRUE(trajectory.empty());
}
}
// Test logic of parsing a trajectory message. Current trajectory is specified, hence trajectory combination takes place
TEST_F(InitTrajectoryTest, InitLogicCombine)
{
const ros::Time msg_start_time = trajectory_msg.header.stamp;
const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
InitJointTrajectoryOptions<Trajectory> options;
options.current_trajectory = &curr_traj;
// Empty trajectory message: Return empty trajectory
{
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msgs::JointTrajectory(), msg_start_time, options);
EXPECT_TRUE(trajectory.empty());
}
// Input time is...
// Before first point, starting the current trajectory: Return 4 segments: Last of current + bridge + full message
{
const ros::Time time(curr_traj[0][0].startTime());
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
EXPECT_EQ(points.size() + 1, trajectory[0].size());
}
// Before first point: Return 4 segments: Last of current + bridge + full message
{
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
EXPECT_EQ(points.size() + 1, trajectory[0].size());
}
// First point: Return partial trajectory, 3 segments: Last of current + bridge + last of message
{
const ros::Time time = msg_start_time + msg_points.begin()->time_from_start;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
EXPECT_EQ(points.size(), trajectory[0].size());
}
// Between the first and second points: Same as before
{
const ros::Time time = msg_start_time +
ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0);
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
EXPECT_EQ(msg_points.size(), trajectory[0].size());
}
// Second point: Return partial trajectory, 2 segments: Last of current + bridge (only one point of message made it)
{
const ros::Time time = msg_start_time + (++msg_points.begin())->time_from_start;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
EXPECT_EQ(msg_points.size() - 1, trajectory[0].size());
}
// Between the second and third points: Same as before
{
const ros::Time time = msg_start_time +
ros::Duration(((++msg_points.begin())->time_from_start + (--msg_points.end())->time_from_start).toSec() / 2.0);
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
EXPECT_EQ(msg_points.size() - 1, trajectory[0].size());
}
// Last point: Return empty trajectory
{
const ros::Time time = msg_start_time + (--msg_points.end())->time_from_start;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
EXPECT_TRUE(trajectory.empty());
}
// After the last point: Return empty trajectory
{
const ros::Time time(msg_start_time + msg_points.back().time_from_start + ros::Duration(1.0));
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
EXPECT_TRUE(trajectory.empty());
}
}
TEST_F(InitTrajectoryTest, JointMapping)
{
// Add an extra joint to the trajectory message created in the fixture
trajectory_msg.points[0].positions.push_back(-2.0);
trajectory_msg.points[0].velocities.push_back(0.0);
trajectory_msg.points[0].accelerations.push_back(0.0);
trajectory_msg.points[1].positions.push_back(-4.0);
trajectory_msg.points[1].velocities.push_back(0.0);
trajectory_msg.points[1].accelerations.push_back(0.0);
trajectory_msg.points[2].positions.push_back(-3.0);
trajectory_msg.points[2].velocities.push_back(0.0);
trajectory_msg.points[2].accelerations.push_back(0.0);
trajectory_msg.joint_names.push_back("bar_joint");
// No reference joint names: Original message order is preserved
{
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp);
// Check position values only
const Segment& segment_foo_joint = trajectory[0][0];
const Segment& segment_bar_joint = trajectory[1][0];
typename Segment::State state;
segment_foo_joint.sample(segment_foo_joint.startTime(), state);
EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[0]);
segment_bar_joint.sample(segment_bar_joint.startTime(), state);
EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]);
}
// Reference joint names vector that reverses joint order
{
vector<string> joint_names; // Joints are reversed
joint_names.push_back(trajectory_msg.joint_names[1]);
joint_names.push_back(trajectory_msg.joint_names[0]);
InitJointTrajectoryOptions<Trajectory> options;
options.joint_names = &joint_names;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp, options);
// Check position values only
const Segment& segment_bar_joint = trajectory[0][0];
const Segment& segment_foo_joint = trajectory[1][0];
typename Segment::State state_foo_joint;
typename Segment::State state_bar_joint;
segment_foo_joint.sample(segment_foo_joint.startTime(), state_foo_joint);
segment_bar_joint.sample(segment_bar_joint.startTime(), state_bar_joint);
EXPECT_NE(trajectory_msg.points[0].positions[0], state_bar_joint.position[0]);
EXPECT_NE(trajectory_msg.points[0].positions[1], state_foo_joint.position[0]);
EXPECT_EQ(trajectory_msg.points[0].positions[0], state_foo_joint.position[0]);
EXPECT_EQ(trajectory_msg.points[0].positions[1], state_bar_joint.position[0]);
}
// Reference joint names size mismatch
{
vector<string> joint_names;
joint_names.push_back(trajectory_msg.joint_names[0]);
InitJointTrajectoryOptions<Trajectory> options;
options.joint_names = &joint_names;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp, options);
EXPECT_TRUE(trajectory.empty());
}
// Reference joint names value mismatch
{
vector<string> joint_names;
joint_names.push_back(trajectory_msg.joint_names[0]);
joint_names.push_back("baz_joint");
InitJointTrajectoryOptions<Trajectory> options;
options.joint_names = &joint_names;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp, options);
EXPECT_TRUE(trajectory.empty());
}
}
// Test logic of parsing a trajectory message. No current trajectory is specified
TEST_F(InitTrajectoryTest, InitLogic)
{
const ros::Time msg_start_time = trajectory_msg.header.stamp;
const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
// Empty trajectory message: Return empty trajectory
{
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msgs::JointTrajectory(), msg_start_time);
EXPECT_TRUE(trajectory.empty());
}
// Input time is...
// Before first point: Return full trajectory (2 segments)
{
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
EXPECT_EQ(points.size() - 1, trajectory[0].size());
}
// First point: Return partial trajectory (last segment)
{
const ros::Time time = msg_start_time + msg_points.begin()->time_from_start;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
EXPECT_EQ(points.size() - 2, trajectory[0].size());
}
// Between the first and second points: Return partial trajectory (last segment)
{
const ros::Time time = msg_start_time +
ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0);
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
EXPECT_EQ(msg_points.size() - 2, trajectory[0].size());
}
// Second point: Return empty trajectory
{
const ros::Time time = msg_start_time + (++msg_points.begin())->time_from_start;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
EXPECT_TRUE(trajectory.empty());
}
// Between the second and third points: Return empty trajectory
{
const ros::Time time = msg_start_time +
ros::Duration(((++msg_points.begin())->time_from_start + (--msg_points.end())->time_from_start).toSec() / 2.0);
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
EXPECT_TRUE(trajectory.empty());
}
// Last point: Return empty trajectory
{
const ros::Time time = msg_start_time + (--msg_points.end())->time_from_start;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
EXPECT_TRUE(trajectory.empty());
}
// After the last point: Return empty trajectory
{
const ros::Time time(msg_start_time + msg_points.back().time_from_start + ros::Duration(1.0));
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
EXPECT_TRUE(trajectory.empty());
}
}