[JTC] Reject messages with effort fields (#699) (#719) (#738)

ros-controls · Aug 7, 2023 · 6a4e7a6 · 6a4e7a6
1 parent 862928c
commit 6a4e7a6
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 16 deletions.
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -292,14 +292,7 @@ controller_interface::return_type JointTrajectoryController::update(
         }
         if (has_effort_command_interface_)
         {
-          if (use_closed_loop_pid_adapter_)
-          {
-            assign_interface_from_point(joint_command_interface_[3], tmp_command_);
-          }
-          else
-          {
-            assign_interface_from_point(joint_command_interface_[3], state_desired_.effort);
-          }
+          assign_interface_from_point(joint_command_interface_[3], tmp_command_);
         }
 
         // store the previous command. Used in open-loop control mode
@@ -1364,8 +1357,9 @@ bool JointTrajectoryController::validate_trajectory_point_field(
   if (joint_names_size != vector_field.size())
   {
     RCLCPP_ERROR(
-      get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.",
-      joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i);
+      get_node()->get_logger(),
+      "Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.", joint_names_size,
+      string_for_vector_field.c_str(), vector_field.size(), i);
     return false;
   }
   return true;
@@ -1481,11 +1475,17 @@ bool JointTrajectoryController::validate_trajectory_msg(
       !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) ||
       !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) ||
       !validate_trajectory_point_field(
-        joint_count, points[i].accelerations, "accelerations", i, true) ||
-      !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true))
+        joint_count, points[i].accelerations, "accelerations", i, true))
     {
       return false;
     }
+    // reject effort entries
+    if (!points[i].effort.empty())
+    {
+      RCLCPP_ERROR(
+        get_node()->get_logger(), "Trajectories with effort fields are currently not supported.");
+      return false;
+    }
   }
   return true;
 }

diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -1110,13 +1110,10 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message)
   traj_msg.points[0].accelerations = {1.0, 2.0};
   EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
 
-// Effort is not supported in trajectory message
-#if 0
-  // TODO(christophfroehlich) activate with #730
+  // Effort is not supported in trajectory message
   traj_msg = good_traj_msg;
   traj_msg.points[0].effort = {1.0, 2.0, 3.0};
   EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-#endif
 
   // Non-strictly increasing waypoint times
   traj_msg = good_traj_msg;