Servo: Ensure yaml param types are correct (#427)

* Ensure yaml param types are correct

* Ignore the casting of int's to double's

* Update includes

* Throw error if parameter type doesn't match

Co-authored-by: Tyler Weaver <tyler@picknik.ai>

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -26,7 +26,7 @@ publish_joint_velocities: false
 publish_joint_accelerations: false
 
 ## Incoming Joint State properties
-low_pass_filter_coeff: 2  # Larger --> trust the filtered data more, trust the measurements less.
+low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## MoveIt properties
 move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
@@ -43,8 +43,8 @@ incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a ne
 num_outgoing_halt_msgs_to_publish: 4
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold:  17  # Start decelerating when the condition number hits this (close to singularity)
-hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
+lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
 joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
 
 ## Topic names
@@ -56,7 +56,7 @@ command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing com
 
 ## Collision checking for the entire robot body
 check_collisions: true # Check collisions?
-collision_check_rate: 10 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 # Two collision check algorithms are available:
 # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
 # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
@@ -65,5 +65,5 @@ collision_check_type: threshold_distance
 self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
 scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
 # Parameters for "stop_distance"-type collision checking
-collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
+collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
 min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml

@@ -25,7 +25,7 @@ publish_joint_velocities: true
 publish_joint_accelerations: false
 
 ## Incoming Joint State properties
-low_pass_filter_coeff: 2  # Larger --> trust the filtered data more, trust the measurements less.
+low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## MoveIt properties
 move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
@@ -42,8 +42,8 @@ incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a ne
 num_outgoing_halt_msgs_to_publish: 4
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold:  17  # Start decelerating when the condition number hits this (close to singularity)
-hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
+lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
 joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
 
 ## Topic names
@@ -55,7 +55,7 @@ command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing com
 
 ## Collision checking for the entire robot body
 check_collisions: true # Check collisions?
-collision_check_rate: 10 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 # Two collision check algorithms are available:
 # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
 # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
@@ -64,5 +64,5 @@ collision_check_type: threshold_distance
 self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
 scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
 # Parameters for "stop_distance"-type collision checking
-collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
+collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
 min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

moveit_ros/moveit_servo/src/servo_parameters.cpp

@@ -39,9 +39,9 @@
    Created   : 07/02/2020
 */
 
-#include <type_traits>
-#include <rclcpp/rclcpp.hpp>
 #include <moveit_servo/servo_parameters.h>
+#include <rclcpp/rclcpp.hpp>
+#include <type_traits>
 
 using namespace std::placeholders;  // for _1, _2 etc.
 
@@ -65,18 +65,8 @@ void declareOrGetParam(T& output_value, const std::string& param_name, const rcl
   catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
   {
     RCLCPP_WARN_STREAM(logger, "InvalidParameterTypeException(" << param_name << "): " << e.what());
-
-    // Catch a <double> parameter written in the yaml as "1" being considered an <int>
-    if (std::is_same<T, double>::value)
-    {
-      node->undeclare_parameter(param_name);
-      output_value = node->declare_parameter<int>(param_name, 0);
-    }
-    else
-    {
-      RCLCPP_ERROR_STREAM(logger, "Error getting parameter \'" << param_name << "\', check parameter type in YAML file");
-      throw e;
-    }
+    RCLCPP_ERROR_STREAM(logger, "Error getting parameter \'" << param_name << "\', check parameter type in YAML file");
+    throw e;
   }
 
   RCLCPP_INFO_STREAM(logger, "Found parameter - " << param_name << ": " << output_value);

moveit_ros/moveit_servo/test/config/servo_settings.yaml

@@ -11,7 +11,7 @@ scale:
   rotational:  0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
   joint: 0.01
-low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the measurements less.
+low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## Properties of outgoing commands
 low_latency_mode: false  # Set this to true to tie the output rate to the input rate
@@ -36,14 +36,14 @@ ee_frame_name: panda_link7  # The name of the end effector link, used to return
 robot_link_command_frame:  panda_link0  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
-incoming_command_timeout:  1  # Stop servoing if X seconds elapse without a new command
+incoming_command_timeout:  1.0  # Stop servoing if X seconds elapse without a new command
 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
 # Important because ROS may drop some messages and we need the robot to halt reliably.
 num_outgoing_halt_msgs_to_publish: 1
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold:  30  # Start decelerating when the condition number hits this (close to singularity)
-hard_stop_singularity_threshold: 45 # Stop when the condition number hits this
+lower_singularity_threshold:  30.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 45.0 # Stop when the condition number hits this
 joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
 
 ## Topic names
@@ -55,7 +55,7 @@ command_out_topic: servo_server/command # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
 check_collisions: true # Check collisions?
-collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 # Two collision check algorithms are available:
 # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
 # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
@@ -64,5 +64,5 @@ collision_check_type: stop_distance
 self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m]
 scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m]
 # Parameters for "stop_distance"-type collision checking
-collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
+collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
 min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml

@@ -11,7 +11,7 @@ scale:
   rotational:  0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
   joint: 0.01
-low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the measurements less.
+low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## Properties of outgoing commands
 low_latency_mode: true  # Set this to true to tie the output rate to the input rate
@@ -36,14 +36,14 @@ ee_frame_name: panda_link7  # The name of the end effector link, used to return
 robot_link_command_frame:  panda_link0  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
-incoming_command_timeout:  1  # Stop servoing if X seconds elapse without a new command
+incoming_command_timeout:  1.0  # Stop servoing if X seconds elapse without a new command
 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
 # Important because ROS may drop some messages and we need the robot to halt reliably.
 num_outgoing_halt_msgs_to_publish: 1
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold:  30  # Start decelerating when the condition number hits this (close to singularity)
-hard_stop_singularity_threshold: 45 # Stop when the condition number hits this
+lower_singularity_threshold:  30.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 45.0 # Stop when the condition number hits this
 joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
 
 ## Topic names
@@ -55,7 +55,7 @@ command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing com
 
 ## Collision checking for the entire robot body
 check_collisions: true # Check collisions?
-collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 # Two collision check algorithms are available:
 # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
 # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
@@ -64,5 +64,5 @@ collision_check_type: stop_distance
 self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m]
 scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m]
 # Parameters for "stop_distance"-type collision checking
-collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
+collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
 min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]