@@ -153,6 +153,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
153
153
time_data.period = rclcpp::Duration::from_nanoseconds (scaling_factor_ * t_period);
154
154
time_data.uptime = time_data_.readFromRT ()->uptime + time_data.period ;
155
155
rclcpp::Time traj_time = time_data_.readFromRT ()->uptime + rclcpp::Duration::from_nanoseconds (t_period);
156
+
156
157
time_data_.writeFromNonRT (time_data);
157
158
158
159
bool first_sample = false ;
@@ -172,6 +173,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
172
173
start_segment_itr, end_segment_itr);
173
174
174
175
if (valid_point) {
176
+ effective_execution_time_ += rclcpp::Duration::from_nanoseconds (t_period);
175
177
const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start ();
176
178
// this is the time instance
177
179
// - 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
180
182
// time_difference is
181
183
// - negative until first point is reached
182
184
// - 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;
184
188
bool tolerance_violated_while_moving = false ;
185
189
bool outside_goal_tolerance = false ;
186
190
bool within_goal_time = true ;
@@ -280,6 +284,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
280
284
281
285
traj_msg_external_point_ptr_.reset ();
282
286
traj_msg_external_point_ptr_.initRT (set_hold_position ());
287
+ effective_execution_time_ = rclcpp::Duration (0 , 0 );
283
288
} else if (!before_last_point) { // check goal tolerance
284
289
if (!outside_goal_tolerance) {
285
290
auto res = std::make_shared<FollowJTrajAction::Result>();
@@ -294,6 +299,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
294
299
295
300
traj_msg_external_point_ptr_.reset ();
296
301
traj_msg_external_point_ptr_.initRT (set_hold_position ());
302
+
303
+ effective_execution_time_ = rclcpp::Duration (0 , 0 );
297
304
} else if (!within_goal_time) {
298
305
auto result = std::make_shared<FollowJTrajAction::Result>();
299
306
result->set__error_code (FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
@@ -308,6 +315,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
308
315
309
316
traj_msg_external_point_ptr_.reset ();
310
317
traj_msg_external_point_ptr_.initRT (set_hold_position ());
318
+ effective_execution_time_ = rclcpp::Duration (0 , 0 );
311
319
}
312
320
}
313
321
} else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT ()) == false ) {
@@ -316,11 +324,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
316
324
317
325
traj_msg_external_point_ptr_.reset ();
318
326
traj_msg_external_point_ptr_.initRT (set_hold_position ());
327
+ effective_execution_time_ = rclcpp::Duration (0 , 0 );
319
328
} else if (!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT ()) == false ) {
320
329
RCLCPP_ERROR (get_node ()->get_logger (), " Exceeded goal_time_tolerance: holding position..." );
321
330
322
331
traj_msg_external_point_ptr_.reset ();
323
332
traj_msg_external_point_ptr_.initRT (set_hold_position ());
333
+ effective_execution_time_ = rclcpp::Duration (0 , 0 );
324
334
}
325
335
// else, run another cycle while waiting for outside_goal_tolerance
326
336
// to be satisfied (will stay in this state until new message arrives)
0 commit comments