Pull upstream/ros2-master commits

Author: ViktorCVS

control_toolbox/CHANGELOG.rst

@@ -357,9 +357,9 @@ Changelog for package control_toolbox
 * catkinizing, could still be cleaned up
 * Fixing doc errors in PID
 * Changing @ commands to \ commands
-* Enforcing i_min_ <= 0 and i_max_ >= 0 in integral bound parameters, reducing duplicated code
+* Enforcing ``i_min_`` <= 0 and ``i_max_`` >= 0 in integral bound parameters, reducing duplicated code
 * Merge pull request `#14 <https://github.com/ros-controls/control_toolbox/issues/14>`_ from bobholmberg/fix-PID-unbounded-i_error
-  Using zero i_gain_ to turn off integral control did unsavory things.
+  Using zero ``i_gain_`` to turn off integral control did unsavory things.
 * Adding alternative name for new pid command computation API
 * Fixing merge error
 * Merge branch 'fix-pid-backwards-compatibility' into fix-PID-unbounded-i_error
@@ -376,13 +376,13 @@ Changelog for package control_toolbox
 * Specifying div-by-zero test, adding other integral term tests
 * Merge branch 'test-bad-integral-bounds' into fix-PID-unbounded-i_error
 * Adding test to expose Pid class zero-division vulnerability
-* If the user did not want integral control and set i_gain_ to zero,
-  then dividing by i_gain_ would set i_error_ to NaN.  This is not
-  desired.  Instead, replace the use of division to create i_term
-  with direct integration of i_term_.
-  Replace private member i_error_ with i_term_.
-  In getCurrentPIDErrors() create & return i_error_ with the same old meaning and units.
-  NOTE: i_error_ is not needed internally anywhere else.
+* If the user did not want integral control and set ``i_gain_`` to zero,
+  then dividing by ``i_gain_`` would set ``i_error_`` to NaN.  This is not
+  desired.  Instead, replace the use of division to create ``i_term``
+  with direct integration of ``i_term_``.
+  Replace private member ``i_error_`` with ``i_term_``.
+  In ``getCurrentPIDErrors()`` create & return ``i_error_`` with the same old meaning and units.
+  NOTE: ``i_error_`` is not needed internally anywhere else.
 * Cleaning up documentation, making argument names in function declaration match those in the implementation
 * adding doxygen deprecation flags
 * Fixing documentation

control_toolbox/include/control_filters/gravity_compensation.hpp

@@ -22,8 +22,8 @@
 
 #include "filters/filter_base.hpp"
 #include "geometry_msgs/msg/vector3_stamped.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 
 #include "control_toolbox/gravity_compensation_filter_parameters.hpp"
 

control_toolbox/src/pid_ros.cpp

@@ -275,8 +275,17 @@ bool PidROS::initialize_from_ros_parameters()
   all_params_available &=
     get_double_param(param_prefix_ + "tracking_time_constant", tracking_time_constant);
 
+  bool saturation = std::isfinite(u_max) || std::isfinite(u_min);
+  get_boolean_param(param_prefix_ + "saturation", saturation);
+  if (!saturation)
+  {
+    u_max = MAX_INFINITY;
+    u_min = -MAX_INFINITY;
+  }
   get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str);
   declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false));
+  declare_param(
+    param_prefix_ + "activate_state_publisher", rclcpp::ParameterValue(rt_state_pub_ != nullptr));
 
   if (all_params_available)
   {
@@ -348,7 +357,9 @@ bool PidROS::initialize_from_args(
         rclcpp::ParameterValue(antiwindup_strat.tracking_time_constant));
       declare_param(
         param_prefix_ + "error_deadband", rclcpp::ParameterValue(antiwindup_strat.error_deadband));
-      declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true));
+      declare_param(
+        param_prefix_ + "saturation",
+        rclcpp::ParameterValue(std::isfinite(gains.u_max_) || std::isfinite(gains.u_min_)));
       declare_param(
         param_prefix_ + "antiwindup_strategy",
         rclcpp::ParameterValue(gains.antiwindup_strat_.to_string()));

control_toolbox/test/control_filters/test_gravity_compensation.cpp

@@ -12,19 +12,24 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_gravity_compensation.hpp"
+#include "test_filter_util.hpp"
 
+#include <memory>
 #include <vector>
 
-TEST_F(GravityCompensationTest, TestGravityCompensationFilterThrowsUnconfigured)
+#include "control_filters/gravity_compensation.hpp"
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "gmock/gmock.h"
+
+TEST_F(FilterTest, TestGravityCompensationFilterThrowsUnconfigured)
 {
   std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
     std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();
   geometry_msgs::msg::WrenchStamped in, out;
   ASSERT_THROW(filter_->update(in, out), std::runtime_error);
 }
 
-TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters)
+TEST_F(FilterTest, TestGravityCompensationMissingParameters)
 {
   std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
     std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();
@@ -40,7 +45,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters)
    */
 }
 
-TEST_F(GravityCompensationTest, TestGravityCompensationInvalidThenFixedParameter)
+TEST_F(FilterTest, TestGravityCompensationInvalidThenFixedParameter)
 {
   std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
     std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();
@@ -67,7 +72,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensationInvalidThenFixedParameter
     node_->get_node_parameters_interface()));
 }
 
-TEST_F(GravityCompensationTest, TestGravityCompensationMissingTransform)
+TEST_F(FilterTest, TestGravityCompensationMissingTransform)
 {
   std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
     std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();
@@ -86,7 +91,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingTransform)
   ASSERT_FALSE(filter_->update(in, out));
 }
 
-TEST_F(GravityCompensationTest, TestGravityCompensationComputation)
+TEST_F(FilterTest, TestGravityCompensationComputation)
 {
   std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
     std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();

control_toolbox/test/control_filters/test_gravity_compensation.hpp

@@ -572,22 +572,22 @@ TEST(PidParametersTest, GetParametersTest)
 TEST(PidParametersTest, GetParametersFromParams)
 {
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");
-
-  TestablePidROS pid(node, "", "", false);
+  const bool ACTIVATE_STATE_PUBLISHER = false;
+  TestablePidROS pid(node, "", "", ACTIVATE_STATE_PUBLISHER);
 
   ASSERT_TRUE(pid.initialize_from_ros_parameters());
 
   rclcpp::Parameter param_p;
   ASSERT_TRUE(node->get_parameter("p", param_p));
-  ASSERT_TRUE(std::isnan(param_p.get_value<double>()));
+  EXPECT_TRUE(std::isnan(param_p.get_value<double>()));
 
   rclcpp::Parameter param_i;
   ASSERT_TRUE(node->get_parameter("i", param_i));
-  ASSERT_TRUE(std::isnan(param_i.get_value<double>()));
+  EXPECT_TRUE(std::isnan(param_i.get_value<double>()));
 
   rclcpp::Parameter param_d;
   ASSERT_TRUE(node->get_parameter("d", param_d));
-  ASSERT_TRUE(std::isnan(param_d.get_value<double>()));
+  EXPECT_TRUE(std::isnan(param_d.get_value<double>()));
 
   rclcpp::Parameter param_i_clamp_max;
   ASSERT_TRUE(node->get_parameter("i_clamp_max", param_i_clamp_max));
@@ -599,15 +599,23 @@ TEST(PidParametersTest, GetParametersFromParams)
 
   rclcpp::Parameter param_u_clamp_max;
   ASSERT_TRUE(node->get_parameter("u_clamp_max", param_u_clamp_max));
-  ASSERT_TRUE(std::isinf(param_u_clamp_max.get_value<double>()));
+  EXPECT_TRUE(std::isinf(param_u_clamp_max.get_value<double>()));
 
   rclcpp::Parameter param_u_clamp_min;
   ASSERT_TRUE(node->get_parameter("u_clamp_min", param_u_clamp_min));
-  ASSERT_TRUE(std::isinf(param_u_clamp_min.get_value<double>()));
+  EXPECT_TRUE(std::isinf(param_u_clamp_min.get_value<double>()));
+
+  rclcpp::Parameter param_saturation;
+  ASSERT_TRUE(node->get_parameter("saturation", param_saturation));
+  EXPECT_FALSE(param_saturation.get_value<bool>());
 
   rclcpp::Parameter param_tracking_time_constant;
   ASSERT_TRUE(node->get_parameter("tracking_time_constant", param_tracking_time_constant));
-  ASSERT_TRUE(std::isnan(param_tracking_time_constant.get_value<double>()));
+  EXPECT_TRUE(std::isnan(param_tracking_time_constant.get_value<double>()));
+
+  rclcpp::Parameter param_activate_state_publisher;
+  ASSERT_TRUE(node->get_parameter("activate_state_publisher", param_activate_state_publisher));
+  EXPECT_EQ(param_activate_state_publisher.get_value<bool>(), ACTIVATE_STATE_PUBLISHER);
 }
 
 TEST(PidParametersTest, MultiplePidInstances)