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/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp index 3e8bce7ccc..3081a62bcd 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.cpp +++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp @@ -39,9 +39,9 @@ Created : 07/02/2020 */ -#include -#include #include +#include +#include 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 parameter written in the yaml as "1" being considered an - if (std::is_same::value) - { - node->undeclare_parameter(param_name); - output_value = node->declare_parameter(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); 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]