diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp index fffab7a841..809bfd777b 100644 --- a/joint_limits/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -38,11 +38,13 @@ struct JointLimits max_position(std::numeric_limits::quiet_NaN()), max_velocity(std::numeric_limits::quiet_NaN()), max_acceleration(std::numeric_limits::quiet_NaN()), + max_deceleration(std::numeric_limits::quiet_NaN()), max_jerk(std::numeric_limits::quiet_NaN()), max_effort(std::numeric_limits::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") << " [" diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 6e0b66641e..5d1fd6a1f7 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/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(param_itf, param_base_name + ".has_acceleration_limits", false); auto_declare( param_itf, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_deceleration_limits", false); + auto_declare( + param_itf, param_base_name + ".max_deceleration", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_jerk_limits", false); auto_declare( param_itf, param_base_name + ".max_jerk", std::numeric_limits::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")) { diff --git a/joint_limits/test/joint_limits_rosparam.yaml b/joint_limits/test/joint_limits_rosparam.yaml index 521fbf93f8..9088610d17 100644 --- a/joint_limits/test/joint_limits_rosparam.yaml +++ b/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 diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp index c5ddb8f585..5f376562a6 100644 --- a/joint_limits/test/joint_limits_rosparam_test.cpp +++ b/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);