Skip to content

Commit 260c1c0

Browse files
author
Lennart Nachtigall
committed
Fix for goaltime tolerance
1 parent 2d250a2 commit 260c1c0

File tree

2 files changed

+13
-1
lines changed

2 files changed

+13
-1
lines changed

ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,8 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
8484
scaled_joint_trajectory_controller::Params scaled_params_;
8585

8686
rclcpp::Subscription<ScalingFactorMsg>::SharedPtr scaling_factor_sub_;
87+
88+
rclcpp::Duration effective_execution_time_{ 0, 0 };
8789
};
8890
} // namespace ur_controllers
8991

ur_controllers/src/scaled_joint_trajectory_controller.cpp

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -153,6 +153,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
153153
time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * t_period);
154154
time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
155155
rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period);
156+
156157
time_data_.writeFromNonRT(time_data);
157158

158159
bool first_sample = false;
@@ -172,6 +173,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
172173
start_segment_itr, end_segment_itr);
173174

174175
if (valid_point) {
176+
effective_execution_time_ += rclcpp::Duration::from_nanoseconds(t_period);
175177
const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start();
176178
// this is the time instance
177179
// - started with the first segment: when the first point will be reached (in the future)
@@ -180,7 +182,9 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
180182
// time_difference is
181183
// - negative until first point is reached
182184
// - counting from zero to time_from_start of next point
183-
double time_difference = time.seconds() - segment_time_from_start.seconds();
185+
const auto time_correction =
186+
effective_execution_time_.seconds() - (segment_time_from_start.seconds() - traj_start.seconds());
187+
double time_difference = time.seconds() - segment_time_from_start.seconds() + time_correction;
184188
bool tolerance_violated_while_moving = false;
185189
bool outside_goal_tolerance = false;
186190
bool within_goal_time = true;
@@ -280,6 +284,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
280284

281285
traj_msg_external_point_ptr_.reset();
282286
traj_msg_external_point_ptr_.initRT(set_hold_position());
287+
effective_execution_time_ = rclcpp::Duration(0, 0);
283288
} else if (!before_last_point) { // check goal tolerance
284289
if (!outside_goal_tolerance) {
285290
auto res = std::make_shared<FollowJTrajAction::Result>();
@@ -294,6 +299,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
294299

295300
traj_msg_external_point_ptr_.reset();
296301
traj_msg_external_point_ptr_.initRT(set_hold_position());
302+
303+
effective_execution_time_ = rclcpp::Duration(0, 0);
297304
} else if (!within_goal_time) {
298305
auto result = std::make_shared<FollowJTrajAction::Result>();
299306
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
@@ -308,6 +315,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
308315

309316
traj_msg_external_point_ptr_.reset();
310317
traj_msg_external_point_ptr_.initRT(set_hold_position());
318+
effective_execution_time_ = rclcpp::Duration(0, 0);
311319
}
312320
}
313321
} else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) {
@@ -316,11 +324,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
316324

317325
traj_msg_external_point_ptr_.reset();
318326
traj_msg_external_point_ptr_.initRT(set_hold_position());
327+
effective_execution_time_ = rclcpp::Duration(0, 0);
319328
} else if (!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) {
320329
RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position...");
321330

322331
traj_msg_external_point_ptr_.reset();
323332
traj_msg_external_point_ptr_.initRT(set_hold_position());
333+
effective_execution_time_ = rclcpp::Duration(0, 0);
324334
}
325335
// else, run another cycle while waiting for outside_goal_tolerance
326336
// to be satisfied (will stay in this state until new message arrives)

0 commit comments

Comments
 (0)