From e7ed18730479487b82c1174913b543a00fb0b35e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 14 Sep 2021 12:19:08 +0200 Subject: [PATCH] Cleanup and use correct cmake macros --- .../joint_limits_interface_extension.h | 3 +- .../src/cartesian_limits_aggregator.cpp | 7 +-- .../test/unit_tests/CMakeLists.txt | 11 ++--- ...ittest_pilz_industrial_motion_planner.test | 48 ------------------- .../unit_tests/src/unittest_joint_limit.cpp | 4 -- .../src/unittest_joint_limits_aggregator.cpp | 1 - 6 files changed, 10 insertions(+), 64 deletions(-) delete mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h index 4d3d18859f..396a656d57 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h @@ -319,7 +319,6 @@ inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::Sh return false; // LCOV_EXCL_LINE // The case where getJointLimits returns // false is covered above. } - try { // Deceleration limits @@ -328,7 +327,7 @@ inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::Sh if (limits.has_deceleration_limits) { limits.max_deceleration = - node->declare_parameter(limits_namespace + ".max_deceleration", limits.max_deceleration); + node->declare_parameter(limits_namespace + ".max_deceleration", std::numeric_limits::quiet_NaN()); } } catch (const std::exception& ex) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp index 84b8e02876..ed2fbbb8f8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp @@ -58,15 +58,16 @@ bool declareAndGetParam(double& output_value, const std::string& param_name, con node->get_parameter(param_name, output_value); if (std::isnan(output_value)) { - RCLCPP_ERROR(node->get_logger(), "Parameter \'%s\', is not set in the config file.", param_name); + RCLCPP_ERROR(node->get_logger(), "Parameter \'%s\', is not set in the config file.", param_name.c_str()); return false; } return true; } catch (const rclcpp::exceptions::InvalidParameterTypeException& e) { - RCLCPP_WARN(node->get_logger(), "InvalidParameterTypeException(\'%s\'): ", param_name, e.what()); - RCLCPP_ERROR(node->get_logger(), "Error getting parameter \'%s\', check parameter type in YAML file", param_name); + RCLCPP_WARN(node->get_logger(), "InvalidParameterTypeException(\'%s\'): %s", param_name.c_str(), e.what()); + RCLCPP_ERROR(node->get_logger(), "Error getting parameter \'%s\', check parameter type in YAML file", + param_name.c_str()); throw e; } } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt index f3545b6156..c25f925d45 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt @@ -20,22 +20,21 @@ set(UNIT_TEST_SOURCEFILES ) # Direct Command Planner Unit Test -ament_add_gtest_executable(unittest_pilz_industrial_motion_planner_direct +ament_add_gtest(unittest_pilz_industrial_motion_planner_direct src/unittest_pilz_industrial_motion_planner.cpp ${CMAKE_SOURCE_DIR}/src/planning_context_loader_ptp.cpp) target_link_libraries(unittest_pilz_industrial_motion_planner_direct ${PROJECT_NAME}_lib ${PROJECT_NAME}_testhelpers) # Asymmetric Trapezoidal Velocity Profile Unit Test -ament_add_gtest_executable(unittest_velocity_profile_atrap +ament_add_gtest(unittest_velocity_profile_atrap src/unittest_velocity_profile_atrap.cpp ${CMAKE_SOURCE_DIR}/src/velocity_profile_atrap.cpp) target_link_libraries(unittest_velocity_profile_atrap ${PROJECT_NAME}_lib) # Trajectory Generator Unit Test -ament_add_gtest_executable(unittest_trajectory_generator +ament_add_gtest(unittest_trajectory_generator src/unittest_trajectory_generator.cpp ${CMAKE_SOURCE_DIR}/src/trajectory_generator.cpp) -ament_target_dependencies(unittest_trajectory_generator rclcpp ${PROJECT_NAME}_testutils) target_link_libraries(unittest_trajectory_generator ${PROJECT_NAME}_lib) # # Trajectory Functions Unit Test @@ -126,12 +125,12 @@ add_ros_test(launch/unittest_joint_limit.test.py ARGS "test_binary_dir:=${CMAKE_ # ) # JointLimitsContainer Unit Test -ament_add_gtest_executable(unittest_joint_limits_container +ament_add_gtest(unittest_joint_limits_container src/unittest_joint_limits_container.cpp) target_link_libraries(unittest_joint_limits_container ${PROJECT_NAME}_lib) # JointLimitsValidator Unit Test -ament_add_gtest_executable(unittest_joint_limits_validator +ament_add_gtest(unittest_joint_limits_validator src/unittest_joint_limits_validator.cpp) target_link_libraries(unittest_joint_limits_validator ${PROJECT_NAME}_lib) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test deleted file mode 100644 index 574f005626..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp index 9eb3aa7212..e822beed01 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp @@ -65,10 +65,6 @@ TEST_F(JointLimitTest, SimpleRead) EXPECT_EQ(1, joint_limits_extended.max_acceleration); EXPECT_EQ(-1, joint_limits_extended.max_deceleration); - EXPECT_EQ(1, joint_limits_extended.max_acceleration); - EXPECT_EQ(-1, joint_limits_extended.max_deceleration); - EXPECT_EQ(1, joint_limits_extended.max_acceleration); - EXPECT_EQ(-1, joint_limits_extended.max_deceleration); } TEST_F(JointLimitTest, readNonExistingJointLimit) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp index 89ca164735..26294d257a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp @@ -202,7 +202,6 @@ TEST_F(JointLimitsAggregator, LimitsViolationVelocity) int main(int argc, char** argv) { - ros::init(argc, argv, "unittest_joint_limits_aggregator"); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }