本文整理汇总了C++中Trajectory::empty方法的典型用法代码示例。如果您正苦于以下问题:C++ Trajectory::empty方法的具体用法?C++ Trajectory::empty怎么用?C++ Trajectory::empty使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Trajectory
的用法示例。
在下文中一共展示了Trajectory::empty方法的2个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: time
//.........这里部分代码省略.........
}
// 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:
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());
}
}