Merge branch 'master' into improve/chainable_export_interfaces_naming

.github/workflows/jazzy-debian-build.yml

@@ -1,4 +1,4 @@
-name: Debian Rolling Source Build
+name: Debian Jazzy Source Build
 on:
   workflow_dispatch:
   pull_request:

admittance_controller/CMakeLists.txt

@@ -19,7 +19,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   generate_parameter_library
   geometry_msgs
   hardware_interface
-  joint_trajectory_controller
   kinematics_interface
   pluginlib
   rclcpp

admittance_controller/package.xml

@@ -31,7 +31,6 @@
   <depend>generate_parameter_library</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>
-  <depend>joint_trajectory_controller</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -141,8 +141,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
   rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};
 
-  bool is_halted = false;
-
   bool reset();
   void halt();
 

diff_drive_controller/src/diff_drive_controller.cpp

@@ -101,15 +101,6 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   auto logger = get_node()->get_logger();
-  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
-  {
-    if (!is_halted)
-    {
-      halt();
-      is_halted = true;
-    }
-    return controller_interface::return_type::OK;
-  }
 
   const std::shared_ptr<TwistStamped> command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT());
 
@@ -149,15 +140,6 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   auto logger = get_node()->get_logger();
-  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
-  {
-    if (!is_halted)
-    {
-      halt();
-      is_halted = true;
-    }
-    return controller_interface::return_type::OK;
-  }
 
   // command may be limited further by SpeedLimit,
   // without affecting the stored twist command
@@ -335,6 +317,10 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout);
   publish_limited_velocity_ = params_.publish_limited_velocity;
 
+  // Allocate reference interfaces if needed
+  const int nr_ref_itfs = 2;
+  reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits<double>::quiet_NaN());
+
   // TODO(christophfroehlich) remove deprecated parameters
   // START DEPRECATED
   if (!params_.linear.x.has_velocity_limits)
@@ -558,7 +544,6 @@ controller_interface::CallbackReturn DiffDriveController::on_activate(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  is_halted = false;
   subscriber_is_active_ = true;
 
   RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active.");
@@ -569,11 +554,7 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate(
   const rclcpp_lifecycle::State &)
 {
   subscriber_is_active_ = false;
-  if (!is_halted)
-  {
-    halt();
-    is_halted = true;
-  }
+  halt();
   reset_buffers();
   registered_left_wheel_handles_.clear();
   registered_right_wheel_handles_.clear();
@@ -612,13 +593,14 @@ bool DiffDriveController::reset()
   subscriber_is_active_ = false;
   velocity_command_subscriber_.reset();
 
-  is_halted = false;
   return true;
 }
 
 void DiffDriveController::reset_buffers()
 {
-  reference_interfaces_ = std::vector<double>(2, std::numeric_limits<double>::quiet_NaN());
+  std::fill(
+    reference_interfaces_.begin(), reference_interfaces_.end(),
+    std::numeric_limits<double>::quiet_NaN());
   // Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations.
   std::queue<std::array<double, 2>> empty;
   std::swap(previous_two_commands_, empty);
@@ -714,10 +696,8 @@ bool DiffDriveController::on_set_chained_mode(bool chained_mode)
 std::vector<hardware_interface::CommandInterface>
 DiffDriveController::on_export_reference_interfaces()
 {
-  const int nr_ref_itfs = 2;
-  reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits<double>::quiet_NaN());
   std::vector<hardware_interface::CommandInterface> reference_interfaces;
-  reference_interfaces.reserve(nr_ref_itfs);
+  reference_interfaces.reserve(reference_interfaces_.size());
 
   reference_interfaces.push_back(hardware_interface::CommandInterface(
     get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY,

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -719,11 +719,16 @@ TEST_F(TestDiffDriveController, cleanup)
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
+  // should be moving
+  EXPECT_LT(0.0, left_wheel_vel_cmd_.get_value());
+  EXPECT_LT(0.0, right_wheel_vel_cmd_.get_value());
+
   state = controller_->get_node()->deactivate();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  ASSERT_EQ(
-    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
+
+  // should be stopped
+  EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()";
+  EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()";
 
   state = controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());

forward_command_controller/src/forward_command_controller.cpp

@@ -31,11 +31,6 @@ void ForwardCommandController::declare_parameters()
 
 controller_interface::CallbackReturn ForwardCommandController::read_parameters()
 {
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   if (params_.joints.empty())

forward_command_controller/src/multi_interface_forward_command_controller.cpp

@@ -32,11 +32,6 @@ void MultiInterfaceForwardCommandController::declare_parameters()
 
 controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters()
 {
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   if (params_.joint.empty())

gpio_controllers/src/gpio_command_controller.cpp

@@ -158,11 +158,6 @@ controller_interface::return_type GpioCommandController::update(
 bool GpioCommandController::update_dynamic_map_parameters()
 {
   auto logger = get_node()->get_logger();
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(logger, "Error encountered during init");
-    return false;
-  }
   // update the dynamic map parameters
   param_listener_->refresh_dynamic_parameters();
   // get parameters from the listener in case they were updated

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -206,11 +206,6 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
   const rclcpp_lifecycle::State &)
 {
   const auto logger = get_node()->get_logger();
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   // Action status checking update rate

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -90,11 +90,6 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf
 controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   if (use_all_available_interfaces())

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -149,23 +149,6 @@ void JointStateBroadcasterTest::assign_state_interfaces(
   state_broadcaster_->assign_interfaces({}, std::move(state_ifs));
 }
 
-TEST_F(JointStateBroadcasterTest, ConfigureErrorTest)
-{
-  // publishers not initialized yet
-  ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_);
-  ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
-
-  // configure failed
-  ASSERT_THROW(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), std::exception);
-
-  SetUpStateBroadcaster();
-  // check state remains unchanged
-
-  // publishers still not initialized
-  ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_);
-  ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
-}
-
 TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
 {
   // publishers not initialized yet

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -144,10 +144,6 @@ JointTrajectoryController::state_interface_configuration() const
 controller_interface::return_type JointTrajectoryController::update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (get_lifecycle_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_))
@@ -655,12 +651,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
 {
   auto logger = get_node()->get_logger();
 
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(logger, "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
-
   // update the dynamic map parameters
   param_listener_->refresh_dynamic_parameters();
 

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -1020,9 +1020,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_acti
   auto state_ref = traj_controller_->get_state_reference();
   auto state = traj_controller_->get_state_feedback();
 
-  // run an update
-  updateControllerAsync(rclcpp::Duration::from_seconds(0.01));
-
   // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint
   // method.
   if (traj_controller_->has_position_command_interface())

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -46,39 +46,6 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_robot_description)
     SetUpTrajectoryControllerLocal({}, "<invalid_robot_description/>"));
 }
 
-TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  SetUpTrajectoryController(executor);
-  traj_controller_->get_node()->set_parameter(
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
-
-  const auto state = traj_controller_->configure();
-  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
-
-  // send msg
-  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
-  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
-  // *INDENT-OFF*
-  std::vector<std::vector<double>> points{
-    {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
-  std::vector<std::vector<double>> points_velocities{
-    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
-  // *INDENT-ON*
-  publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
-  traj_controller_->wait_for_trajectory(executor);
-
-  traj_controller_->update(
-    rclcpp::Time(static_cast<uint64_t>(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5));
-
-  // hw position == 0 because controller is not activated
-  EXPECT_EQ(0.0, joint_pos_[0]);
-  EXPECT_EQ(0.0, joint_pos_[1]);
-  EXPECT_EQ(0.0, joint_pos_[2]);
-
-  executor.cancel();
-}
-
 TEST_P(TrajectoryControllerTestParameterized, check_interface_names)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
@@ -148,7 +115,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup)
 
   auto state = traj_controller_->get_node()->deactivate();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
 
   state = traj_controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
@@ -223,8 +189,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
   std::vector<double> deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]};
   state = traj_controller_->get_node()->deactivate();
   ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
+
   // it should be holding the current point
-  traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
   expectHoldingPointDeactivated(deactivated_positions);
 
   // reactivate

parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp

@@ -226,11 +226,6 @@ controller_interface::CallbackReturn GripperActionController::on_configure(
   const rclcpp_lifecycle::State &)
 {
   const auto logger = get_node()->get_logger();
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   // Action status checking update rate

parallel_gripper_controller/test/test_parallel_gripper_controller.cpp

@@ -67,23 +67,13 @@ TEST_F(GripperControllerTest, ParametersNotSet)
   this->SetUpController(
     "test_gripper_action_position_controller_no_parameters",
     controller_interface::return_type::ERROR);
-
-  // configure failed, 'joints' parameter not set
-  ASSERT_EQ(
-    this->controller_->on_configure(rclcpp_lifecycle::State()),
-    controller_interface::CallbackReturn::ERROR);
 }
 
 TEST_F(GripperControllerTest, JointParameterIsEmpty)
 {
   this->SetUpController(
     "test_gripper_action_position_controller_empty_joint",
     controller_interface::return_type::ERROR);
-
-  // configure failed, 'joints' is empty
-  ASSERT_EQ(
-    this->controller_->on_configure(rclcpp_lifecycle::State()),
-    controller_interface::CallbackReturn::ERROR);
 }
 
 TEST_F(GripperControllerTest, ConfigureParamsSuccess)

steering_controllers_library/src/steering_odometry.cpp

@@ -17,6 +17,8 @@
  * Author: Dr. Ing. Denis Stogl
  */
 
+#define _USE_MATH_DEFINES
+
 #include "steering_controllers_library/steering_odometry.hpp"
 
 #include <cmath>

steering_controllers_library/test/test_steering_odometry.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#define _USE_MATH_DEFINES
+
 #include <gmock/gmock.h>
 
 #include "steering_controllers_library/steering_odometry.hpp"

tricycle_controller/include/tricycle_controller/tricycle_controller.hpp

@@ -135,8 +135,6 @@ class TricycleController : public controller_interface::ControllerInterface
   TractionLimiter limiter_traction_;
   SteeringLimiter limiter_steering_;
 
-  bool is_halted = false;
-
   void reset_odometry(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<std_srvs::srv::Empty::Request> req,