Merge branch 'master' into fix/realtimebox

ros-controls · Dec 2, 2024 · cc5beec · cc5beec
2 parents 2edbe33 + 36068e1
commit cc5beec
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 9 deletions.
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -63,7 +63,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v19.1.3
+    rev: v19.1.4
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.4
+    rev: 0.30.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp
@@ -165,7 +165,8 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
     // Time since the last call to update
     const auto period = std::chrono::steady_clock::now() - last_update_time_;
     // Update PIDs
-    double command = pid_->computeCommand(error_position, error_velocity, period.count());
+    double command =
+      pid_->computeCommand(error_position, error_velocity, static_cast<uint64_t>(period.count()));
     command = std::min<double>(
       fabs(max_allowed_effort), std::max<double>(-fabs(max_allowed_effort), command));
     joint_handle_->get().set_value(command);

diff --git a/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -127,6 +127,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   Params params_;
   rclcpp::Duration update_period_{0, 0};
 
+  rclcpp::Time traj_time_;
+
   trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
   /// Specify interpolation method. Default to splines.
   interpolation_methods::InterpolationMethod interpolation_method_{

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -196,15 +196,20 @@ controller_interface::return_type JointTrajectoryController::update(
         traj_external_point_ptr_->set_point_before_trajectory_msg(
           time, state_current_, joints_angle_wraparound_);
       }
+      traj_time_ = time;
+    }
+    else
+    {
+      traj_time_ += period;
     }
 
     // Sample expected state from the trajectory
     traj_external_point_ptr_->sample(
-      time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
+      traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
 
     // Sample setpoint for next control cycle
     const bool valid_point = traj_external_point_ptr_->sample(
-      time + update_period_, interpolation_method_, command_next_, start_segment_itr,
+      traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr,
       end_segment_itr, false);
 
     if (valid_point)
@@ -217,7 +222,7 @@ controller_interface::return_type JointTrajectoryController::update(
       // time_difference is
       // - negative until first point is reached
       // - counting from zero to time_from_start of next point
-      double time_difference = time.seconds() - segment_time_from_start.seconds();
+      double time_difference = traj_time_.seconds() - segment_time_from_start.seconds();
       bool tolerance_violated_while_moving = false;
       bool outside_goal_tolerance = false;
       bool within_goal_time = true;

diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp
@@ -83,12 +83,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest
       {
         // controller hardware cycle update loop
         auto clock = rclcpp::Clock(RCL_STEADY_TIME);
-        auto start_time = clock.now();
+        auto now_time = clock.now();
+        auto last_time = now_time;
         rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0);
-        auto end_time = start_time + wait;
+        auto end_time = last_time + wait;
         while (clock.now() < end_time)
         {
-          traj_controller_->update(clock.now(), clock.now() - start_time);
+          now_time = clock.now();
+          traj_controller_->update(now_time, now_time - last_time);
+          last_time = now_time;
         }
       });