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

Check for duplicated joint names in a trajectory #162

Merged
merged 3 commits into from
Oct 2, 2023
Merged

Check for duplicated joint names in a trajectory #162

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b59ec8b
Check for duplicated joint names in a trajectory
ted-miller Sep 27, 2023
2f71399
Report joint index on duplicate name
ted-miller Sep 28, 2023
d0609b5
Starting index for comparison should point to NEXT entry in array
ted-miller Sep 28, 2023
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
4 changes: 4 additions & 0 deletions src/ActionServer_FJT.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -320,6 +320,10 @@ rcl_ret_t Ros_ActionServer_FJT_Goal_Received(rclc_action_goal_handle_t* goal_han
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.");
Expand Down
1 change: 1 addition & 0 deletions src/ErrorHandling.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ typedef enum
INIT_TRAJ_WRONG_NUMBER_OF_VELOCITIES,
INIT_TRAJ_INVALID_ENDING_VELOCITY,
INIT_TRAJ_INVALID_ENDING_ACCELERATION,
INIT_TRAJ_DUPLICATE_JOINT_NAME,
} Init_Trajectory_Status;

typedef enum
Expand Down
13 changes: 12 additions & 1 deletion src/MotionControl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ Init_Trajectory_Status Ros_MotionControl_Init(rosidl_runtime_c__String__Sequence
{
long pulsePos[MAX_PULSE_AXES];
long curPos[MAX_PULSE_AXES];
int grpIndex, jointIndexInTraj, pointIndex;
int grpIndex, jointIndexInTraj, pointIndex, checkForDupIndex;

//Verify we're not already running a trajectory
for (grpIndex = 0; grpIndex < g_Ros_Controller.numGroup; grpIndex += 1)
Expand Down Expand Up @@ -94,6 +94,17 @@ Init_Trajectory_Status Ros_MotionControl_Init(rosidl_runtime_c__String__Sequence
int jointIndexInCtrlGroup;
CtrlGroup* ctrlGroup;

//check to ensure there are no duplicate joint names in the list
for (checkForDupIndex = jointIndexInTraj; checkForDupIndex < sequenceGoalJointNames->size; checkForDupIndex += 1)
gavanderhoorn marked this conversation as resolved.
Show resolved Hide resolved
{
if (strncmp(sequenceGoalJointNames->data[jointIndexInTraj].data,
sequenceGoalJointNames->data[checkForDupIndex].data,
MAX_JOINT_NAME_LENGTH) == 0)
{
Ros_Debug_BroadcastMsg("Joint name [%s] is used for multiple joints in the trajectory.", sequenceGoalJointNames->data[jointIndexInTraj].data);
ted-miller marked this conversation as resolved.
Show resolved Hide resolved
return INIT_TRAJ_DUPLICATE_JOINT_NAME;
}
}

//find the ctrlgroup for this joint
BOOL bFound = FALSE;
Expand Down
Loading