Extend joint limits structure with deceleration limits. (#977)

joint_limits/include/joint_limits/joint_limits.hpp

@@ -38,11 +38,13 @@ struct JointLimits
  max_position(std::numeric_limits<double>::quiet_NaN()),
  max_velocity(std::numeric_limits<double>::quiet_NaN()),
  max_acceleration(std::numeric_limits<double>::quiet_NaN()),
+ max_deceleration(std::numeric_limits<double>::quiet_NaN()),
  max_jerk(std::numeric_limits<double>::quiet_NaN()),
  max_effort(std::numeric_limits<double>::quiet_NaN()),
  has_position_limits(false),
  has_velocity_limits(false),
  has_acceleration_limits(false),
+ has_deceleration_limits(false),
  has_jerk_limits(false),
  has_effort_limits(false),
  angle_wraparound(false)
@@ -53,12 +55,14 @@ struct JointLimits
  double max_position;
  double max_velocity;
  double max_acceleration;
+ double max_deceleration;
  double max_jerk;
  double max_effort;
 
  bool has_position_limits;
  bool has_velocity_limits;
  bool has_acceleration_limits;
+ bool has_deceleration_limits;
  bool has_jerk_limits;
  bool has_effort_limits;
  bool angle_wraparound;
@@ -73,6 +77,8 @@ struct JointLimits
  << max_velocity << "]\n";
  ss_output << " has acceleration limits: " << (has_acceleration_limits ? "true" : "false")
  << " [" << max_acceleration << "]\n";
+ ss_output << " has deceleration limits: " << (has_deceleration_limits ? "true" : "false")
+ << " [" << max_deceleration << "]\n";
  ss_output << " has jerk limits: " << (has_jerk_limits ? "true" : "false") << " [" << max_jerk
  << "]\n";
  ss_output << " has effort limits: " << (has_effort_limits ? "true" : "false") << " ["

joint_limits/include/joint_limits/joint_limits_rosparam.hpp

@@ -62,6 +62,8 @@ namespace joint_limits
  * max_velocity: double
  * has_acceleration_limits: bool
  * max_acceleration: double
+ * has_deceleration_limits: bool
+ * max_deceleration: double
  * has_jerk_limits: bool
  * max_jerk: double
  * has_effort_limits: bool
@@ -101,6 +103,9 @@ inline bool declare_parameters(
  auto_declare<bool>(param_itf, param_base_name + ".has_acceleration_limits", false);
  auto_declare<double>(
  param_itf, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
+ auto_declare<bool>(param_itf, param_base_name + ".has_deceleration_limits", false);
+ auto_declare<double>(
+ param_itf, param_base_name + ".max_deceleration", std::numeric_limits<double>::quiet_NaN());
  auto_declare<bool>(param_itf, param_base_name + ".has_jerk_limits", false);
  auto_declare<double>(
  param_itf, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
@@ -173,6 +178,8 @@ inline bool declare_parameters(
  * max_velocity: double
  * has_acceleration_limits: bool
  * max_acceleration: double
+ * has_deceleration_limits: bool
+ * max_deceleration: double
  * has_jerk_limits: bool
  * max_jerk: double
  * has_effort_limits: bool
@@ -194,6 +201,8 @@ inline bool declare_parameters(
  * max_velocity: 2.0
  * has_acceleration_limits: true
  * max_acceleration: 5.0
+ * has_deceleration_limits: true
+ * max_deceleration: 7.5
  * has_jerk_limits: true
  * max_jerk: 100.0
  * has_effort_limits: true
@@ -231,6 +240,8 @@ inline bool get_joint_limits(
  !param_itf->has_parameter(param_base_name + ".max_velocity") &&
  !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") &&
  !param_itf->has_parameter(param_base_name + ".max_acceleration") &&
+ !param_itf->has_parameter(param_base_name + ".has_deceleration_limits") &&
+ !param_itf->has_parameter(param_base_name + ".max_deceleration") &&
  !param_itf->has_parameter(param_base_name + ".has_jerk_limits") &&
  !param_itf->has_parameter(param_base_name + ".max_jerk") &&
  !param_itf->has_parameter(param_base_name + ".has_effort_limits") &&
@@ -315,6 +326,24 @@ inline bool get_joint_limits(
  }
  }
 
+ // Deceleration limits
+ if (param_itf->has_parameter(param_base_name + ".has_deceleration_limits"))
+ {
+ limits.has_deceleration_limits =
+ param_itf->get_parameter(param_base_name + ".has_deceleration_limits").as_bool();
+ if (
+ limits.has_deceleration_limits &&
+ param_itf->has_parameter(param_base_name + ".max_deceleration"))
+ {
+ limits.max_deceleration =
+ param_itf->get_parameter(param_base_name + ".max_deceleration").as_double();
+ }
+ else
+ {
+ limits.has_deceleration_limits = false;
+ }
+ }
+
  // Jerk limits
  if (param_itf->has_parameter(param_base_name + ".has_jerk_limits"))
  {

joint_limits/test/joint_limits_rosparam.yaml

@@ -10,6 +10,8 @@ JointLimitsRosparamTestNode:
  max_velocity: 2.0
  has_acceleration_limits: true
  max_acceleration: 5.0
+ has_deceleration_limits: true
+ max_deceleration: 7.5
  has_jerk_limits: true
  max_jerk: 100.0
  has_effort_limits: true
@@ -26,6 +28,7 @@ JointLimitsRosparamTestNode:
  has_position_limits: true
  has_velocity_limits: true
  has_acceleration_limits: true
+ has_deceleration_limits: true
  has_jerk_limits: true
  has_effort_limits: true
 
@@ -35,6 +38,7 @@ JointLimitsRosparamTestNode:
  max_position: 1.0
  max_velocity: 2.0
  max_acceleration: 5.0
+ max_deceleration: 7.5
  max_jerk: 100.0
  max_effort: 20.0
 
@@ -43,6 +47,7 @@ JointLimitsRosparamTestNode:
  has_position_limits: false
  has_velocity_limits: false
  has_acceleration_limits: false
+ has_deceleration_limits: false
  has_jerk_limits: false
  has_effort_limits: false
  angle_wraparound: true # should be accepted, has no position limits

joint_limits/test/joint_limits_rosparam_test.cpp

@@ -54,6 +54,8 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
  EXPECT_TRUE(std::isnan(limits.max_velocity));
  EXPECT_FALSE(limits.has_acceleration_limits);
  EXPECT_TRUE(std::isnan(limits.max_acceleration));
+ EXPECT_FALSE(limits.has_deceleration_limits);
+ EXPECT_TRUE(std::isnan(limits.max_deceleration));
  EXPECT_FALSE(limits.has_jerk_limits);
  EXPECT_TRUE(std::isnan(limits.max_jerk));
  EXPECT_FALSE(limits.has_effort_limits);
@@ -76,6 +78,8 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
  EXPECT_TRUE(std::isnan(limits.max_velocity));
  EXPECT_FALSE(limits.has_acceleration_limits);
  EXPECT_TRUE(std::isnan(limits.max_acceleration));
+ EXPECT_FALSE(limits.has_deceleration_limits);
+ EXPECT_TRUE(std::isnan(limits.max_deceleration));
  EXPECT_FALSE(limits.has_jerk_limits);
  EXPECT_TRUE(std::isnan(limits.max_jerk));
  EXPECT_FALSE(limits.has_effort_limits);
@@ -100,6 +104,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
  EXPECT_TRUE(limits.has_acceleration_limits);
  EXPECT_EQ(5.0, limits.max_acceleration);
 
+ EXPECT_TRUE(limits.has_deceleration_limits);
+ EXPECT_EQ(7.5, limits.max_deceleration);
+
  EXPECT_TRUE(limits.has_jerk_limits);
  EXPECT_EQ(100.0, limits.max_jerk);
 
@@ -120,6 +127,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
  EXPECT_FALSE(limits.has_position_limits);
  EXPECT_FALSE(limits.has_velocity_limits);
  EXPECT_FALSE(limits.has_acceleration_limits);
+ EXPECT_FALSE(limits.has_deceleration_limits);
  EXPECT_FALSE(limits.has_jerk_limits);
  EXPECT_FALSE(limits.has_effort_limits);
  }
@@ -134,6 +142,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
  EXPECT_FALSE(limits.has_position_limits);
  EXPECT_FALSE(limits.has_velocity_limits);
  EXPECT_FALSE(limits.has_acceleration_limits);
+ EXPECT_FALSE(limits.has_deceleration_limits);
  EXPECT_FALSE(limits.has_jerk_limits);
  EXPECT_FALSE(limits.has_effort_limits);
  }
@@ -147,6 +156,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
  EXPECT_TRUE(limits.has_position_limits);
  EXPECT_TRUE(limits.has_velocity_limits);
  EXPECT_TRUE(limits.has_acceleration_limits);
+ EXPECT_TRUE(limits.has_deceleration_limits);
  EXPECT_TRUE(limits.has_jerk_limits);
  EXPECT_TRUE(limits.has_effort_limits);
 
@@ -156,6 +166,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
  EXPECT_FALSE(limits.has_position_limits);
  EXPECT_FALSE(limits.has_velocity_limits);
  EXPECT_FALSE(limits.has_acceleration_limits);
+ EXPECT_FALSE(limits.has_deceleration_limits);
  EXPECT_FALSE(limits.has_jerk_limits);
  EXPECT_FALSE(limits.has_effort_limits);
  EXPECT_TRUE(limits.angle_wraparound);
@@ -191,6 +202,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
  EXPECT_FALSE(limits.has_acceleration_limits);
  EXPECT_TRUE(std::isnan(limits.max_acceleration));
 
+ EXPECT_FALSE(limits.has_deceleration_limits);
+ EXPECT_TRUE(std::isnan(limits.max_deceleration));
+
  EXPECT_FALSE(limits.has_jerk_limits);
  EXPECT_TRUE(std::isnan(limits.max_jerk));
 
@@ -317,6 +331,9 @@ TEST_F(JointLimitsUndeclaredRosParamTest, parse_declared_joint_limits_node)
  EXPECT_TRUE(limits.has_acceleration_limits);
  EXPECT_EQ(5.0, limits.max_acceleration);
 
+ EXPECT_TRUE(limits.has_deceleration_limits);
+ EXPECT_EQ(7.5, limits.max_deceleration);
+
  EXPECT_TRUE(limits.has_jerk_limits);
  EXPECT_EQ(100.0, limits.max_jerk);
 
@@ -356,6 +373,9 @@ TEST_F(JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_joint_limi
  EXPECT_TRUE(limits.has_acceleration_limits);
  EXPECT_EQ(5.0, limits.max_acceleration);
 
+ EXPECT_TRUE(limits.has_deceleration_limits);
+ EXPECT_EQ(7.5, limits.max_deceleration);
+
  EXPECT_TRUE(limits.has_jerk_limits);
  EXPECT_EQ(100.0, limits.max_jerk);