From 1eac1c558db55b7c8d8806a33e12128318ede22b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 17 Apr 2021 13:29:22 -0500 Subject: [PATCH 1/4] Ensure yaml param types are correct --- .../moveit_servo/config/panda_simulated_config.yaml | 10 +++++----- .../config/panda_simulated_config_pose_tracking.yaml | 10 +++++----- .../moveit_servo/test/config/servo_settings.yaml | 12 ++++++------ .../test/config/servo_settings_low_latency.yaml | 12 ++++++------ 4 files changed, 22 insertions(+), 22 deletions(-) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index da8eb0a872..7997d69d32 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/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] diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml index b55d1900d8..652a13c298 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +++ b/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] diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml index b081f28c73..fa36c1e1ee 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ b/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] diff --git a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml index 30341906f1..4342c9e29c 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml +++ b/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] From 21d622aff8db28773febba479c8b18671491b803 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 20 Apr 2021 09:45:58 -0500 Subject: [PATCH 2/4] Ignore the casting of int's to double's --- .../moveit_servo/src/servo_parameters.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp index 3e8bce7ccc..ef8ae8652b 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.cpp +++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp @@ -42,6 +42,7 @@ #include #include #include +#include using namespace std::placeholders; // for _1, _2 etc. @@ -64,16 +65,20 @@ 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 parameter written in the yaml as "1" being considered an - if (std::is_same::value) + // Ignore int's when a double is expected because int's get cast properly + int int_parameter; + if (node->get_parameter(param_name, int_parameter) && std::is_floating_point::value) { - node->undeclare_parameter(param_name); - output_value = node->declare_parameter(param_name, 0); + if (std::is_same::value) + { + node->undeclare_parameter(param_name); + // Implicitly cast to double + output_value = node->declare_parameter(param_name, 0); + } } else { + RCLCPP_WARN_STREAM(logger, "InvalidParameterTypeException(" << param_name << "): " << e.what()); RCLCPP_ERROR_STREAM(logger, "Error getting parameter \'" << param_name << "\', check parameter type in YAML file"); throw e; } From 52732fea5598b8ad04f2b72696604f0c4659c16c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 20 Apr 2021 09:53:38 -0500 Subject: [PATCH 3/4] Update includes --- moveit_ros/moveit_servo/src/servo_parameters.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp index ef8ae8652b..4993d323b1 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.cpp +++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp @@ -39,10 +39,9 @@ Created : 07/02/2020 */ -#include -#include #include -#include +#include +#include using namespace std::placeholders; // for _1, _2 etc. From e7c5e5e387eb6ff15884320fba10d41610078b76 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 20 Apr 2021 13:15:27 -0500 Subject: [PATCH 4/4] Throw error if parameter type doesn't match --- .../moveit_servo/src/servo_parameters.cpp | 20 +++---------------- 1 file changed, 3 insertions(+), 17 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp index 4993d323b1..3081a62bcd 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.cpp +++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp @@ -64,23 +64,9 @@ void declareOrGetParam(T& output_value, const std::string& param_name, const rcl } catch (const rclcpp::exceptions::InvalidParameterTypeException& e) { - // Ignore int's when a double is expected because int's get cast properly - int int_parameter; - if (node->get_parameter(param_name, int_parameter) && std::is_floating_point::value) - { - if (std::is_same::value) - { - node->undeclare_parameter(param_name); - // Implicitly cast to double - output_value = node->declare_parameter(param_name, 0); - } - } - else - { - RCLCPP_WARN_STREAM(logger, "InvalidParameterTypeException(" << param_name << "): " << e.what()); - RCLCPP_ERROR_STREAM(logger, "Error getting parameter \'" << param_name << "\', check parameter type in YAML file"); - throw e; - } + RCLCPP_WARN_STREAM(logger, "InvalidParameterTypeException(" << param_name << "): " << e.what()); + 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);