diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h index 26ff507e9cf594dc4f6419ce85d308fed38372e7..0140d0228012869a9e8002f009bb8c42e10da53a 100644 --- a/franka_hw/include/franka_hw/franka_hw.h +++ b/franka_hw/include/franka_hw/franka_hw.h @@ -445,9 +445,9 @@ class FrankaHW : public hardware_interface::RobotHW { * to be valid. * @return A set parsed parameters if valid parameters where found, the default values otherwise. */ - virtual std::vector<double> getCollisionThresholds(const std::string& name, - ros::NodeHandle& robot_hw_nh, - const std::vector<double>& defaults) const; + static std::vector<double> getCollisionThresholds(const std::string& name, + ros::NodeHandle& robot_hw_nh, + const std::vector<double>& defaults); struct CollisionConfig { std::array<double, 7> lower_torque_thresholds_acceleration; diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp index 9aada40183998aecd71b9950bc541d3a14d699ff..c4cce12ab6c5bb9a7815e5d982152b39bfe8495e 100644 --- a/franka_hw/src/franka_hw.cpp +++ b/franka_hw/src/franka_hw.cpp @@ -562,13 +562,14 @@ bool FrankaHW::commandHasNaN(const franka::CartesianVelocities& command) { std::vector<double> FrankaHW::getCollisionThresholds(const std::string& name, ros::NodeHandle& robot_hw_nh, - const std::vector<double>& defaults) const { + const std::vector<double>& defaults) { std::vector<double> thresholds; if (!robot_hw_nh.getParam("collision_config/" + name, thresholds) || thresholds.size() != defaults.size()) { std::string message; for (const double& threshold : defaults) { - message = message + std::to_string(threshold) + " "; + message += std::to_string(threshold); + message += " "; } ROS_INFO("No parameter %s found, using default values: %s", name.c_str(), message.c_str()); return defaults;