Skip to content

Commit

Permalink
Adjust tests and add wheel names size verification to init
Browse files Browse the repository at this point in the history
  • Loading branch information
@bmagyar
bmagyar committed Feb 9, 2024
1 parent a92f048 commit 8a20e2a
Show file tree
Hide file tree
Showing 2 changed files with 26 additions and 170 deletions.
9 changes: 9 additions & 0 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,15 @@ controller_interface::CallbackReturn DiffDriveController::on_init()
// Create the parameter listener and get the parameters
param_listener_ = std::make_shared<ParamListener>(get_node());
params_ = param_listener_->get_params();

if (params_.left_wheel_names.size() != params_.right_wheel_names.size())
{
RCLCPP_ERROR(
get_node()->get_logger(),
"The number of left wheels [%zu] and the number of right wheels [%zu] are different",
params_.left_wheel_names.size(), params_.right_wheel_names.size());
return controller_interface::CallbackReturn::ERROR;
}
}
catch (const std::exception & e)
{
Expand Down
187 changes: 17 additions & 170 deletions diff_drive_controller/test/test_diff_drive_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,7 +188,7 @@ class TestDiffDriveController : public ::testing::Test
parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end());
node_options.parameter_overrides(parameter_overrides);

return controller_->init(controller_name, urdf_, 0, ns, node_options);
return controller_->init(controller_name, ns, node_options);
}

const std::string controller_name = "test_diff_drive_controller";
Expand All @@ -214,67 +214,32 @@ class TestDiffDriveController : public ::testing::Test
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_publisher;
};

TEST_F(TestDiffDriveController, init_fails_without_parameters)
TEST_F(TestDiffDriveController, fails_without_parameters)
{
<<<<<<< HEAD
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);
=======
const auto ret = controller_->init(controller_name, urdf_, 0);
ASSERT_EQ(ret, controller_interface::return_type::ERROR);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))
}

TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined)
TEST_F(TestDiffDriveController, fails_with_only_left_side_defined)
{
<<<<<<< HEAD
const auto ret = controller_->init(controller_name);
ASSERT_EQ(ret, controller_interface::return_type::OK);
=======
ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR);
const auto ret = InitController(left_wheel_names, {});
ASSERT_EQ(ret, controller_interface::return_type::ERROR);
}

TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size)
TEST_F(TestDiffDriveController, fails_with_mismatching_wheel_side_size)
{
<<<<<<< HEAD
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);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
const auto ret = InitController(left_wheel_names, extended_right_wheel_names);
ASSERT_EQ(ret, controller_interface::return_type::ERROR);
}

TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified)
{
<<<<<<< HEAD
const auto ret = controller_->init(controller_name);
const auto ret = InitController(left_wheel_names, right_wheel_names);
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);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

ASSERT_THAT(
Expand All @@ -284,15 +249,8 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified)
controller_->command_interface_configuration().names,
SizeIs(left_wheel_names.size() + right_wheel_names.size()));
}

TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace)
{
<<<<<<< HEAD
const auto ret = controller_->init(controller_name);
ASSERT_EQ(ret, controller_interface::return_type::OK);

=======
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))
std::string odom_id = "odom";
std::string base_link_id = "base_link";
std::string frame_prefix = "test_prefix";
Expand All @@ -311,19 +269,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
std::string test_odom_frame_id = odometry_message.header.frame_id;
std::string test_base_frame_id = odometry_message.child_frame_id;
/* tf_frame_prefix_enable is false so no modifications to the frame id's */
// tf_frame_prefix_enable is false so no modifications to the frame ids
ASSERT_EQ(test_odom_frame_id, odom_id);
ASSERT_EQ(test_base_frame_id, base_link_id);
}

TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace)
{
<<<<<<< HEAD
const auto ret = controller_->init(controller_name);
ASSERT_EQ(ret, controller_interface::return_type::OK);

=======
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))
std::string odom_id = "odom";
std::string base_link_id = "base_link";
std::string frame_prefix = "test_prefix";
Expand All @@ -343,20 +295,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp
std::string test_odom_frame_id = odometry_message.header.frame_id;
std::string test_base_frame_id = odometry_message.child_frame_id;

/* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame
* id's */
// tf_frame_prefix_enable true and frame_prefix not blank, should be appended to the frame ids
ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id);
ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id);
}

TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace)
{
<<<<<<< HEAD
const auto ret = controller_->init(controller_name);
ASSERT_EQ(ret, controller_interface::return_type::OK);

=======
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))
std::string odom_id = "odom";
std::string base_link_id = "base_link";
std::string frame_prefix = "";
Expand All @@ -375,22 +320,14 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
std::string test_odom_frame_id = odometry_message.header.frame_id;
std::string test_base_frame_id = odometry_message.child_frame_id;
/* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame
* id's */
// tf_frame_prefix_enable true but frame_prefix blank, should not be appended to the frame ids
ASSERT_EQ(test_odom_frame_id, odom_id);
ASSERT_EQ(test_base_frame_id, base_link_id);
}

TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace)
{
std::string test_namespace = "/test_namespace";

<<<<<<< HEAD
const auto ret = controller_->init(controller_name, test_namespace);
ASSERT_EQ(ret, controller_interface::return_type::OK);

=======
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))
std::string odom_id = "odom";
std::string base_link_id = "base_link";
std::string frame_prefix = "test_prefix";
Expand All @@ -410,21 +347,14 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
std::string test_odom_frame_id = odometry_message.header.frame_id;
std::string test_base_frame_id = odometry_message.child_frame_id;
/* tf_frame_prefix_enable is false so no modifications to the frame id's */
// tf_frame_prefix_enable is false so no modifications to the frame ids
ASSERT_EQ(test_odom_frame_id, odom_id);
ASSERT_EQ(test_base_frame_id, base_link_id);
}

TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace)
{
std::string test_namespace = "/test_namespace";

<<<<<<< HEAD
const auto ret = controller_->init(controller_name, test_namespace);
ASSERT_EQ(ret, controller_interface::return_type::OK);

=======
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))
std::string odom_id = "odom";
std::string base_link_id = "base_link";
std::string frame_prefix = "test_prefix";
Expand All @@ -445,22 +375,15 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names
std::string test_odom_frame_id = odometry_message.header.frame_id;
std::string test_base_frame_id = odometry_message.child_frame_id;

/* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame
* id's instead of the namespace*/
// tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame
// ids instead of the namespace
ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id);
ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id);
}

TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace)
{
std::string test_namespace = "/test_namespace";

<<<<<<< HEAD
const auto ret = controller_->init(controller_name, test_namespace);
ASSERT_EQ(ret, controller_interface::return_type::OK);

=======
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))
std::string odom_id = "odom";
std::string base_link_id = "base_link";
std::string frame_prefix = "";
Expand All @@ -480,38 +403,23 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
std::string test_odom_frame_id = odometry_message.header.frame_id;
std::string test_base_frame_id = odometry_message.child_frame_id;
/* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the
* frame id's */
// tf_frame_prefix_enable true but frame_prefix blank, namespace should be appended to the frame
// ids
ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id);
ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id);
}

TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned)
{
<<<<<<< HEAD
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);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
}

TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned)
{
<<<<<<< HEAD
const auto ret = controller_->init(controller_name);
ASSERT_EQ(ret, controller_interface::return_type::OK);
=======
ASSERT_EQ(InitController(), controller_interface::return_type::OK);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

// We implicitly test that by default position feedback is required
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
Expand All @@ -521,48 +429,23 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned)

TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned)
{
<<<<<<< HEAD
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);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
assignResourcesVelFeedback();
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
}

TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1)
{
<<<<<<< HEAD
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);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
assignResourcesPosFeedback();
Expand All @@ -571,23 +454,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1)

TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2)
{
<<<<<<< HEAD
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);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
assignResourcesVelFeedback();
Expand All @@ -596,23 +467,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2)

TEST_F(TestDiffDriveController, cleanup)
{
<<<<<<< HEAD
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);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(controller_->get_node()->get_node_base_interface());
Expand Down Expand Up @@ -653,23 +512,11 @@ TEST_F(TestDiffDriveController, cleanup)

TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
{
<<<<<<< HEAD
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);
>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958))

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(controller_->get_node()->get_node_base_interface());
Expand Down

0 comments on commit 8a20e2a

Please sign in to comment.