Skip to content

Commit

Permalink
Revert "Fix conflicts, still need to adjust InitController"
Browse files Browse the repository at this point in the history 
This reverts commit 21ed794.
  • Loading branch information
@bmagyar
bmagyar committed Feb 7, 2024
1 parent 21ed794 commit a92f048
Showing 1 changed file with 157 additions and 1 deletion.
158 changes: 157 additions & 1 deletion diff_drive_controller/test/test_diff_drive_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -216,29 +216,65 @@ class TestDiffDriveController : public ::testing::Test

TEST_F(TestDiffDriveController, init_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);
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::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)
{
<<<<<<< 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);
}

TEST_F(TestDiffDriveController, configure_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);
}

TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified)
{
<<<<<<< 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_THAT(
Expand All @@ -251,6 +287,12 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified)

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 @@ -276,6 +318,12 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names

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 Down Expand Up @@ -303,6 +351,12 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp

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 Down Expand Up @@ -330,6 +384,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names
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 Down Expand Up @@ -357,6 +418,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name
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 Down Expand Up @@ -386,6 +454,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names
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 Down Expand Up @@ -413,14 +488,31 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name

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);
assignResourcesPosFeedback();
Expand All @@ -429,47 +521,99 @@ 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();
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
}

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();
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
}

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());
auto state = controller_->get_node()->configure();
Expand Down Expand Up @@ -509,11 +653,23 @@ 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 a92f048

Please sign in to comment.