Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -82,11 +82,11 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
ASSERT_EQ(reference_interfaces.size(), reference_interface_names_.size());
for (size_t i = 0; i < reference_interface_names_.size(); ++i)
{
const std::string ref_itf_prefix_name =
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names_[i];
EXPECT_EQ(
reference_interfaces[i]->get_name(),
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
Expand Down Expand Up @@ -186,7 +186,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
1.4179821977774734, COMMON_THRESHOLD);

EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size());
for (const auto & interface : controller_->reference_interfaces_)
{
EXPECT_TRUE(std::isnan(interface));
Expand Down Expand Up @@ -226,7 +226,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
1.4179821977774734, COMMON_THRESHOLD);

EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size());
for (const auto & interface : controller_->reference_interfaces_)
{
EXPECT_TRUE(std::isnan(interface));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -303,7 +303,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test

std::array<double, 4> joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}};
std::array<double, 4> joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}};
std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};
std::array<std::string, 2> reference_interface_names_ = {{"linear", "angular"}};
std::string steering_interface_name_ = "position";
// defined in setup
std::string traction_interface_name_ = "";
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -84,11 +84,11 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
ASSERT_EQ(reference_interfaces.size(), reference_interface_names_.size());
for (size_t i = 0; i < reference_interface_names_.size(); ++i)
{
const std::string ref_itf_prefix_name =
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names_[i];
EXPECT_EQ(
reference_interfaces[i]->get_name(),
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,11 +68,11 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
ASSERT_EQ(reference_interfaces.size(), reference_interface_names_.size());
for (size_t i = 0; i < reference_interface_names_.size(); ++i)
{
const std::string ref_itf_prefix_name =
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names_[i];
EXPECT_EQ(
reference_interfaces[i]->get_name(),
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
Expand Down Expand Up @@ -165,7 +165,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
COMMON_THRESHOLD);

EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size());
for (const auto & interface : controller_->reference_interfaces_)
{
EXPECT_TRUE(std::isnan(interface));
Expand Down Expand Up @@ -198,7 +198,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
COMMON_THRESHOLD);

EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size());
for (const auto & interface : controller_->reference_interfaces_)
{
EXPECT_TRUE(std::isnan(interface));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -272,7 +272,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test

std::array<double, 2> joint_state_values_ = {{3.3, 0.5}};
std::array<double, 2> joint_command_values_ = {{1.1, 2.2}};
std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};
std::array<std::string, 2> reference_interface_names_ = {{"linear", "angular"}};
std::string steering_interface_name_ = "position";

// defined in setup
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,11 +71,11 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
ASSERT_EQ(reference_interfaces.size(), reference_interface_names_.size());
for (size_t i = 0; i < reference_interface_names_.size(); ++i)
{
const std::string ref_itf_prefix_name =
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names_[i];
EXPECT_EQ(
reference_interfaces[i]->get_name(),
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,11 +66,11 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces)

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
ASSERT_EQ(reference_interfaces.size(), reference_interface_names_.size());
for (size_t i = 0; i < reference_interface_names_.size(); ++i)
{
const std::string ref_itf_prefix_name =
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names_[i];
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
EXPECT_EQ(
reference_interfaces[i]->get_name(),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -320,7 +320,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test
std::array<double, 4> joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}};
std::array<double, 4> joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}};

std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};
std::array<std::string, 2> reference_interface_names_ = {{"linear", "angular"}};
std::string steering_interface_name_ = "position";
// defined in setup
std::string traction_interface_name_ = "";
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -75,11 +75,11 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces)

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
ASSERT_EQ(reference_interfaces.size(), reference_interface_names_.size());
for (size_t i = 0; i < reference_interface_names_.size(); ++i)
{
const std::string ref_itf_prefix_name =
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names_[i];
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
EXPECT_EQ(
reference_interfaces[i]->get_name(),
Expand Down Expand Up @@ -175,7 +175,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic)
COMMON_THRESHOLD);

EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size());
for (const auto & interface : controller_->reference_interfaces_)
{
EXPECT_TRUE(std::isnan(interface));
Expand Down Expand Up @@ -212,7 +212,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained)
COMMON_THRESHOLD);

EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size());
for (const auto & interface : controller_->reference_interfaces_)
{
EXPECT_TRUE(std::isnan(interface));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -288,7 +288,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test

std::array<double, 3> joint_state_values_{{0.5, 0.5, 0.0}};
std::array<double, 3> joint_command_values_{{1.1, 3.3, 2.2}};
std::array<std::string, 2> joint_reference_interfaces_{{"linear", "angular"}};
std::array<std::string, 2> reference_interface_names_{{"linear", "angular"}};
std::string steering_interface_name_ = "position";
// defined in setup
std::string traction_interface_name_ = "";
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,11 +77,11 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces)

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
ASSERT_EQ(reference_interfaces.size(), reference_interface_names_.size());
for (size_t i = 0; i < reference_interface_names_.size(); ++i)
{
const std::string ref_itf_prefix_name =
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names_[i];
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
EXPECT_EQ(
reference_interfaces[i]->get_name(),
Expand Down