diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 09ca27e1d2..83f18a9931 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -30,6 +30,12 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(diff_drive_controller_parameters + src/diff_drive_controller_parameter.yaml +) + add_library(${PROJECT_NAME} SHARED src/diff_drive_controller.cpp src/odometry.cpp @@ -39,6 +45,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(${PROJECT_NAME} + diff_drive_controller_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index dc158d7ff4..1ef47bd975 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -70,79 +70,10 @@ Services Parameters ------------ -Wheels -,,,,,,, -left_wheel_names [array] - Link names of the left side wheels. - -right_wheel_names [array] - Link names of the right side wheels. - -wheel_separation [double] - Shortest distance between the left and right wheels. - If this parameter is wrong, the robot will not behave correctly in curves. - -wheels_per_side [integer] - Number of wheels on each wide of the robot. - This is important to take the wheels slip into account when multiple wheels on each side are present. - If there are more wheels then control signals for each side, you should enter number or control signals. - For example, Husky has two wheels on each side, but they use one control signal, in this case "1" is the correct value of the parameter. - -wheel_radius [double] - Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. - If this parameter is wrong the robot will move faster or slower then expected. - -wheel_separation_multiplier [double] - Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly) - -left_wheel_radius_multiplier [double] - Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter. - -right_wheel_radius_multiplier [double] - Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter. - -Odometry -,,,,,,,,, -base_frame_id [string] (default: "base_link") - Name of the frame for odometry. - This frame is parent of ``base_frame_id`` when controller publishes odometry. - -odom_frame_id [string] (default: "odom") - Name of the robot's base frame that is child of the odometry frame. +Check `parameter definition file for details `_. -pose_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") - Odometry covariance for the encoder output of the robot for the pose. - These values should be tuned to your robot's sample odometry data, but these values are a good place to start: - ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. - -twist_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") - Odometry covariance for the encoder output of the robot for the speed. - These values should be tuned to your robot's sample odometry data, but these values are a good place to start: - ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. - -open_loop [bool] (default: "false") - If set to false the odometry of the robot will be calculated from the commanded values and not from feedback. - -position_feedback [bool] (default: "true") - Is there position feedback from hardware. - -enable_odom_tf [bool] (default: "true") - Publish transformation between ``odom_frame_id`` and ``base_frame_id``. - -velocity_rolling_window_size [int] (default: "10") - (TODO(destogl): Please help me describe this correctly) - -Commands -``````````` -cmd_vel_timeout [double] (default: "0.5s") - Timeout after which input command on ``cmd_vel`` topic is considered staled. - -publish_limited_velocity [double] (default: "false") - Publish limited velocity value. - - -use_stamped_vel [bool] (default: "true") - Use stamp from input velocity message to calculate how old the command actually is. +Note that the documentation on parameters for joint limits can be found in `their header file `_. +Those parameters are: linear.x [JointLimits structure] Joint limits structure for the linear X-axis. @@ -153,6 +84,3 @@ angular.z [JointLimits structure] Joint limits structure for the rotation about Z-axis. The limiter ignores position limits. For details see ``joint_limits`` package from ros2_control repository. - -publish_rate [double] (default: "50.0") - Publishing rate (Hz) of the odometry and TF messages. diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index ce357e4116..f229667fb9 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -42,6 +42,8 @@ #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" +#include "diff_drive_controller_parameters.hpp" + namespace diff_drive_controller { class DiffDriveController : public controller_interface::ControllerInterface @@ -101,35 +103,18 @@ class DiffDriveController : public controller_interface::ControllerInterface const std::string & side, const std::vector & wheel_names, std::vector & registered_handles); - std::vector left_wheel_names_; - std::vector right_wheel_names_; - std::vector registered_left_wheel_handles_; std::vector registered_right_wheel_handles_; - struct WheelParams - { - size_t wheels_per_side = 0; - double separation = 0.0; // w.r.t. the midpoint of the wheel width - double radius = 0.0; // Assumed to be the same for both wheels - double separation_multiplier = 1.0; - double left_radius_multiplier = 1.0; - double right_radius_multiplier = 1.0; - } wheel_params_; - - struct OdometryParams - { - bool open_loop = false; - bool position_feedback = true; - bool enable_odom_tf = true; - std::string base_frame_id = "base_link"; - std::string odom_frame_id = "odom"; - std::array pose_covariance_diagonal; - std::array twist_covariance_diagonal; - } odom_params_; + // Parameters from ROS for diff_drive_controller + std::shared_ptr param_listener_; + Params params_; Odometry odometry_; + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; @@ -139,9 +124,6 @@ class DiffDriveController : public controller_interface::ControllerInterface std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; - // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; - bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 764f52c66d..6b675ccc10 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake + generate_parameter_library controller_interface geometry_msgs diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index c4fec1430a..ce953c6027 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -50,58 +50,16 @@ DiffDriveController::DiffDriveController() : controller_interface::ControllerInt const char * DiffDriveController::feedback_type() const { - return odom_params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; + return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; } controller_interface::CallbackReturn DiffDriveController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare>("left_wheel_names", std::vector()); - auto_declare>("right_wheel_names", std::vector()); - - auto_declare("wheel_separation", wheel_params_.separation); - auto_declare("wheels_per_side", wheel_params_.wheels_per_side); - auto_declare("wheel_radius", wheel_params_.radius); - auto_declare("wheel_separation_multiplier", wheel_params_.separation_multiplier); - auto_declare("left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier); - auto_declare("right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier); - - auto_declare("odom_frame_id", odom_params_.odom_frame_id); - auto_declare("base_frame_id", odom_params_.base_frame_id); - auto_declare>("pose_covariance_diagonal", std::vector()); - auto_declare>("twist_covariance_diagonal", std::vector()); - auto_declare("open_loop", odom_params_.open_loop); - auto_declare("position_feedback", odom_params_.position_feedback); - auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); - - auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - publish_limited_velocity_ = - auto_declare("publish_limited_velocity", publish_limited_velocity_); - auto_declare("velocity_rolling_window_size", 10); - use_stamped_vel_ = auto_declare("use_stamped_vel", use_stamped_vel_); - - auto_declare("linear.x.has_velocity_limits", false); - auto_declare("linear.x.has_acceleration_limits", false); - auto_declare("linear.x.has_jerk_limits", false); - auto_declare("linear.x.max_velocity", NAN); - auto_declare("linear.x.min_velocity", NAN); - auto_declare("linear.x.max_acceleration", NAN); - auto_declare("linear.x.min_acceleration", NAN); - auto_declare("linear.x.max_jerk", NAN); - auto_declare("linear.x.min_jerk", NAN); - - auto_declare("angular.z.has_velocity_limits", false); - auto_declare("angular.z.has_acceleration_limits", false); - auto_declare("angular.z.has_jerk_limits", false); - auto_declare("angular.z.max_velocity", NAN); - auto_declare("angular.z.min_velocity", NAN); - auto_declare("angular.z.max_acceleration", NAN); - auto_declare("angular.z.min_acceleration", NAN); - auto_declare("angular.z.max_jerk", NAN); - auto_declare("angular.z.min_jerk", NAN); - publish_rate_ = auto_declare("publish_rate", publish_rate_); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -115,11 +73,11 @@ controller_interface::CallbackReturn DiffDriveController::on_init() InterfaceConfiguration DiffDriveController::command_interface_configuration() const { std::vector conf_names; - for (const auto & joint_name : left_wheel_names_) + for (const auto & joint_name : params_.left_wheel_names) { conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } - for (const auto & joint_name : right_wheel_names_) + for (const auto & joint_name : params_.right_wheel_names) { conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } @@ -129,11 +87,11 @@ InterfaceConfiguration DiffDriveController::command_interface_configuration() co InterfaceConfiguration DiffDriveController::state_interface_configuration() const { std::vector conf_names; - for (const auto & joint_name : left_wheel_names_) + for (const auto & joint_name : params_.left_wheel_names) { conf_names.push_back(joint_name + "/" + feedback_type()); } - for (const auto & joint_name : right_wheel_names_) + for (const auto & joint_name : params_.right_wheel_names) { conf_names.push_back(joint_name + "/" + feedback_type()); } @@ -180,12 +138,11 @@ controller_interface::return_type DiffDriveController::update( previous_update_timestamp_ = time; // Apply (possibly new) multipliers: - const auto wheels = wheel_params_; - const double wheel_separation = wheels.separation_multiplier * wheels.separation; - const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; - const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; - if (odom_params_.open_loop) + if (params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, time); } @@ -193,7 +150,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < wheels.wheels_per_side; ++index) + for (size_t index = 0; index < params_.wheels_per_side; ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -210,10 +167,10 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= wheels.wheels_per_side; - right_feedback_mean /= wheels.wheels_per_side; + left_feedback_mean /= params_.wheels_per_side; + right_feedback_mean /= params_.wheels_per_side; - if (odom_params_.position_feedback) + if (params_.position_feedback) { odometry_.update(left_feedback_mean, right_feedback_mean, time); } @@ -246,7 +203,7 @@ controller_interface::return_type DiffDriveController::update( realtime_odometry_publisher_->unlockAndPublish(); } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; @@ -286,7 +243,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < wheels.wheels_per_side; ++index) + for (size_t index = 0; index < params_.wheels_per_side; ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -300,100 +257,50 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { auto logger = get_node()->get_logger(); - // update parameters - left_wheel_names_ = get_node()->get_parameter("left_wheel_names").as_string_array(); - right_wheel_names_ = get_node()->get_parameter("right_wheel_names").as_string_array(); + // update parameters if they have changed + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); + } - if (left_wheel_names_.size() != right_wheel_names_.size()) + if (params_.left_wheel_names.size() != params_.right_wheel_names.size()) { RCLCPP_ERROR( logger, "The number of left wheels [%zu] and the number of right wheels [%zu] are different", - left_wheel_names_.size(), right_wheel_names_.size()); + params_.left_wheel_names.size(), params_.right_wheel_names.size()); return controller_interface::CallbackReturn::ERROR; } - if (left_wheel_names_.empty()) + if (params_.left_wheel_names.empty()) { RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); return controller_interface::CallbackReturn::ERROR; } - wheel_params_.separation = get_node()->get_parameter("wheel_separation").as_double(); - wheel_params_.wheels_per_side = - static_cast(get_node()->get_parameter("wheels_per_side").as_int()); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - wheel_params_.separation_multiplier = - get_node()->get_parameter("wheel_separation_multiplier").as_double(); - wheel_params_.left_radius_multiplier = - get_node()->get_parameter("left_wheel_radius_multiplier").as_double(); - wheel_params_.right_radius_multiplier = - get_node()->get_parameter("right_wheel_radius_multiplier").as_double(); - - const auto wheels = wheel_params_; - - const double wheel_separation = wheels.separation_multiplier * wheels.separation; - const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; - const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); - odometry_.setVelocityRollingWindowSize( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); - - odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); - - auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); - std::copy( - pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - - auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); - std::copy( - twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - - odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); - odom_params_.position_feedback = get_node()->get_parameter("position_feedback").as_bool(); - odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); cmd_vel_timeout_ = std::chrono::milliseconds{ static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - try - { - limiter_linear_ = SpeedLimiter( - get_node()->get_parameter("linear.x.has_velocity_limits").as_bool(), - get_node()->get_parameter("linear.x.has_acceleration_limits").as_bool(), - get_node()->get_parameter("linear.x.has_jerk_limits").as_bool(), - get_node()->get_parameter("linear.x.min_velocity").as_double(), - get_node()->get_parameter("linear.x.max_velocity").as_double(), - get_node()->get_parameter("linear.x.min_acceleration").as_double(), - get_node()->get_parameter("linear.x.max_acceleration").as_double(), - get_node()->get_parameter("linear.x.min_jerk").as_double(), - get_node()->get_parameter("linear.x.max_jerk").as_double()); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring linear speed limiter: %s", e.what()); - } + limiter_linear_ = SpeedLimiter( + params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, + params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, + params_.linear.x.max_jerk); - try - { - limiter_angular_ = SpeedLimiter( - get_node()->get_parameter("angular.z.has_velocity_limits").as_bool(), - get_node()->get_parameter("angular.z.has_acceleration_limits").as_bool(), - get_node()->get_parameter("angular.z.has_jerk_limits").as_bool(), - get_node()->get_parameter("angular.z.min_velocity").as_double(), - get_node()->get_parameter("angular.z.max_velocity").as_double(), - get_node()->get_parameter("angular.z.min_acceleration").as_double(), - get_node()->get_parameter("angular.z.max_acceleration").as_double(), - get_node()->get_parameter("angular.z.min_jerk").as_double(), - get_node()->get_parameter("angular.z.max_jerk").as_double()); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring angular speed limiter: %s", e.what()); - } + limiter_angular_ = SpeedLimiter( + params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, + params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, + params_.angular.z.max_velocity, params_.angular.z.min_acceleration, + params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); if (!reset()) { @@ -401,7 +308,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - wheel_params_.wheels_per_side = left_wheel_names_.size(); + params_.wheels_per_side = params_.left_wheel_names.size(); if (publish_limited_velocity_) { @@ -482,12 +389,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( controller_namespace = controller_namespace + "/"; } - odom_params_.odom_frame_id = controller_namespace + odom_params_.odom_frame_id; - odom_params_.base_frame_id = controller_namespace + odom_params_.base_frame_id; + const auto odom_frame_id = controller_namespace + params_.odom_frame_id; + const auto base_frame_id = controller_namespace + params_.base_frame_id; auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = odom_params_.odom_frame_id; - odometry_message.child_frame_id = odom_params_.base_frame_id; + odometry_message.header.frame_id = controller_namespace + odom_frame_id; + odometry_message.child_frame_id = controller_namespace + base_frame_id; // limit the publication on the topics /odom and /tf publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); @@ -503,9 +410,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = - odom_params_.twist_covariance_diagonal[index]; + odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message @@ -518,8 +424,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( // keeping track of odom and base_link transforms only auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + odometry_transform_message.transforms.front().header.frame_id = odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = base_frame_id; previous_update_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; @@ -529,9 +435,9 @@ controller_interface::CallbackReturn DiffDriveController::on_activate( const rclcpp_lifecycle::State &) { const auto left_result = - configure_side("left", left_wheel_names_, registered_left_wheel_handles_); + configure_side("left", params_.left_wheel_names, registered_left_wheel_handles_); const auto right_result = - configure_side("right", right_wheel_names_, registered_right_wheel_handles_); + configure_side("right", params_.right_wheel_names, registered_right_wheel_handles_); if ( left_result == controller_interface::CallbackReturn::ERROR || diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml new file mode 100644 index 0000000000..82ba210113 --- /dev/null +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -0,0 +1,177 @@ +diff_drive_controller: + left_wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the left side wheels", + } + right_wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the right side wheels", + } + wheel_separation: { + type: double, + default_value: 0.0, + description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + } + wheels_per_side: { + type: int, + default_value: 0, + description: "Number of wheels on each wide of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number or control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", + } + wheel_radius: { + type: double, + default_value: 0.0, + description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + } + wheel_separation_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly)", + } + left_wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter.", + } + right_wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + } + base_frame_id: { + type: string, + default_value: "odom", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + open_loop: { + type: bool, + default_value: false, + description: "If set to false the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + position_feedback: { + type: bool, + default_value: true, + description: "Is there position feedback from hardware.", + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + cmd_vel_timeout: { + type: double, + default_value: 0.5, # seconds + description: "Timeout after which input command on ``cmd_vel`` topic is considered staled.", + } + publish_limited_velocity: { + type: bool, + default_value: false, + description: "Publish limited velocity value.", + } + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "Size of the rolling window for calculation of mean velocity use in odometry.", + } + use_stamped_vel: { + type: bool, + default_value: true, + description: "Use stamp from input velocity message to calculate how old the command actually is.", + } + publish_rate: { + type: double, + default_value: 50.0, # Hz + description: "Publishing rate (Hz) of the odometry and TF messages.", + } + linear: + x: + has_velocity_limits: { + type: bool, + default_value: false, + } + has_acceleration_limits: { + type: bool, + default_value: false, + } + has_jerk_limits: { + type: bool, + default_value: false, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } + angular: + z: + has_velocity_limits: { + type: bool, + default_value: false, + } + has_acceleration_limits: { + type: bool, + default_value: false, + } + has_jerk_limits: { + type: bool, + default_value: false, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + }