diff --git a/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index 92e6227d..f5156a06 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -22,32 +22,32 @@ namespace trackjoint */ enum ErrorCodeEnum { - kNoError = 0, - kDesiredDurationTooShort = 1, - kMaxDurationExceeded = 2, - kVelocityExceedsLimit = 3, - kAccelExceedsLimit = 4, - kMaxDurationLessThanDesiredDuration = 5, - kLimitNotPositive = 6, - kGoalPositionMismatch = 7, - kFailureToGenerateSingleWaypoint = 8, - kLessThanTenTimestepsForStreamingMode = 9 + NO_ERROR = 0, + DESIRED_DURATION_TOO_SHORT = 1, + MAX_DURATION_EXCEEDED = 2, + VELOCITY_EXCEEDS_LIMIT = 3, + ACCEL_EXCEEDS_LIMIT = 4, + MAX_DURATION_LESS_THAN_DESIRED_DURATION = 5, + LIMIT_NOT_POSITIVE = 6, + GOAL_POSITION_MISMATCH = 7, + FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8, + LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9 }; /** * \brief Use this map to look up human-readable strings for each error code */ -const std::unordered_map kErrorCodeMap( - { { kNoError, "No error, trajectory generation was successful" }, - { kDesiredDurationTooShort, "Desired duration is too short, cannot have less than one timestep in a " - "trajectory" }, - { kMaxDurationExceeded, "Max duration was exceeded" }, - { kVelocityExceedsLimit, "A velocity input exceeds the velocity limit" }, - { kAccelExceedsLimit, "An acceleration input exceeds the acceleration limit" }, - { kMaxDurationLessThanDesiredDuration, "max_duration should not be less than desired_duration" }, - { kLimitNotPositive, "Vel/accel/jerk limits should be greater than zero" }, - { kGoalPositionMismatch, "Mismatch between the final position and the goal position" }, - { kFailureToGenerateSingleWaypoint, "Failed to generate even a single new waypoint" }, - { kLessThanTenTimestepsForStreamingMode, "In streaming mode, desired duration should be at least 10 " - "timesteps" } }); +const std::unordered_map ERROR_CODE_MAP( + { { NO_ERROR, "No error, trajectory generation was successful" }, + { DESIRED_DURATION_TOO_SHORT, "Desired duration is too short, cannot have less than one timestep in a " + "trajectory" }, + { MAX_DURATION_EXCEEDED, "Max duration was exceeded" }, + { VELOCITY_EXCEEDS_LIMIT, "A velocity input exceeds the velocity limit" }, + { ACCEL_EXCEEDS_LIMIT, "An acceleration input exceeds the acceleration limit" }, + { MAX_DURATION_LESS_THAN_DESIRED_DURATION, "max_duration should not be less than desired_duration" }, + { LIMIT_NOT_POSITIVE, "Vel/accel/jerk limits should be greater than zero" }, + { GOAL_POSITION_MISMATCH, "Mismatch between the final position and the goal position" }, + { FAILURE_TO_GENERATE_SINGLE_WAYPOINT, "Failed to generate even a single new waypoint" }, + { LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, "In streaming mode, desired duration should be at least 10 " + "timesteps" } }); } // end namespace trackjoint diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 49e4ac54..63795669 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -68,9 +68,9 @@ int main(int argc, char** argv) traj_gen.inputChecking(current_joint_states, goal_joint_states, limits, timestep); // Input error handling - if an error is found, the trajectory is not generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -81,9 +81,9 @@ int main(int argc, char** argv) auto end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -91,7 +91,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 1d0e729f..05f4a2a4 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -273,7 +273,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde } } - return ErrorCodeEnum::kNoError; + return ErrorCodeEnum::NO_ERROR; } inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t* index_last_successful) @@ -397,7 +397,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // limits. // This gives a new duration estimate. - ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; + ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // If in normal mode, we can extend trajectories if (!use_streaming_mode_) @@ -474,12 +474,12 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // Normal mode: Error if we extended the duration to the maximum and it still wasn't successful if (!use_streaming_mode_ && index_last_successful_ < static_cast(waypoints_.elapsed_times.size() - 1)) { - error_code = ErrorCodeEnum::kMaxDurationExceeded; + error_code = ErrorCodeEnum::MAX_DURATION_EXCEEDED; } // Error if not even a single waypoint could be generated if (waypoints_.positions.size() < 2) { - error_code = ErrorCodeEnum::kFailureToGenerateSingleWaypoint; + error_code = ErrorCodeEnum::FAILURE_TO_GENERATE_SINGLE_WAYPOINT; } return error_code; diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index 04d095da..5b85f7d9 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -70,15 +70,15 @@ int main(int argc, char** argv) trackjoint::ErrorCodeEnum error_code = traj_gen.inputChecking(start_state, goal_joint_states, limits, timestep); if (error_code) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } // Generate the initial trajectory error_code = traj_gen.generateTrajectories(&output_trajectories); - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } std::cout << "Initial trajectory calculation:" << std::endl; @@ -110,9 +110,9 @@ int main(int argc, char** argv) std::cout << "Run time (microseconds): " << std::chrono::duration_cast(end_time - start_time).count() << std::endl; - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 2d5a2d53..56cb26c2 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -74,9 +74,9 @@ int main(int argc, char** argv) // Input error handling - if an error is found, the trajectory is not // generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -86,9 +86,9 @@ int main(int argc, char** argv) auto end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -96,7 +96,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); @@ -167,9 +167,9 @@ int main(int argc, char** argv) // Input error handling - if an error is found, the trajectory is not // generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -179,9 +179,9 @@ int main(int argc, char** argv) end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -189,7 +189,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index a3779cd3..728fd8f8 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -194,13 +194,13 @@ ErrorCodeEnum TrajectoryGenerator::inputChecking(const std::vector limits[joint].velocity_limit) { - return ErrorCodeEnum::kVelocityExceedsLimit; + return ErrorCodeEnum::VELOCITY_EXCEEDS_LIMIT; } // Check that goal vels. are less than the limits. if (abs(goal_joint_states[joint].velocity) > limits[joint].velocity_limit) { - return ErrorCodeEnum::kVelocityExceedsLimit; + return ErrorCodeEnum::VELOCITY_EXCEEDS_LIMIT; } // Check that current accels. are less than the limits. if (abs(current_joint_states[joint].acceleration) > limits[joint].acceleration_limit) { - return ErrorCodeEnum::kAccelExceedsLimit; + return ErrorCodeEnum::ACCEL_EXCEEDS_LIMIT; } // Check that goal accels. are less than the limits. if (abs(goal_joint_states[joint].acceleration) > limits[joint].acceleration_limit) { - return ErrorCodeEnum::kAccelExceedsLimit; + return ErrorCodeEnum::ACCEL_EXCEEDS_LIMIT; } // Check for positive limits. if (limits[joint].velocity_limit <= 0 || limits[joint].acceleration_limit <= 0 || limits[joint].jerk_limit <= 0) { - return ErrorCodeEnum::kLimitNotPositive; + return ErrorCodeEnum::LIMIT_NOT_POSITIVE; } // In streaming mode, the user-requested duration should be >= kNumWaypointsThreshold * timestep. // upsample and downSample aren't used in streaming mode. if (rounded_duration < kNumWaypointsThreshold * nominal_timestep && use_streaming_mode_) { - return ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode; + return ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE; } } - return ErrorCodeEnum::kNoError; + return ErrorCodeEnum::NO_ERROR; } void TrajectoryGenerator::saveTrajectoriesToFile(const std::vector& output_trajectories, @@ -309,7 +309,7 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vector* trajectory) @@ -391,7 +391,7 @@ void TrajectoryGenerator::clipVectorsForOutput(std::vector* tra ErrorCodeEnum TrajectoryGenerator::generateTrajectories(std::vector* output_trajectories) { - ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; + ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // Generate individual joint trajectories for (size_t joint = 0; joint < kNumDof; ++joint) { diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 535a8375..5bde2e57 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -205,7 +205,7 @@ TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -252,7 +252,7 @@ TEST_F(TrajectoryGenerationTest, OneTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -282,7 +282,7 @@ TEST_F(TrajectoryGenerationTest, RoughlyTwoTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -318,7 +318,7 @@ TEST_F(TrajectoryGenerationTest, FourTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -365,7 +365,7 @@ TEST_F(TrajectoryGenerationTest, SixTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -405,7 +405,7 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); verifyVelAccelJerkLimits(output_trajectories_, limits_); @@ -589,7 +589,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) // Saving Trackjoint output to .csv files for plotting traj_gen.saveTrajectoriesToFile(output_trajectories_, BASE_FILEPATH + "_joint"); - EXPECT_EQ(ErrorCodeEnum::kNoError, error_code); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, error_code); // Timestep const double timestep_tolerance = 0.25 * timestep_; EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, @@ -637,7 +637,7 @@ TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -683,7 +683,7 @@ TEST_F(TrajectoryGenerationTest, LimitCompensation) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -732,7 +732,7 @@ TEST_F(TrajectoryGenerationTest, DurationExtension) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -787,7 +787,7 @@ TEST_F(TrajectoryGenerationTest, PositiveAndNegativeLimits) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -831,9 +831,9 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); output_trajectories_.resize(num_dof_); - EXPECT_EQ(ErrorCodeEnum::kNoError, + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 2e-3; @@ -892,7 +892,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(error_code, ErrorCodeEnum::kNoError); + EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); double position_error = std::numeric_limits::max(); double velocity_error = std::numeric_limits::max(); @@ -904,7 +904,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) traj_gen.reset(timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); error_code = traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(error_code, ErrorCodeEnum::kNoError); + EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); // Get a new seed state for next trajectory generation if ((std::size_t)output_trajectories_.at(joint_to_update).positions.size() > next_waypoint) { @@ -939,7 +939,7 @@ TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode, + EXPECT_EQ(ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); } } // namespace trackjoint @@ -951,4 +951,4 @@ int main(int argc, char** argv) int result = RUN_ALL_TESTS(); return result; -} \ No newline at end of file +}