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

Fix SJTC from immediately returning success (#697) #710

Closed
wants to merge 1 commit into from
Closed

Fix SJTC from immediately returning success (#697) #710

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
8 changes: 7 additions & 1 deletion ur_controllers/src/scaled_joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
fill_partial_goal(*new_external_msg);
sort_to_local_joint_order(*new_external_msg);
traj_external_point_ptr_->update(*new_external_msg);
// set the active trajectory pointer to the new goal
traj_point_active_ptr_ = &traj_external_point_ptr_;
}

JointTrajectoryPoint state_current, state_desired, state_error;
Expand Down Expand Up @@ -238,6 +240,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
}
active_goal->setAborted(result);
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
// remove the active trajectory pointer so that we stop commanding the hardware
traj_point_active_ptr_ = nullptr;
}

// check goal tolerance
Expand All @@ -247,7 +251,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
active_goal->setSucceeded(res);
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());

// remove the active trajectory pointer so that we stop commanding the hardware
traj_point_active_ptr_ = nullptr;
RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!");
} else if (default_tolerances_.goal_time_tolerance != 0.0) {
// if we exceed goal_time_toleralance set it to aborted
Expand All @@ -262,6 +267,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
active_goal->setAborted(result);
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would we need to also set traj_point_active_ptr_ = nullptr; at this line, since we reset the goal pointer here?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes i thought about that too, but i did not see it in the original too when return error code goal_tolerance_violated.
this could be an error on the original or not needed or potentially breaking if inserted.
Furthermore it set's the nullptr on_cleanup and on_reset methods which i think are inhereted into the SJTC

RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds",
difference);
}
Expand Down