diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index b944ff1e88..aa71c70745 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -53,8 +53,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) + test/test_diff_drive_controller.cpp) target_link_libraries(test_diff_drive_controller diff_drive_controller ) @@ -69,8 +68,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_diff_drive_controller + add_rostest_with_parameters_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml ) ament_target_dependencies(test_load_diff_drive_controller controller_manager 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 5923a27d4d..56d4970945 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 @@ -110,6 +110,12 @@ class DiffDriveController : public controller_interface::ControllerInterface // Parameters from ROS for diff_drive_controller std::shared_ptr param_listener_; Params params_; + /* Number of wheels on each side 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 for 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. */ + int wheels_per_side_; Odometry odometry_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 23097c0df2..b1c74625cb 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -150,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 < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -167,8 +167,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= static_cast(params_.wheels_per_side); - right_feedback_mean /= static_cast(params_.wheels_per_side); + left_feedback_mean /= static_cast(wheels_per_side_); + right_feedback_mean /= static_cast(wheels_per_side_); if (params_.position_feedback) { @@ -258,7 +258,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 < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(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); @@ -287,12 +287,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.left_wheel_names.empty()) - { - RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); - return controller_interface::CallbackReturn::ERROR; - } - 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; @@ -322,7 +316,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - params_.wheels_per_side = params_.left_wheel_names.size(); + wheels_per_side_ = static_cast(params_.left_wheel_names.size()); if (publish_limited_velocity_) { diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index e878ad5481..509a7ab1e1 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -2,23 +2,24 @@ diff_drive_controller: left_wheel_names: { type: string_array, default_value: [], - description: "Link names of the left side wheels", + description: "Names of the left side wheels' joints", + validation: { + not_empty<>: [] + } } right_wheel_names: { type: string_array, default_value: [], - description: "Link names of the right side wheels", + description: "Names of the right side wheels' joints", + validation: { + not_empty<>: [] + } } 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 side 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 for 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, diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index a2149eb6bc..bfbf8f2d19 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -2,7 +2,6 @@ test_diff_drive_controller: ros__parameters: left_wheel_names: ["left_wheels"] right_wheel_names: ["right_wheels"] - write_op_modes: ["motor_controller"] wheel_separation: 0.40 wheels_per_side: 1 # actually 2, but both are controlled by 1 signal @@ -21,7 +20,7 @@ test_diff_drive_controller: open_loop: true enable_odom_tf: true - cmd_vel_timeout: 500 # milliseconds + cmd_vel_timeout: 0.5 # seconds publish_limited_velocity: true velocity_rolling_window_size: 10 diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index db89d4f873..61ab57c0a4 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -36,6 +36,12 @@ using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + class TestableDiffDriveController : public diff_drive_controller::DiffDriveController { public: @@ -166,11 +172,28 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, ns, node_options); + } + const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; - const std::vector left_wheel_names = {"left_wheel_joint"}; - const std::vector right_wheel_names = {"right_wheel_joint"}; std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; @@ -191,59 +214,31 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Publisher::SharedPtr velocity_publisher; }; -TEST_F(TestDiffDriveController, configure_fails_without_parameters) +TEST_F(TestDiffDriveController, init_fails_without_parameters) { const auto ret = controller_->init(controller_name); - 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(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) +TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR); } TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - - auto extended_right_wheel_names = right_wheel_names; - extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); + ASSERT_EQ( + InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -257,26 +252,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -290,26 +277,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -325,26 +304,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -361,26 +332,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -396,26 +360,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -433,26 +390,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -467,13 +417,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + 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); @@ -481,15 +425,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name); - 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("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -497,15 +435,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -514,15 +448,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); @@ -531,15 +461,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -548,15 +474,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 0.1)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -597,15 +519,11 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 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()); diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 660208c560..4c9d2f984f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -24,8 +24,6 @@ TEST(TestLoadDiffDriveController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -36,6 +34,14 @@ TEST(TestLoadDiffDriveController, load_controller) ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); rclcpp::shutdown(); + return result; }