From 8d732f1a3bc2879f89162e7d0ddae275180451c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Jan 2024 18:58:52 +0100 Subject: [PATCH] [tricycle_controller] Use generate_parameter_library (#957) --- tricycle_controller/CMakeLists.txt | 13 +- tricycle_controller/doc/userdoc.rst | 10 +- .../tricycle_controller.hpp | 27 +--- tricycle_controller/package.xml | 1 + .../src/tricycle_controller.cpp | 151 +++++------------- .../src/tricycle_controller_parameter.yaml | 149 +++++++++++++++++ .../test/test_load_tricycle_controller.cpp | 9 +- .../test/test_tricycle_controller.cpp | 115 ++++++------- 8 files changed, 275 insertions(+), 200 deletions(-) create mode 100644 tricycle_controller/src/tricycle_controller_parameter.yaml diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 4f6d064dc8..cdc69f182a 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces controller_interface geometry_msgs + generate_parameter_library hardware_interface nav_msgs pluginlib @@ -29,6 +30,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(tricycle_controller_parameters + src/tricycle_controller_parameter.yaml +) + add_library(tricycle_controller SHARED src/tricycle_controller.cpp src/odometry.cpp @@ -40,6 +45,7 @@ target_include_directories(tricycle_controller PUBLIC $ $ ) +target_link_libraries(tricycle_controller PUBLIC tricycle_controller_parameters) ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) @@ -50,8 +56,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_tricycle_controller - test/test_tricycle_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) + test/test_tricycle_controller.cpp) target_link_libraries(test_tricycle_controller tricycle_controller ) @@ -66,8 +71,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_tricycle_controller + add_rostest_with_parameters_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml ) target_link_libraries(test_load_tricycle_controller tricycle_controller @@ -85,6 +91,7 @@ install( install( TARGETS tricycle_controller + tricycle_controller_parameters EXPORT export_tricycle_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index d153aeacba..991627eca2 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -5,7 +5,8 @@ tricycle_controller ===================== -Controller for mobile robots with tricycle drive. +Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle. + Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. @@ -24,3 +25,10 @@ Other features Odometry publishing Velocity, acceleration and jerk limits Automatic stop after command timeout + +Parameters +-------------- + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/tricycle_controller_parameter.yaml diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index cef90d026a..24aaf89de9 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -45,6 +45,9 @@ #include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/visibility_control.h" +// auto-generated by generate_parameter_library +#include "tricycle_controller_parameters.hpp" + namespace tricycle_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -109,31 +112,14 @@ class TricycleController : public controller_interface::ControllerInterface double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); std::tuple twist_to_ackermann(double linear_command, double angular_command); - std::string traction_joint_name_; - std::string steering_joint_name_; + // Parameters from ROS for tricycle_controller + std::shared_ptr param_listener_; + Params params_; // HACK: put into vector to avoid initializing structs because they have no default constructors std::vector traction_joint_; std::vector steering_joint_; - struct WheelParams - { - double wheelbase = 0.0; - double radius = 0.0; - } wheel_params_; - - struct OdometryParams - { - bool open_loop = false; - bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in separate node - 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_; - - bool publish_ackermann_command_ = false; std::shared_ptr> ackermann_command_publisher_ = nullptr; std::shared_ptr> realtime_ackermann_command_publisher_ = nullptr; @@ -168,7 +154,6 @@ class TricycleController : public controller_interface::ControllerInterface SteeringLimiter limiter_steering_; bool is_halted = false; - bool use_stamped_vel_ = true; void reset_odometry( const std::shared_ptr request_header, diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cebd332a60..cc50f0d58c 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -10,6 +10,7 @@ Tony Najjar ament_cmake + generate_parameter_library ackermann_msgs backward_ros diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index d6aeec0af1..07be8c2aae 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -52,41 +52,9 @@ CallbackReturn TricycleController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare("traction_joint_name", std::string()); - auto_declare("steering_joint_name", std::string()); - - auto_declare("wheelbase", wheel_params_.wheelbase); - auto_declare("wheel_radius", wheel_params_.radius); - - 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("enable_odom_tf", odom_params_.enable_odom_tf); - auto_declare("odom_only_twist", odom_params_.odom_only_twist); - - auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count())); - auto_declare("publish_ackermann_command", publish_ackermann_command_); - auto_declare("velocity_rolling_window_size", 10); - auto_declare("use_stamped_vel", use_stamped_vel_); - - auto_declare("traction.max_velocity", NAN); - auto_declare("traction.min_velocity", NAN); - auto_declare("traction.max_acceleration", NAN); - auto_declare("traction.min_acceleration", NAN); - auto_declare("traction.max_deceleration", NAN); - auto_declare("traction.min_deceleration", NAN); - auto_declare("traction.max_jerk", NAN); - auto_declare("traction.min_jerk", NAN); - - auto_declare("steering.max_position", NAN); - auto_declare("steering.min_position", NAN); - auto_declare("steering.max_velocity", NAN); - auto_declare("steering.min_velocity", NAN); - auto_declare("steering.max_acceleration", NAN); - auto_declare("steering.min_acceleration", NAN); + // 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) { @@ -101,8 +69,8 @@ InterfaceConfiguration TricycleController::command_interface_configuration() con { InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + command_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY); + command_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION); return command_interfaces_config; } @@ -110,8 +78,8 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const { InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + state_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY); + state_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION); return state_interfaces_config; } @@ -151,7 +119,7 @@ controller_interface::return_type TricycleController::update( double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians - if (odom_params_.open_loop) + if (params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, period); } @@ -172,7 +140,7 @@ controller_interface::return_type TricycleController::update( { auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.stamp = time; - if (!odom_params_.odom_only_twist) + if (!params_.odom_only_twist) { odometry_message.pose.pose.position.x = odometry_.getX(); odometry_message.pose.pose.position.y = odometry_.getY(); @@ -186,7 +154,7 @@ controller_interface::return_type TricycleController::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; @@ -239,7 +207,7 @@ controller_interface::return_type TricycleController::update( previous_commands_.emplace(ackermann_command); // Publish ackermann command - if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) + if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_->trylock()) { auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel @@ -258,74 +226,40 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { auto logger = get_node()->get_logger(); - // update parameters - traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); - steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); - if (traction_joint_name_.empty()) + // update parameters if they have changed + if (param_listener_->is_old(params_)) { - RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); - return CallbackReturn::ERROR; + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); } - if (steering_joint_name_.empty()) - { - RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); - return CallbackReturn::ERROR; - } - - wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); - odometry_.setVelocityRollingWindowSize( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); + odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); - 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_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); - odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - - cmd_vel_timeout_ = - std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; - publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; + params_.publish_ackermann_command = + get_node()->get_parameter("publish_ackermann_command").as_bool(); + params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool(); try { limiter_traction_ = TractionLimiter( - get_node()->get_parameter("traction.min_velocity").as_double(), - get_node()->get_parameter("traction.max_velocity").as_double(), - get_node()->get_parameter("traction.min_acceleration").as_double(), - get_node()->get_parameter("traction.max_acceleration").as_double(), - get_node()->get_parameter("traction.min_deceleration").as_double(), - get_node()->get_parameter("traction.max_deceleration").as_double(), - get_node()->get_parameter("traction.min_jerk").as_double(), - get_node()->get_parameter("traction.max_jerk").as_double()); + params_.traction.min_velocity, params_.traction.max_velocity, + params_.traction.min_acceleration, params_.traction.max_acceleration, + params_.traction.min_deceleration, params_.traction.max_deceleration, + params_.traction.min_jerk, params_.traction.max_jerk); } catch (const std::invalid_argument & e) { RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); return CallbackReturn::ERROR; } - try { limiter_steering_ = SteeringLimiter( - get_node()->get_parameter("steering.min_position").as_double(), - get_node()->get_parameter("steering.max_position").as_double(), - get_node()->get_parameter("steering.min_velocity").as_double(), - get_node()->get_parameter("steering.max_velocity").as_double(), - get_node()->get_parameter("steering.min_acceleration").as_double(), - get_node()->get_parameter("steering.max_acceleration").as_double()); + params_.steering.min_position, params_.steering.max_position, params_.steering.min_velocity, + params_.steering.max_velocity, params_.steering.min_acceleration, + params_.steering.max_acceleration); } catch (const std::invalid_argument & e) { @@ -347,7 +281,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & previous_commands_.emplace(empty_ackermann_drive); // initialize ackermann command publisher - if (publish_ackermann_command_) + if (params_.publish_ackermann_command) { ackermann_command_publisher_ = get_node()->create_publisher( DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -357,7 +291,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // initialize command subscriber - if (use_stamped_vel_) + if (params_.use_stamped_vel) { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -409,8 +343,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_publisher_); 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 = params_.odom_frame_id; + odometry_message.child_frame_id = params_.base_frame_id; // initialize odom values zeros odometry_message.twist = @@ -421,13 +355,12 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { // 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 - if (odom_params_.enable_odom_tf) + if (params_.enable_odom_tf) { odometry_transform_publisher_ = get_node()->create_publisher( DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -438,8 +371,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & // 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 = params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; } // Create odom reset service @@ -456,8 +389,8 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); // Initialize the joints - const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); - const auto steering_result = get_steering(steering_joint_name_, steering_joint_); + const auto wheel_front_result = get_traction(params_.traction_joint_name, traction_joint_); + const auto steering_result = get_steering(params_.steering_joint_name, steering_joint_); if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) { return CallbackReturn::ERROR; @@ -644,12 +577,12 @@ std::tuple TricycleController::twist_to_ackermann(double Vx, dou if (Vx == 0 && theta_dot != 0) { // is spin action alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; + Ws = abs(theta_dot) * params_.wheelbase / params_.wheel_radius; return std::make_tuple(alpha, Ws); } - alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); - Ws = Vx / (wheel_params_.radius * std::cos(alpha)); + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, params_.wheelbase); + Ws = Vx / (params_.wheel_radius * std::cos(alpha)); return std::make_tuple(alpha, Ws); } diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml new file mode 100644 index 0000000000..68fc2202c2 --- /dev/null +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -0,0 +1,149 @@ +tricycle_controller: + traction_joint_name: { + type: string, + default_value: "", + description: "Name of the traction joint", + validation: { + not_empty<>: [] + } + } + steering_joint_name: { + type: string, + default_value: "", + description: "Name of the steering joint", + validation: { + not_empty<>: [] + } + } + wheelbase: { + type: double, + default_value: 0.0, + description: "Shortest distance between the front wheel and the rear axle. If this parameter is wrong, the robot will not behave correctly in curves.", + } + 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.", + } + 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: "base_link", + 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 true the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + enable_odom_tf: { + type: bool, + default_value: false, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + odom_only_twist: { + type: bool, + default_value: false, + description: "for doing the pose integration in separate node.", + } + cmd_vel_timeout: { + type: int, + default_value: 500, # milliseconds + description: "Timeout in milliseconds, after which input command on ``cmd_vel`` topic is considered staled.", + } + publish_ackermann_command: { + type: bool, + default_value: false, + description: "Publish limited commands.", + } + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "Size of the rolling window for calculation of mean velocity use in odometry.", + validation: { + gt<>: [0] + } + } + use_stamped_vel: { + type: bool, + default_value: true, + description: "Use stamp from input velocity message to calculate how old the command actually is.", + } + traction: + # "The positive limit will be applied to both directions. Setting different limits for positive " + # "and negative directions is not supported. Actuators are " + # "assumed to have the same constraints in both directions" + 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_deceleration: { + type: double, + default_value: .NAN, + } + min_deceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } + steering: + # "The positive limit will be applied to both directions. Setting different limits for positive " + # "and negative directions is not supported. Actuators are " + # "assumed to have the same constraints in both directions" + max_position: { + type: double, + default_value: .NAN, + } + min_position: { + type: double, + default_value: .NAN, + } + 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, + } diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 9298fae574..245523c844 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -26,8 +26,6 @@ TEST(TestLoadTricycleController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -39,6 +37,13 @@ TEST(TestLoadTricycleController, load_controller) ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index d8beedae42..018727e260 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -41,6 +41,12 @@ using lifecycle_msgs::msg::State; using testing::SizeIs; using testing::UnorderedElementsAre; +namespace +{ +const char traction_joint_name[] = "traction_joint"; +const char steering_joint_name[] = "steering_joint"; +} // namespace + class TestableTricycleController : public tricycle_controller::TricycleController { public: @@ -146,11 +152,28 @@ class TestTricycleController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::string traction_joint_name_init = traction_joint_name, + const std::string steering_joint_name_init = steering_joint_name, + const std::vector & parameters = {}) + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name_init))); + parameter_overrides.push_back( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name_init))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, urdf_, 0, "", node_options); + } + const std::string controller_name = "test_tricycle_controller"; std::unique_ptr controller_; - const std::string traction_joint_name = "traction_joint"; - const std::string steering_joint_name = "steering_joint"; double position_ = 0.1; double velocity_ = 0.2; @@ -172,42 +195,24 @@ class TestTricycleController : public ::testing::Test const std::string urdf_ = ""; }; -TEST_F(TestTricycleController, configure_fails_without_parameters) +TEST_F(TestTricycleController, init_fails_without_parameters) { const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) +TEST_F(TestTricycleController, init_fails_if_only_traction_or_steering_side_defined) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ( + InitController(traction_joint_name, std::string()), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + InitController(std::string(), steering_joint_name), controller_interface::return_type::ERROR); } TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -215,26 +220,22 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) auto cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); ASSERT_THAT( - cmd_if_conf.names, - UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + cmd_if_conf.names, UnorderedElementsAre( + std::string(traction_joint_name) + "/velocity", + std::string(steering_joint_name) + "/position")); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); ASSERT_THAT( - state_if_conf.names, - UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + state_if_conf.names, UnorderedElementsAre( + std::string(traction_joint_name) + "/velocity", + std::string(steering_joint_name) + "/position")); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestTricycleController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -242,15 +243,9 @@ TEST_F(TestTricycleController, activate_fails_without_resources_assigned) TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResources(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -258,15 +253,11 @@ TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) TEST_F(TestTricycleController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); + ASSERT_EQ( + InitController( + traction_joint_name, steering_joint_name, + {rclcpp::Parameter("wheelbase", 1.2), rclcpp::Parameter("wheel_radius", 0.12)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -307,15 +298,11 @@ TEST_F(TestTricycleController, cleanup) TEST_F(TestTricycleController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + traction_joint_name, steering_joint_name, + {rclcpp::Parameter("wheelbase", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface());