Merge branch 'master' into feature/swerve-drive-controller

Author: nitin2606

.github/workflows/rolling-semi-binary-build-win.yml

@@ -27,6 +27,6 @@ jobs:
     uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-win-build.yml@master
     with:
       ros_distro: rolling
-      pixi_dependencies: typeguard jinja2 boost compilers
+      pixi_dependencies: typeguard jinja2 boost compilers octomap
       ninja_packages: rsl
       target_cmake_args: -DBUILD_TESTING=OFF

doc/migration.rst

@@ -4,6 +4,11 @@ Migration Guides: Kilted Kaiju to Lyrical Luth
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases, where changes to user code might be necessary.
 
+joint_state_broadcaster
+*****************************
+* Removed interfaces with other data types than double for publishing to ``dynamic_joint_states``. (`#2115 <https://github.com/ros-controls/ros2_controllers/pull/2115>`_).
+  Use a custom controller for publishing non-double interfaces.
+
 effort_controllers
 *****************************
 * ``effort_controllers/JointGroupEffortController`` is deprecated. Use :ref:`forward_command_controller <forward_command_controller_userdoc>` instead by adding the ``interface_name`` parameter and set it to ``effort``. (`#1913 <https://github.com/ros-controls/ros2_controllers/pull/1913>`_).

doc/release_notes.rst

@@ -27,6 +27,7 @@ joint_state_broadcaster
 ************************
 * Make all parameters read-only (the never got re-evaluated after initialization anyways). (`#2064 <https://github.com/ros-controls/ros2_controllers/pull/2064>`_)
 * Added parameter ``publish_dynamic_joint_states`` to enable/disable publishing of dynamic joint states. (`#2064 <https://github.com/ros-controls/ros2_controllers/pull/2064>`_)
+* Removed interfaces with other data types than double for publishing to ``dynamic_joint_states``. (`#2115 <https://github.com/ros-controls/ros2_controllers/pull/2115>`_)
 
 omni_wheel_drive_controller
 *****************************

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -236,6 +236,14 @@ bool JointStateBroadcaster::init_joint_data()
     HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT};
   for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++)
   {
+    if (si->get_data_type() != hardware_interface::HandleDataType::DOUBLE)
+    {
+      RCLCPP_WARN(
+        get_node()->get_logger(),
+        "State interface '%s' of joint '%s' has non-double data type and will be ignored.",
+        si->get_interface_name().c_str(), si->get_prefix_name().c_str());
+      continue;
+    }
     const std::string prefix_name = si->get_prefix_name();
     // initialize map if name is new
     if (name_if_value_mapping_.count(prefix_name) == 0)
@@ -305,6 +313,10 @@ void JointStateBroadcaster::init_auxiliary_data()
   mapped_values_.clear();
   for (auto i = 0u; i < state_interfaces_.size(); ++i)
   {
+    if (state_interfaces_[i].get_data_type() != hardware_interface::HandleDataType::DOUBLE)
+    {
+      continue;
+    }
     std::string interface_name = state_interfaces_[i].get_interface_name();
     if (map_interface_to_joint_state_.count(interface_name) > 0)
     {
@@ -399,13 +411,17 @@ bool JointStateBroadcaster::use_all_available_interfaces() const
 controller_interface::return_type JointStateBroadcaster::update(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
+  size_t map_index = 0u;
   for (auto i = 0u; i < state_interfaces_.size(); ++i)
   {
-    // no retries, just try to get the latest value once
-    const auto & opt = state_interfaces_[i].get_optional(0);
-    if (opt.has_value())
+    if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::DOUBLE)
     {
-      *mapped_values_[i] = opt.value();
+      // no retries, just try to get the latest value once
+      const auto & opt = state_interfaces_[i].get_optional(0);
+      if (opt.has_value())
+      {
+        *mapped_values_[map_index++] = opt.value();
+      }
     }
   }
 

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -1324,3 +1324,68 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest)
   ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
   ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
 }
+
+TEST_F(JointStateBroadcasterTest, NoThrowWithBooleanInterfaceTest)
+{
+  const std::string JOINT_NAME = joint_names_[0];
+  const std::string IF_NAME = "is_moving";
+  SetUpStateBroadcaster({JOINT_NAME}, {IF_NAME});
+
+  init_broadcaster_and_set_parameters("", {JOINT_NAME}, {IF_NAME});
+
+  std::vector<LoanedStateInterface> state_ifs;
+  state_ifs.emplace_back(joint_1_moving_state_);
+  state_broadcaster_->assign_interfaces({}, std::move(state_ifs));
+
+  // configure and activate ok
+  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // update should not throw
+  ASSERT_NO_THROW(
+    state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+
+  const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_;
+  ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, IsEmpty());
+}
+
+TEST_F(JointStateBroadcasterTest, NoThrowWithBooleanAndDoubleInterfaceTest)
+{
+  const std::string JOINT_NAME = joint_names_[0];
+  const std::string IF_NAME = "is_moving";
+  SetUpStateBroadcaster({JOINT_NAME}, {IF_NAME});
+
+  init_broadcaster_and_set_parameters("", {JOINT_NAME}, {IF_NAME});
+
+  std::vector<LoanedStateInterface> state_ifs;
+  state_ifs.emplace_back(joint_1_moving_state_);
+  state_ifs.emplace_back(joint_1_pos_state_);
+  state_ifs.emplace_back(joint_1_vel_state_);
+  state_ifs.emplace_back(joint_1_eff_state_);
+  state_broadcaster_->assign_interfaces({}, std::move(state_ifs));
+
+  // configure and activate ok
+  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // update should not throw
+  ASSERT_NO_THROW(
+    state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+
+  const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_;
+  ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(1))
+    << "Boolean interface should be skipped";
+  ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(1));
+
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[0].interface_names,
+    ElementsAreArray({HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}));
+
+  // joint states
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray({joint_names_[0]}));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(1));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(1));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(1));
+}

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -50,6 +50,8 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr
   FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMapping);
   FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate);
   FRIEND_TEST(JointStateBroadcasterTest, ExtraJointStatePublishTest);
+  FRIEND_TEST(JointStateBroadcasterTest, NoThrowWithBooleanInterfaceTest);
+  FRIEND_TEST(JointStateBroadcasterTest, NoThrowWithBooleanAndDoubleInterfaceTest);
 };
 
 class JointStateBroadcasterTest : public ::testing::Test
@@ -122,6 +124,10 @@ class JointStateBroadcasterTest : public ::testing::Test
     std::make_shared<hardware_interface::StateInterface>(
       joint_names_[0], custom_interface_name_, &custom_joint_value_);
 
+  hardware_interface::StateInterface::SharedPtr joint_1_moving_state_ =
+    std::make_shared<hardware_interface::StateInterface>(
+      joint_names_[0], "is_moving", "bool", "false");
+
   std::vector<hardware_interface::StateInterface::SharedPtr> test_interfaces_;
 
   std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;

joint_trajectory_controller/CMakeLists.txt

@@ -37,6 +37,7 @@ generate_parameter_library(joint_trajectory_controller_parameters
 add_library(joint_trajectory_controller SHARED
   src/joint_trajectory_controller.cpp
   src/trajectory.cpp
+  src/validate_jtc_parameters.cpp
 )
 target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17)
 if(WIN32)

joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp

@@ -24,79 +24,26 @@
 
 namespace joint_trajectory_controller
 {
-tl::expected<void, std::string> command_interface_type_combinations(
-  rclcpp::Parameter const & parameter)
-{
-  auto const & interface_types = parameter.as_string_array();
-
-  // Check if command interfaces combination is valid. Valid combinations are:
-  // 1. effort
-  // 2. velocity
-  // 3. position [velocity, [acceleration]]
-  // 4. position, effort
-
-  if (
-    rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
-    interface_types.size() > 1 &&
-    !rsl::contains<std::vector<std::string>>(interface_types, "position"))
-  {
-    return tl::make_unexpected(
-      "'velocity' command interface can be used either alone or 'position' "
-      "command interface has to be present");
-  }
-
-  if (
-    rsl::contains<std::vector<std::string>>(interface_types, "acceleration") &&
-    (!rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
-     !rsl::contains<std::vector<std::string>>(interface_types, "position")))
-  {
-    return tl::make_unexpected(
-      "'acceleration' command interface can only be used if 'velocity' and "
-      "'position' command interfaces are present");
-  }
-
-  if (
-    rsl::contains<std::vector<std::string>>(interface_types, "effort") &&
-    !(interface_types.size() == 1 ||
-      (interface_types.size() == 2 &&
-       rsl::contains<std::vector<std::string>>(interface_types, "position"))))
-  {
-    return tl::make_unexpected(
-      "'effort' command interface has to be used alone or with a 'position' interface");
-  }
-
-  return {};
-}
 
+/**
+ * \brief Validate command interface type combinations for joint trajectory controller.
+ *
+ * \param[in] parameter The rclcpp parameter containing the command interface types.
+ * \return tl::expected<void, std::string> An empty expected on success, or an error message on
+ * failure.
+ */
+tl::expected<void, std::string> command_interface_type_combinations(
+  rclcpp::Parameter const & parameter);
+
+/**
+ * \brief Validate state interface type combinations for joint trajectory controller.
+ *
+ * \param[in] parameter The rclcpp parameter containing the state interface types.
+ * \return tl::expected<void, std::string> An empty expected on success, or an error message on
+ * failure.
+ */
 tl::expected<void, std::string> state_interface_type_combinations(
-  rclcpp::Parameter const & parameter)
-{
-  auto const & interface_types = parameter.as_string_array();
-
-  // Valid combinations are
-  // 1. position [velocity, [acceleration]]
-
-  if (
-    rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
-    !rsl::contains<std::vector<std::string>>(interface_types, "position"))
-  {
-    return tl::make_unexpected(
-      "'velocity' state interface cannot be used if 'position' interface "
-      "is missing.");
-  }
-
-  if (
-    rsl::contains<std::vector<std::string>>(interface_types, "acceleration") &&
-    (!rsl::contains<std::vector<std::string>>(interface_types, "position") ||
-     !rsl::contains<std::vector<std::string>>(interface_types, "velocity")))
-  {
-    return tl::make_unexpected(
-      "'acceleration' state interface cannot be used if 'position' and 'velocity' "
-      "interfaces are not present.");
-  }
-
-  return {};
-}
+  rclcpp::Parameter const & parameter);
 
 }  // namespace joint_trajectory_controller
 

joint_trajectory_controller/src/validate_jtc_parameters.cpp

@@ -0,0 +1,94 @@
+// Copyright (c) 2022 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "joint_trajectory_controller/validate_jtc_parameters.hpp"
+
+namespace joint_trajectory_controller
+{
+
+tl::expected<void, std::string> command_interface_type_combinations(
+  rclcpp::Parameter const & parameter)
+{
+  auto const & interface_types = parameter.as_string_array();
+
+  // Check if command interfaces combination is valid. Valid combinations are:
+  // 1. effort
+  // 2. velocity
+  // 3. position [velocity, [acceleration]]
+  // 4. position, effort
+
+  if (
+    rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
+    interface_types.size() > 1 &&
+    !rsl::contains<std::vector<std::string>>(interface_types, "position"))
+  {
+    return tl::make_unexpected(
+      "'velocity' command interface can be used either alone or 'position' "
+      "command interface has to be present");
+  }
+
+  if (
+    rsl::contains<std::vector<std::string>>(interface_types, "acceleration") &&
+    (!rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
+     !rsl::contains<std::vector<std::string>>(interface_types, "position")))
+  {
+    return tl::make_unexpected(
+      "'acceleration' command interface can only be used if 'velocity' and "
+      "'position' command interfaces are present");
+  }
+
+  if (
+    rsl::contains<std::vector<std::string>>(interface_types, "effort") &&
+    !(interface_types.size() == 1 ||
+      (interface_types.size() == 2 &&
+       rsl::contains<std::vector<std::string>>(interface_types, "position"))))
+  {
+    return tl::make_unexpected(
+      "'effort' command interface has to be used alone or with a 'position' interface");
+  }
+
+  return {};
+}
+
+tl::expected<void, std::string> state_interface_type_combinations(
+  rclcpp::Parameter const & parameter)
+{
+  auto const & interface_types = parameter.as_string_array();
+
+  // Valid combinations are
+  // 1. position [velocity, [acceleration]]
+
+  if (
+    rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
+    !rsl::contains<std::vector<std::string>>(interface_types, "position"))
+  {
+    return tl::make_unexpected(
+      "'velocity' state interface cannot be used if 'position' interface "
+      "is missing.");
+  }
+
+  if (
+    rsl::contains<std::vector<std::string>>(interface_types, "acceleration") &&
+    (!rsl::contains<std::vector<std::string>>(interface_types, "position") ||
+     !rsl::contains<std::vector<std::string>>(interface_types, "velocity")))
+  {
+    return tl::make_unexpected(
+      "'acceleration' state interface cannot be used if 'position' and 'velocity' "
+      "interfaces are not present.");
+  }
+
+  return {};
+}
+
+}  // namespace joint_trajectory_controller

steering_controllers_library/src/steering_controllers_library.cpp

@@ -290,14 +290,15 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
   odom_state_msg_.child_frame_id = base_frame_id;
   odom_state_msg_.pose.pose.position.z = 0;
 
-  auto & covariance = odom_state_msg_.twist.covariance;
-  constexpr size_t NUM_DIMENSIONS = 6;
-  for (size_t index = 0; index < 6; ++index)
-  {
-    // 0, 7, 14, 21, 28, 35
-    const size_t diagonal_index = NUM_DIMENSIONS * index + index;
-    covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
-    covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
+  const size_t NUM_DIMENSIONS = 6;
+  auto & pose_cov = odom_state_msg_.pose.covariance;
+  auto & twist_cov = odom_state_msg_.twist.covariance;
+  for (size_t i = 0; i < NUM_DIMENSIONS; ++i)
+  {
+    // indices of the diagonal: 0, 7, 14, 21, 28, 35
+    const size_t index = (NUM_DIMENSIONS + 1) * i;
+    pose_cov[index] = params_.pose_covariance_diagonal[i];
+    twist_cov[index] = params_.twist_covariance_diagonal[i];
   }
 
   try