C++ Trajectory::empty方法代码示例

//.........这里部分代码省略.........
    }

    // 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_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());
  }
}