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

Updated scaled JTC to latest JTC updates #1067

Merged
merged 1 commit into from
Jul 26, 2024
Merged

Updated scaled JTC to latest JTC updates #1067

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
58 changes: 36 additions & 22 deletions 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 @@ -86,10 +86,11 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
return controller_interface::return_type::OK;
}
auto logger = this->get_node()->get_logger();
// update dynamic parameters
if (param_listener_->is_old(params_)) {
params_ = param_listener_->get_params();
default_tolerances_ = get_segment_tolerances(params_);
default_tolerances_ = get_segment_tolerances(logger, params_);
}

auto compute_error_for_joint = [&](JointTrajectoryPoint& error, int index, const JointTrajectoryPoint& current,
Expand Down Expand Up @@ -138,6 +139,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const

// currently carrying out a trajectory
if (has_active_trajectory()) {
// Main Speed scaling difference...
// Adjust time with scaling factor
TimeData time_data;
time_data.time = time;
Expand All @@ -153,9 +155,10 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
if (!traj_external_point_ptr_->is_sampled_already()) {
first_sample = true;
if (params_.open_loop_control) {
traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_);
traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_,
joints_angle_wraparound_);
} else {
traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_);
traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_, joints_angle_wraparound_);
}
}

Expand All @@ -178,12 +181,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
bool outside_goal_tolerance = false;
bool within_goal_time = true;
const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end();
auto active_tol = active_tolerances_.readFromRT();

// have we reached the end, are not holding position, and is a timeout configured?
// Check independently of other tolerances
if (!before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 &&
time_difference > cmd_timeout_) {
RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout");
RCLCPP_WARN(logger, "Aborted due to command timeout");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
Expand All @@ -195,21 +199,25 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const

// Always check the state tolerance on the first sample in case the first sample
// is the last point
if ((before_last_point || first_sample) &&
!check_state_tolerance_per_joint(state_error_, index, default_tolerances_.state_tolerance[index], false) &&
*(rt_is_holding_.readFromRT()) == false) {
// print output per default, goal will be aborted afterwards
if ((before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false &&
!check_state_tolerance_per_joint(state_error_, index, active_tol->state_tolerance[index],
true /* show_errors */)) {
tolerance_violated_while_moving = true;
}
// past the final point, check that we end up inside goal tolerance
if (!before_last_point &&
!check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index],
false) &&
*(rt_is_holding_.readFromRT()) == false) {
if (!before_last_point && *(rt_is_holding_.readFromRT()) == false &&
!check_state_tolerance_per_joint(state_error_, index, active_tol->goal_state_tolerance[index],
false /* show_errors */)) {
outside_goal_tolerance = true;

if (default_tolerances_.goal_time_tolerance != 0.0) {
if (time_difference > default_tolerances_.goal_time_tolerance) {
if (active_tol->goal_time_tolerance != 0.0) {
// if we exceed goal_time_tolerance set it to aborted
if (time_difference > active_tol->goal_time_tolerance) {
within_goal_time = false;
// print once, goal will be aborted afterwards
check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index],
true /* show_errors */);
}
}
}
Expand Down Expand Up @@ -263,54 +271,60 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
if (tolerance_violated_while_moving) {
auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
result->set__error_string("Aborted due to path tolerance violation");
active_goal->setAborted(result);
// TODO(matthew-reynolds): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
rt_has_pending_goal_.writeFromNonRT(false);

RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation");
RCLCPP_WARN(logger, "Aborted due to state tolerance violation");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
} else if (!before_last_point) {
// check goal tolerance
if (!outside_goal_tolerance) {
auto res = std::make_shared<FollowJTrajAction::Result>();
res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
active_goal->setSucceeded(res);
auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
result->set__error_string("Goal successfully reached!");
active_goal->setSucceeded(result);
// TODO(matthew-reynolds): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
rt_has_pending_goal_.writeFromNonRT(false);

RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!");
RCLCPP_INFO(logger, "Goal reached, success!");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_success_trajectory_point());
} else if (!within_goal_time) {
const std::string error_string =
"Aborted due to goal_time_tolerance exceeding by " + std::to_string(time_difference) + " seconds";

auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
result->set__error_string(error_string);
active_goal->setAborted(result);
// TODO(matthew-reynolds): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
rt_has_pending_goal_.writeFromNonRT(false);

RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds",
time_difference);
RCLCPP_WARN(logger, error_string.c_str());

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
}
}
} else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) {
// we need to ensure that there is no pending goal -> we get a race condition otherwise
RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation");
RCLCPP_ERROR(logger, "Holding position due to state tolerance violation");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
} else if (!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) {
RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position...");
RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position...");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
Expand Down
Loading