コード例 #1
0
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());
  }
}
コード例 #2
0
// 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());
  }
}
コード例 #3
0
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());
  }
}
コード例 #4
0
// 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());
  }
}