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 @@ -146,63 +146,63 @@ class AckermannSteeringControllerFixture : public ::testing::Test
traction_interface_name_ = "velocity";
}

std::vector<hardware_interface::LoanedCommandInterface> command_ifs;
std::vector<hardware_interface::LoanedCommandInterface> loaned_command_ifs;
command_itfs_.reserve(joint_command_values_.size());
command_ifs.reserve(joint_command_values_.size());
loaned_command_ifs.reserve(joint_command_values_.size());

command_itfs_.emplace_back(
hardware_interface::CommandInterface(
std::make_shared<hardware_interface::CommandInterface>(
traction_joints_names_[0], traction_interface_name_,
&joint_command_values_[CMD_TRACTION_RIGHT_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());
loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr);

command_itfs_.emplace_back(
hardware_interface::CommandInterface(
std::make_shared<hardware_interface::CommandInterface>(
traction_joints_names_[1], steering_interface_name_,
&joint_command_values_[CMD_TRACTION_LEFT_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());
loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr);

command_itfs_.emplace_back(
hardware_interface::CommandInterface(
std::make_shared<hardware_interface::CommandInterface>(
steering_joints_names_[0], steering_interface_name_,
&joint_command_values_[CMD_STEER_RIGHT_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());
loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr);

command_itfs_.emplace_back(
hardware_interface::CommandInterface(
std::make_shared<hardware_interface::CommandInterface>(
steering_joints_names_[1], steering_interface_name_,
&joint_command_values_[CMD_STEER_LEFT_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());
loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr);

std::vector<hardware_interface::LoanedStateInterface> state_ifs;
std::vector<hardware_interface::LoanedStateInterface> loaned_state_ifs;
state_itfs_.reserve(joint_state_values_.size());
state_ifs.reserve(joint_state_values_.size());
loaned_state_ifs.reserve(joint_state_values_.size());

state_itfs_.emplace_back(
hardware_interface::StateInterface(
std::make_shared<hardware_interface::StateInterface>(
traction_joints_names_[0], traction_interface_name_,
&joint_state_values_[STATE_TRACTION_RIGHT_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());
loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr);

state_itfs_.emplace_back(
hardware_interface::StateInterface(
std::make_shared<hardware_interface::StateInterface>(
traction_joints_names_[1], traction_interface_name_,
&joint_state_values_[STATE_TRACTION_LEFT_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());
loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr);

state_itfs_.emplace_back(
hardware_interface::StateInterface(
std::make_shared<hardware_interface::StateInterface>(
steering_joints_names_[0], steering_interface_name_,
&joint_state_values_[STATE_STEER_RIGHT_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());
loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr);

state_itfs_.emplace_back(
hardware_interface::StateInterface(
std::make_shared<hardware_interface::StateInterface>(
steering_joints_names_[1], steering_interface_name_,
&joint_state_values_[STATE_STEER_LEFT_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());
loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr);

controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs));
}

void subscribe_and_get_messages(ControllerStateMsg & msg)
Expand Down Expand Up @@ -305,8 +305,8 @@ class AckermannSteeringControllerFixture : public ::testing::Test
std::string traction_interface_name_ = "";
std::string preceding_prefix_ = "pid_controller";

std::vector<hardware_interface::StateInterface> state_itfs_;
std::vector<hardware_interface::CommandInterface> command_itfs_;
std::vector<hardware_interface::StateInterface::SharedPtr> state_itfs_;
std::vector<hardware_interface::CommandInterface::SharedPtr> command_itfs_;

// Test related parameters
std::unique_ptr<TestableAckermannSteeringController> controller_;
Expand Down
26 changes: 13 additions & 13 deletions admittance_controller/test/test_admittance_controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -171,32 +171,32 @@ class AdmittanceControllerTest : public ::testing::Test

void assign_interfaces()
{
std::vector<hardware_interface::LoanedCommandInterface> command_ifs;
std::vector<hardware_interface::LoanedCommandInterface> loaned_command_ifs;
command_itfs_.reserve(joint_command_values_.size());
command_ifs.reserve(joint_command_values_.size());
loaned_command_ifs.reserve(joint_command_values_.size());

for (auto i = 0u; i < joint_command_values_.size(); ++i)
{
command_itfs_.emplace_back(
hardware_interface::CommandInterface(
std::make_shared<hardware_interface::CommandInterface>(
joint_names_[i], command_interface_types_[0], &joint_command_values_[i]));
command_ifs.emplace_back(command_itfs_.back());
loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr);
}

auto sc_fts = semantic_components::ForceTorqueSensor(ft_sensor_name_);
fts_state_names_ = sc_fts.get_state_interface_names();
std::vector<hardware_interface::LoanedStateInterface> state_ifs;
std::vector<hardware_interface::LoanedStateInterface> loaned_state_ifs;

const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size();
state_itfs_.reserve(num_state_ifs);
state_ifs.reserve(num_state_ifs);
loaned_state_ifs.reserve(num_state_ifs);

for (auto i = 0u; i < joint_state_values_.size(); ++i)
{
state_itfs_.emplace_back(
hardware_interface::StateInterface(
std::make_shared<hardware_interface::StateInterface>(
joint_names_[i], state_interface_types_[0], &joint_state_values_[i]));
state_ifs.emplace_back(state_itfs_.back());
loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr);
}

std::vector<std::string> fts_itf_names = {"force.x", "force.y", "force.z",
Expand All @@ -205,12 +205,12 @@ class AdmittanceControllerTest : public ::testing::Test
for (auto i = 0u; i < fts_state_names_.size(); ++i)
{
state_itfs_.emplace_back(
hardware_interface::StateInterface(
std::make_shared<hardware_interface::StateInterface>(
ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i]));
state_ifs.emplace_back(state_itfs_.back());
loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr);
}

controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs));
}

void broadcast_tfs()
Expand Down Expand Up @@ -386,8 +386,8 @@ class AdmittanceControllerTest : public ::testing::Test
std::array<double, 6> fts_state_values_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
std::vector<std::string> fts_state_names_;

std::vector<hardware_interface::StateInterface> state_itfs_;
std::vector<hardware_interface::CommandInterface> command_itfs_;
std::vector<hardware_interface::StateInterface::SharedPtr> state_itfs_;
std::vector<hardware_interface::CommandInterface::SharedPtr> command_itfs_;

// Test related parameters
std::unique_ptr<TestableAdmittanceController> controller_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -144,39 +144,39 @@ class BicycleSteeringControllerFixture : public ::testing::Test
traction_interface_name_ = "velocity";
}

std::vector<hardware_interface::LoanedCommandInterface> command_ifs;
std::vector<hardware_interface::LoanedCommandInterface> loaned_command_ifs;
command_itfs_.reserve(joint_command_values_.size());
command_ifs.reserve(joint_command_values_.size());
loaned_command_ifs.reserve(joint_command_values_.size());

command_itfs_.emplace_back(
hardware_interface::CommandInterface(
std::make_shared<hardware_interface::CommandInterface>(
traction_joints_names_[0], traction_interface_name_,
&joint_command_values_[CMD_TRACTION_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());
loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr);

command_itfs_.emplace_back(
hardware_interface::CommandInterface(
std::make_shared<hardware_interface::CommandInterface>(
steering_joints_names_[0], steering_interface_name_,
&joint_command_values_[CMD_STEER_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());
loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr);

std::vector<hardware_interface::LoanedStateInterface> state_ifs;
std::vector<hardware_interface::LoanedStateInterface> loaned_state_ifs;
state_itfs_.reserve(joint_state_values_.size());
state_ifs.reserve(joint_state_values_.size());
loaned_state_ifs.reserve(joint_state_values_.size());

state_itfs_.emplace_back(
hardware_interface::StateInterface(
std::make_shared<hardware_interface::StateInterface>(
traction_joints_names_[0], traction_interface_name_,
&joint_state_values_[STATE_TRACTION_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());
loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr);

state_itfs_.emplace_back(
hardware_interface::StateInterface(
std::make_shared<hardware_interface::StateInterface>(
steering_joints_names_[0], steering_interface_name_,
&joint_state_values_[STATE_STEER_AXIS]));
state_ifs.emplace_back(state_itfs_.back());
loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr);

controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs));
}

void subscribe_and_get_messages(ControllerStateMsg & msg)
Expand Down Expand Up @@ -275,8 +275,8 @@ class BicycleSteeringControllerFixture : public ::testing::Test
std::string traction_interface_name_ = "";
std::string preceding_prefix_ = "pid_controller";

std::vector<hardware_interface::StateInterface> state_itfs_;
std::vector<hardware_interface::CommandInterface> command_itfs_;
std::vector<hardware_interface::StateInterface::SharedPtr> state_itfs_;
std::vector<hardware_interface::CommandInterface::SharedPtr> command_itfs_;

// Test related parameters
std::unique_ptr<TestableBicycleSteeringController> controller_;
Expand Down
6 changes: 3 additions & 3 deletions chained_filter_controller/test/test_chained_filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -123,18 +123,18 @@ TEST_F(ChainedFilterTest, UpdateFilter)
controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)),
controller_interface::return_type::OK);
// input state interface should not change
EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]);
EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]);
// output should be the same
auto state_if_exported_conf = controller_->export_state_interfaces();
ASSERT_THAT(state_if_exported_conf, SizeIs(1u));
EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]);

ASSERT_TRUE(joint_1_pos_.set_value(2.0));
ASSERT_TRUE(joint_1_pos_->set_value(2.0));
ASSERT_EQ(
controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)),
controller_interface::return_type::OK);
// input and output should have changed
EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]);
EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]);
EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55);
ASSERT_EQ(
controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)),
Expand Down
3 changes: 2 additions & 1 deletion chained_filter_controller/test/test_chained_filter.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ class ChainedFilterTest : public ::testing::Test
const std::vector<std::string> joint_names_ = {"wheel_left"};
std::vector<double> joint_states_ = {1.1};

StateInterface joint_1_pos_{joint_names_[0], HW_IF_POSITION, &joint_states_[0]};
StateInterface::SharedPtr joint_1_pos_ =
std::make_shared<StateInterface>(joint_names_[0], HW_IF_POSITION, &joint_states_[0]);
rclcpp::executors::SingleThreadedExecutor executor;
};

Expand Down
28 changes: 14 additions & 14 deletions chained_filter_controller/test/test_multiple_chained_filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,8 @@ void MultipleChainedFilterTest::SetUpController(
ASSERT_EQ(result, controller_interface::return_type::OK);

std::vector<LoanedStateInterface> state_ifs;
state_ifs.emplace_back(joint_1_pos_);
state_ifs.emplace_back(joint_2_pos_);
state_ifs.emplace_back(joint_1_pos_, nullptr);
state_ifs.emplace_back(joint_2_pos_, nullptr);
controller_->assign_interfaces({}, std::move(state_ifs));
executor.add_node(controller_->get_node()->get_node_base_interface());
}
Expand Down Expand Up @@ -118,23 +118,23 @@ TEST_F(MultipleChainedFilterTest, UpdateFilter_multiple_interfaces)
controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)),
controller_interface::return_type::OK);
// input state interface should not change
EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]);
EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]);
EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]);
EXPECT_EQ(joint_2_pos_->get_optional().value(), joint_states_[1]);
// output should be the same
auto state_if_exported_conf = controller_->export_state_interfaces();
ASSERT_THAT(state_if_exported_conf, SizeIs(2u));
EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]);
EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), joint_states_[1]);

ASSERT_TRUE(joint_1_pos_.set_value(2.0));
ASSERT_TRUE(joint_2_pos_.set_value(3.0));
ASSERT_TRUE(joint_1_pos_->set_value(2.0));
ASSERT_TRUE(joint_2_pos_->set_value(3.0));
ASSERT_EQ(
controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)),
controller_interface::return_type::OK);
// input and output should have changed
EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]);
EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]);
EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55);
EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]);
EXPECT_EQ(joint_2_pos_->get_optional().value(), joint_states_[1]);
EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), 2.6);

ASSERT_EQ(
Expand All @@ -155,23 +155,23 @@ TEST_F(MultipleChainedFilterTest, UpdateFilter_multiple_interfaces_config_per_in
controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)),
controller_interface::return_type::OK);
// input state interface should not change
EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]);
EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]);
EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]);
EXPECT_EQ(joint_2_pos_->get_optional().value(), joint_states_[1]);
// output should be the same
auto state_if_exported_conf = controller_->export_state_interfaces();
ASSERT_THAT(state_if_exported_conf, SizeIs(2u));
EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]);
EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), joint_states_[1]);

ASSERT_TRUE(joint_1_pos_.set_value(2.0));
ASSERT_TRUE(joint_2_pos_.set_value(2.8));
ASSERT_TRUE(joint_1_pos_->set_value(2.0));
ASSERT_TRUE(joint_2_pos_->set_value(2.8));
ASSERT_EQ(
controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)),
controller_interface::return_type::OK);
// input and output should have changed
EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]);
EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]);
EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55);
EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]);
EXPECT_EQ(joint_2_pos_->get_optional().value(), joint_states_[1]);
// second update call, mean of (2.2, 2.8)
EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), 2.5);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,10 @@ class MultipleChainedFilterTest : public ::testing::Test
const std::vector<std::string> joint_names_ = {"wheel_left", "wheel_right"};
std::vector<double> joint_states_ = {1.1, 2.2};

StateInterface joint_1_pos_{joint_names_[0], HW_IF_POSITION, &joint_states_[0]};
StateInterface joint_2_pos_{joint_names_[1], HW_IF_POSITION, &joint_states_[1]};
StateInterface::SharedPtr joint_1_pos_ =
std::make_shared<StateInterface>(joint_names_[0], HW_IF_POSITION, &joint_states_[0]);
StateInterface::SharedPtr joint_2_pos_ =
std::make_shared<StateInterface>(joint_names_[1], HW_IF_POSITION, &joint_states_[1]);
rclcpp::executors::SingleThreadedExecutor executor;
};

Expand Down
Loading