remove the enforce methods of the double vector as it can be handled now by types

Author: saikishor

joint_limits/include/joint_limits/joint_limiter_interface.hpp

@@ -206,19 +206,6 @@ class JointLimiterInterface
     return on_enforce(current_joint_states, desired_joint_states, dt);
   }
 
-  /** \brief Enforce joint limits to desired joint state for single physical quantity.
-   *
-   * Generic enforce method that calls implementation-specific `on_enforce` method.
-   *
-   * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
-   * \returns true if limits are enforced, otherwise false.
-   */
-  JOINT_LIMITS_PUBLIC virtual bool enforce(std::vector<double> & desired_joint_states)
-  {
-    joint_limits_ = *(updated_limits_.readFromRT());
-    return on_enforce(desired_joint_states);
-  }
-
 protected:
   /** \brief Method is realized by an implementation.
    *
@@ -249,18 +236,6 @@ class JointLimiterInterface
     JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0;
 
-  /** \brief Method is realized by an implementation.
-   *
-   * Filter-specific implementation of the joint limits enforce algorithm for single physical
-   * quantity.
-   * This method might use "effort" limits since they are often used as wild-card.
-   * Check the documentation of the exact implementation for more details.
-   *
-   * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
-   * \returns true if limits are enforced, otherwise false.
-   */
-  JOINT_LIMITS_PUBLIC virtual bool on_enforce(std::vector<double> & desired_joint_states) = 0;
-
   /** \brief Checks if the logging interface is set.
    * \returns true if the logging interface is available, otherwise false.
    */

joint_limits/include/joint_limits/joint_range_limiter.hpp

@@ -75,17 +75,6 @@ class JointRangeLimiter : public JointLimiterInterface<LimitsType, JointLimitsSt
     JointLimitsStateDataType & actual, JointLimitsStateDataType & desired,
     const rclcpp::Duration & dt) override;
 
-  /** \brief Enforce joint limits to desired arbitrary physical quantity.
-   * Additional, commonly used method for enforcing limits by clamping desired input value.
-   * The limit is defined in effort limits of the `joint::limits/JointLimit` structure.
-   *
-   * If `has_effort_limits` is set to false, the limits will be not enforced to a joint.
-   *
-   * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the
-   * limits. \returns true if limits are enforced, otherwise false.
-   */
-  JOINT_LIMITS_PUBLIC bool on_enforce(std::vector<double> & /*desired_joint_states*/) override;
-
 private:
   rclcpp::Clock::SharedPtr clock_;
   JointLimitsStateDataType prev_command_;

joint_limits/include/joint_limits/joint_saturation_limiter.hpp

@@ -73,17 +73,6 @@ class JointSaturationLimiter : public JointLimiterInterface<LimitsType, JointLim
     JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override;
 
-  /** \brief Enforce joint limits to desired arbitrary physical quantity.
-   * Additional, commonly used method for enforcing limits by clamping desired input value.
-   * The limit is defined in effort limits of the `joint::limits/JointLimit` structure.
-   *
-   * If `has_effort_limits` is set to false, the limits will be not enforced to a joint.
-   *
-   * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the
-   * limits. \returns true if limits are enforced, otherwise false.
-   */
-  JOINT_LIMITS_PUBLIC bool on_enforce(std::vector<double> & desired_joint_states) override;
-
 private:
   rclcpp::Clock::SharedPtr clock_;
 };

joint_limits/src/joint_range_limiter.cpp

@@ -132,19 +132,6 @@ bool JointRangeLimiter<JointLimits, JointControlInterfacesData>::on_init()
   return result;
 }
 
-template <>
-bool JointRangeLimiter<JointLimits, JointControlInterfacesData>::on_enforce(std::vector<double> &)
-{
-  if (has_logging_interface())
-  {
-    RCLCPP_WARN(
-      node_logging_itf_->get_logger(),
-      "JointRangeLimiter::on_enforce"
-      "(std::vector<double> & desired_joint_states) is not needed for this limiter.");
-  }
-  return false;
-}
-
 template <>
 bool JointRangeLimiter<JointLimits, JointControlInterfacesData>::on_enforce(
   JointControlInterfacesData & actual, JointControlInterfacesData & desired,

joint_limits/src/joint_saturation_limiter.cpp

@@ -427,55 +427,13 @@ bool JointSaturationLimiter<JointLimits, trajectory_msgs::msg::JointTrajectoryPo
   return limits_enforced;
 }
 
-template <>
-bool JointSaturationLimiter<JointLimits, trajectory_msgs::msg::JointTrajectoryPoint>::on_enforce(
-  std::vector<double> & desired_joint_states)
-{
-  std::vector<std::string> limited_joints_effort;
-  bool limits_enforced = false;
-
-  bool has_cmd = (desired_joint_states.size() == number_of_joints_);
-  if (!has_cmd)
-  {
-    return false;
-  }
-
-  for (size_t index = 0; index < number_of_joints_; ++index)
-  {
-    if (joint_limits_[index].has_effort_limits)
-    {
-      double max_effort = joint_limits_[index].max_effort;
-      if (std::fabs(desired_joint_states[index]) > max_effort)
-      {
-        desired_joint_states[index] = std::copysign(max_effort, desired_joint_states[index]);
-        limited_joints_effort.emplace_back(joint_names_[index]);
-        limits_enforced = true;
-      }
-    }
-  }
-
-  if (limited_joints_effort.size() > 0)
-  {
-    std::ostringstream ostr;
-    for (auto jnt : limited_joints_effort)
-    {
-      ostr << jnt << " ";
-    }
-    ostr << "\b \b";  // erase last character
-    RCLCPP_WARN_STREAM_THROTTLE(
-      node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
-      "Joint(s) [" << ostr.str().c_str() << "] would exceed effort limits, limiting");
-  }
-
-  return limits_enforced;
-}
-
 }  // namespace joint_limits
 
 #include "pluginlib/class_list_macros.hpp"
 
 // typedefs are needed here to avoid issues with macro expansion. ref:
 // https://stackoverflow.com/a/8942986
+typedef std::map<int, int> int_map;
 typedef joint_limits::JointSaturationLimiter<
   joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint>
   JointSaturationLimiterTrajectoryPoint;

joint_limits/test/test_joint_saturation_limiter.cpp

@@ -517,48 +517,6 @@ TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_exp
   }
 }
 
-TEST_F(JointSaturationLimiterTest, when_there_are_effort_limits_expect_them_to_be_applyed)
-{
-  SetupNode("joint_saturation_limiter");
-  Load();
-
-  if (joint_limiter_)
-  {
-    Init();
-    Configure();
-
-    // value not over limit
-    desired_joint_states_.effort[0] = 15.0;
-    ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort));
-
-    // value over limit
-    desired_joint_states_.effort[0] = 21.0;
-    ASSERT_TRUE(joint_limiter_->enforce(desired_joint_states_.effort));
-    ASSERT_EQ(desired_joint_states_.effort[0], 20.0);
-  }
-}
-
-TEST_F(JointSaturationLimiterTest, when_there_are_no_effort_limits_expect_them_not_applyed)
-{
-  SetupNode("joint_saturation_limiter");
-  Load();
-
-  if (joint_limiter_)
-  {
-    Init("foo_joint_no_effort");
-    Configure();
-
-    // value not over limit
-    desired_joint_states_.effort[0] = 15.0;
-    ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort));
-
-    // value over limit
-    desired_joint_states_.effort[0] = 21.0;
-    ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort));
-    ASSERT_EQ(desired_joint_states_.effort[0], 21.0);
-  }
-}
-
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);