From a7bb43d85f04bdbfa142ced8c75bdf48224f52c0 Mon Sep 17 00:00:00 2001 From: jimmy-mcelwain Date: Thu, 3 Oct 2024 12:03:38 -0400 Subject: [PATCH] The Init_Trajectory_Status enum and all messages associated with the enum moved to motoros2_interfaces --- src/ActionServer_FJT.c | 66 ++++-------------------------------------- src/ErrorHandling.c | 44 ++++++++++++++++++++++++++++ src/ErrorHandling.h | 34 ++++++++++++---------- src/MotoROS.h | 1 + 4 files changed, 68 insertions(+), 77 deletions(-) diff --git a/src/ActionServer_FJT.c b/src/ActionServer_FJT.c index 544b0411..16b48c64 100644 --- a/src/ActionServer_FJT.c +++ b/src/ActionServer_FJT.c @@ -247,8 +247,8 @@ rcl_ret_t Ros_ActionServer_FJT_Goal_Received(rclc_action_goal_handle_t* goal_han if (!bSizeOk) { motomanErrorCode = INIT_TRAJ_TOO_BIG; - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory contains too many points (Not enough memory)."); + rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, + Ros_ErrorHandling_Init_Trajectory_Status_ToString((Init_Trajectory_Status) motomanErrorCode)); } else if (!bMotionReady) { @@ -260,69 +260,13 @@ rcl_ret_t Ros_ActionServer_FJT_Goal_Received(rclc_action_goal_handle_t* goal_han { motomanErrorCode = INIT_TRAJ_WRONG_MODE; rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Must call " SERVICE_NAME_START_TRAJ_MODE " service."); + Ros_ErrorHandling_Init_Trajectory_Status_ToString((Init_Trajectory_Status) motomanErrorCode)); } else if (!bInitOk) { motomanErrorCode = trajStatus; - switch (motomanErrorCode) - { - case INIT_TRAJ_TOO_SMALL: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory must contain at least two points."); - break; - case INIT_TRAJ_INVALID_STARTING_POS: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The first point must match the robot's current position."); - break; - case INIT_TRAJ_INVALID_VELOCITY: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The commanded velocity is too high."); - break; - case INIT_TRAJ_ALREADY_IN_MOTION: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Already running a trajectory."); - break; - case INIT_TRAJ_INVALID_JOINTNAME: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Invalid joint name specified. Check motoros2_config.yaml."); - break; - case INIT_TRAJ_INCOMPLETE_JOINTLIST: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory must contain data for all joints."); - break; - case INIT_TRAJ_INVALID_TIME: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Invalid time in trajectory."); - break; - case INIT_TRAJ_BACKWARD_TIME: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory message contains waypoints that are not strictly increasing in time."); - break; - case INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory did not contain position data for all axes."); - break; - case INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory did not contain velocity data for all axes."); - break; - case INIT_TRAJ_INVALID_ENDING_VELOCITY: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The final point in the trajectory must have zero velocity."); - break; - case INIT_TRAJ_INVALID_ENDING_ACCELERATION: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The final point in the trajectory must have zero acceleration."); - break; - case INIT_TRAJ_DUPLICATE_JOINT_NAME: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "The trajectory contains duplicate joint names."); - break; - default: - rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, - "Trajectory initialization failed. Generic failure."); - } + rosidl_runtime_c__String__assign(&fjt_result_response.result.error_string, + Ros_ErrorHandling_Init_Trajectory_Status_ToString(trajStatus)); } fjt_result_response.result.error_code = RESULT_REPONSE_ERROR_CODE(control_msgs__action__FollowJointTrajectory_Result__INVALID_GOAL, motomanErrorCode); diff --git a/src/ErrorHandling.c b/src/ErrorHandling.c index 3897c0d7..c71b4329 100644 --- a/src/ErrorHandling.c +++ b/src/ErrorHandling.c @@ -78,6 +78,50 @@ const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCo } } +const char* const Ros_ErrorHandling_Init_Trajectory_Status_ToString(Init_Trajectory_Status code) +{ + //messages defined in motoros2_interfaces/msg/InitTrajEnum.msg + switch (code) + { + case INIT_TRAJ_OK: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_OK_STR; + case INIT_TRAJ_UNSPECIFIED: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_UNSPECIFIED_STR; + case INIT_TRAJ_TOO_SMALL: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_TOO_SMALL_STR; + case INIT_TRAJ_TOO_BIG: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_TOO_BIG_STR; + case INIT_TRAJ_ALREADY_IN_MOTION: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_ALREADY_IN_MOTION_STR; + case INIT_TRAJ_INVALID_STARTING_POS: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_STARTING_POS_STR; + case INIT_TRAJ_INVALID_VELOCITY: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_VELOCITY_STR; + case INIT_TRAJ_INVALID_JOINTNAME: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_JOINTNAME_STR; + case INIT_TRAJ_INCOMPLETE_JOINTLIST: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INCOMPLETE_JOINTLIST_STR; + case INIT_TRAJ_INVALID_TIME: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_TIME_STR; + case INIT_TRAJ_WRONG_MODE: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_MODE_STR; + case INIT_TRAJ_BACKWARD_TIME: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_BACKWARD_TIME_STR; + case INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS_STR; + case INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES_STR; + case INIT_TRAJ_INVALID_ENDING_VELOCITY: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_ENDING_VELOCITY_STR; + case INIT_TRAJ_INVALID_ENDING_ACCELERATION: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_ENDING_ACCELERATION_STR; + case INIT_TRAJ_DUPLICATE_JOINT_NAME: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_DUPLICATE_JOINT_NAME_STR; + default: + return motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_UNSPECIFIED_STR; + } +} + void motoRosAssert(BOOL mustBeTrue, ALARM_ASSERTION_FAIL_SUBCODE subCodeIfFalse) { motoRosAssert_withMsg(mustBeTrue, subCodeIfFalse, APPLICATION_NAME ": Fatal Error"); diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index 366a7ded..658f0305 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -37,22 +37,23 @@ typedef enum typedef enum { - INIT_TRAJ_OK = 0, - INIT_TRAJ_TOO_SMALL = 200, - INIT_TRAJ_TOO_BIG, - INIT_TRAJ_ALREADY_IN_MOTION, - INIT_TRAJ_INVALID_STARTING_POS, - INIT_TRAJ_INVALID_VELOCITY, - INIT_TRAJ_INVALID_JOINTNAME, - INIT_TRAJ_INCOMPLETE_JOINTLIST, - INIT_TRAJ_INVALID_TIME, - INIT_TRAJ_WRONG_MODE, - INIT_TRAJ_BACKWARD_TIME, - INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS, - INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES, - INIT_TRAJ_INVALID_ENDING_VELOCITY, - INIT_TRAJ_INVALID_ENDING_ACCELERATION, - INIT_TRAJ_DUPLICATE_JOINT_NAME, + INIT_TRAJ_OK = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_OK, + INIT_TRAJ_UNSPECIFIED = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_UNSPECIFIED, + INIT_TRAJ_TOO_SMALL = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_TOO_SMALL, + INIT_TRAJ_TOO_BIG = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_TOO_BIG, + INIT_TRAJ_ALREADY_IN_MOTION = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_ALREADY_IN_MOTION, + INIT_TRAJ_INVALID_STARTING_POS = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_STARTING_POS, + INIT_TRAJ_INVALID_VELOCITY = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_VELOCITY, + INIT_TRAJ_INVALID_JOINTNAME = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_JOINTNAME, + INIT_TRAJ_INCOMPLETE_JOINTLIST = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INCOMPLETE_JOINTLIST, + INIT_TRAJ_INVALID_TIME = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_TIME, + INIT_TRAJ_WRONG_MODE = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_MODE, + INIT_TRAJ_BACKWARD_TIME = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_BACKWARD_TIME, + INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_NUMBER_OF_POSITIONS, + INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES, + INIT_TRAJ_INVALID_ENDING_VELOCITY = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_ENDING_VELOCITY, + INIT_TRAJ_INVALID_ENDING_ACCELERATION = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_INVALID_ENDING_ACCELERATION, + INIT_TRAJ_DUPLICATE_JOINT_NAME = motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_DUPLICATE_JOINT_NAME, } Init_Trajectory_Status; typedef enum @@ -229,5 +230,6 @@ extern void motoRosAssert_withMsg(BOOL mustBeTrue, ALARM_ASSERTION_FAIL_SUBCODE extern const char* const Ros_ErrorHandling_ErrNo_ToString(int errNo); extern const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCode code); +extern const char* const Ros_ErrorHandling_Init_Trajectory_Status_ToString(Init_Trajectory_Status code); #endif // MOTOROS2_ERROR_HANDLING_H diff --git a/src/MotoROS.h b/src/MotoROS.h index 3f2f5a88..19dc651d 100644 --- a/src/MotoROS.h +++ b/src/MotoROS.h @@ -65,6 +65,7 @@ #include #include #include +#include #include #include #include