Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Init_Trajectory_Status enum and messages moved to motoros2_interfaces #316

Merged
merged 1 commit into from
Oct 14, 2024
Merged

Init_Trajectory_Status enum and messages moved to motoros2_interfaces #316

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
66 changes: 5 additions & 61 deletions src/ActionServer_FJT.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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);
Expand Down
44 changes: 44 additions & 0 deletions src/ErrorHandling.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
34 changes: 18 additions & 16 deletions src/ErrorHandling.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
1 change: 1 addition & 0 deletions src/MotoROS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@
#include <motoros2_interfaces/srv/read_m_register.h>
#include <motoros2_interfaces/srv/write_m_register.h>
#include <motoros2_interfaces/msg/io_result_codes.h>
#include <motoros2_interfaces/msg/init_traj_enum.h>
#include <motoros2_interfaces/srv/reset_error.h>
#include <motoros2_interfaces/srv/start_traj_mode.h>
#include <motoros2_interfaces/srv/start_point_queue_mode.h>
Expand Down
Loading