Skip to content
76 changes: 38 additions & 38 deletions controller_interface/test/test_force_torque_sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,20 +48,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names)
std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names();

// assign values to force
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_y{
sensor_name_, fts_interface_names_[1], &force_values_[1]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
auto force_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[0], &force_values_[0]);
auto force_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[1], &force_values_[1]);
auto force_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[2], &force_values_[2]);

// assign values to torque
hardware_interface::StateInterface torque_x{
sensor_name_, fts_interface_names_[3], &torque_values_[0]};
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
auto torque_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[3], &torque_values_[0]);
auto torque_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[4], &torque_values_[1]);
auto torque_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[5], &torque_values_[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down Expand Up @@ -132,16 +132,16 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names)
std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names();

// assign values to force.x and force.z
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
auto force_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[0], &force_values_[0]);
auto force_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[2], &force_values_[2]);

// assign values to torque.y and torque.z
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
auto torque_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[4], &torque_values_[1]);
auto torque_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[5], &torque_values_[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down Expand Up @@ -214,31 +214,31 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names)
ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z");

// assign values to force
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_y{
sensor_name_, fts_interface_names_[1], &force_values_[1]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
auto force_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[0], &force_values_[0]);
auto force_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[1], &force_values_[1]);
auto force_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[2], &force_values_[2]);

// assign values to torque
hardware_interface::StateInterface torque_x{
sensor_name_, fts_interface_names_[3], &torque_values_[0]};
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
auto torque_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[3], &torque_values_[0]);
auto torque_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[4], &torque_values_[1]);
auto torque_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[5], &torque_values_[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;

// insert the interfaces in jumbled sequence
temp_state_interfaces.emplace_back(torque_y);
temp_state_interfaces.emplace_back(force_z);
temp_state_interfaces.emplace_back(force_x);
temp_state_interfaces.emplace_back(torque_z);
temp_state_interfaces.emplace_back(torque_x);
temp_state_interfaces.emplace_back(force_y);
temp_state_interfaces.emplace_back(torque_y, nullptr);
temp_state_interfaces.emplace_back(force_z, nullptr);
temp_state_interfaces.emplace_back(force_x, nullptr);
temp_state_interfaces.emplace_back(torque_z, nullptr);
temp_state_interfaces.emplace_back(torque_x, nullptr);
temp_state_interfaces.emplace_back(force_y, nullptr);

// now call the function to make them in order like interface_names
force_torque_sensor_->assign_loaned_state_interfaces(temp_state_interfaces);
Expand Down
65 changes: 39 additions & 26 deletions controller_interface/test/test_gps_sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,16 +46,21 @@ struct GPSSensorTest : public testing::Test
gps_sensor_name};
std::vector<std::string> full_interface_names;

hardware_interface::StateInterface gps_state{
gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)};
hardware_interface::StateInterface gps_service{
gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)};
hardware_interface::StateInterface latitude{
gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)};
hardware_interface::StateInterface longitude{
gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)};
hardware_interface::StateInterface altitude{
gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)};
hardware_interface::StateInterface::SharedPtr gps_state =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0));
hardware_interface::StateInterface::SharedPtr gps_service =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1));
hardware_interface::StateInterface::SharedPtr latitude =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2));
hardware_interface::StateInterface::SharedPtr longitude =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3));
hardware_interface::StateInterface::SharedPtr altitude =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4));
std::vector<hardware_interface::LoanedStateInterface> state_interface;
};

Expand Down Expand Up @@ -144,22 +149,30 @@ struct GPSSensorWithCovarianceTest : public testing::Test
gps_sensor_name};
std::vector<std::string> full_interface_names;

hardware_interface::StateInterface gps_state{
gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)};
hardware_interface::StateInterface gps_service{
gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)};
hardware_interface::StateInterface latitude{
gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)};
hardware_interface::StateInterface longitude{
gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)};
hardware_interface::StateInterface altitude{
gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)};
hardware_interface::StateInterface latitude_covariance{
gps_sensor_name, gps_interface_names.at(5), &gps_states.at(5)};
hardware_interface::StateInterface longitude_covariance{
gps_sensor_name, gps_interface_names.at(6), &gps_states.at(6)};
hardware_interface::StateInterface altitude_covariance{
gps_sensor_name, gps_interface_names.at(7), &gps_states.at(7)};
hardware_interface::StateInterface::SharedPtr gps_state =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0));
hardware_interface::StateInterface::SharedPtr gps_service =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1));
hardware_interface::StateInterface::SharedPtr latitude =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2));
hardware_interface::StateInterface::SharedPtr longitude =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3));
hardware_interface::StateInterface::SharedPtr altitude =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4));
hardware_interface::StateInterface::SharedPtr latitude_covariance =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(5), &gps_states.at(5));
hardware_interface::StateInterface::SharedPtr longitude_covariance =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(6), &gps_states.at(6));
hardware_interface::StateInterface::SharedPtr altitude_covariance =
std::make_shared<hardware_interface::StateInterface>(
gps_sensor_name, gps_interface_names.at(7), &gps_states.at(7));
std::vector<hardware_interface::LoanedStateInterface> state_interface;
};

Expand Down
40 changes: 20 additions & 20 deletions controller_interface/test/test_imu_sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,30 +47,30 @@ TEST_F(IMUSensorTest, validate_all)
std::vector<std::string> interface_names = imu_sensor_->get_state_interface_names();

// assign values to orientation
hardware_interface::StateInterface orientation_x{
sensor_name_, imu_interface_names_[0], &orientation_values_[0]};
hardware_interface::StateInterface orientation_y{
sensor_name_, imu_interface_names_[1], &orientation_values_[1]};
hardware_interface::StateInterface orientation_z{
sensor_name_, imu_interface_names_[2], &orientation_values_[2]};
hardware_interface::StateInterface orientation_w{
sensor_name_, imu_interface_names_[3], &orientation_values_[4]};
auto orientation_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[0], &orientation_values_[0]);
auto orientation_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[1], &orientation_values_[1]);
auto orientation_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[2], &orientation_values_[2]);
auto orientation_w = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[3], &orientation_values_[4]);

// assign values to angular velocity
hardware_interface::StateInterface angular_velocity_x{
sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]};
hardware_interface::StateInterface angular_velocity_y{
sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]};
hardware_interface::StateInterface angular_velocity_z{
sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]};
auto angular_velocity_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]);
auto angular_velocity_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]);
auto angular_velocity_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]);

// assign values to linear acceleration
hardware_interface::StateInterface linear_acceleration_x{
sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]};
hardware_interface::StateInterface linear_acceleration_y{
sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]};
hardware_interface::StateInterface linear_acceleration_z{
sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]};
auto linear_acceleration_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]);
auto linear_acceleration_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]);
auto linear_acceleration_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down
15 changes: 9 additions & 6 deletions controller_interface/test/test_led_rgb_device.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,16 +47,19 @@ TEST_F(LedDeviceTest, validate_all)
std::vector<std::string> interface_names = led_device_->get_command_interface_names();

// Assign values to position
hardware_interface::CommandInterface led_r{device_name_, interface_names_[0], &led_values_[0]};
hardware_interface::CommandInterface led_g{device_name_, interface_names_[1], &led_values_[1]};
hardware_interface::CommandInterface led_b{device_name_, interface_names_[2], &led_values_[2]};
auto led_r = std::make_shared<hardware_interface::CommandInterface>(
device_name_, interface_names_[0], &led_values_[0]);
auto led_g = std::make_shared<hardware_interface::CommandInterface>(
device_name_, interface_names_[1], &led_values_[1]);
auto led_b = std::make_shared<hardware_interface::CommandInterface>(
device_name_, interface_names_[2], &led_values_[2]);

// Create command interface vector in jumbled order
std::vector<hardware_interface::LoanedCommandInterface> temp_command_interfaces;
temp_command_interfaces.reserve(3);
temp_command_interfaces.emplace_back(led_r);
temp_command_interfaces.emplace_back(led_g);
temp_command_interfaces.emplace_back(led_b);
temp_command_interfaces.emplace_back(led_r, nullptr);
temp_command_interfaces.emplace_back(led_g, nullptr);
temp_command_interfaces.emplace_back(led_b, nullptr);

// Assign interfaces
led_device_->assign_loaned_command_interfaces(temp_command_interfaces);
Expand Down
28 changes: 14 additions & 14 deletions controller_interface/test/test_pose_sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,22 +45,22 @@ TEST_F(PoseSensorTest, validate_all)
std::vector<std::string> interface_names = pose_sensor_->get_state_interface_names();

// Assign values to position
hardware_interface::StateInterface position_x{
sensor_name_, interface_names_[0], &position_values_[0]};
hardware_interface::StateInterface position_y{
sensor_name_, interface_names_[1], &position_values_[1]};
hardware_interface::StateInterface position_z{
sensor_name_, interface_names_[2], &position_values_[2]};
auto position_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, interface_names_[0], &position_values_[0]);
auto position_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, interface_names_[1], &position_values_[1]);
auto position_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, interface_names_[2], &position_values_[2]);

// Assign values to orientation
hardware_interface::StateInterface orientation_x{
sensor_name_, interface_names_[3], &orientation_values_[0]};
hardware_interface::StateInterface orientation_y{
sensor_name_, interface_names_[4], &orientation_values_[1]};
hardware_interface::StateInterface orientation_z{
sensor_name_, interface_names_[5], &orientation_values_[2]};
hardware_interface::StateInterface orientation_w{
sensor_name_, interface_names_[6], &orientation_values_[3]};
auto orientation_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, interface_names_[3], &orientation_values_[0]);
auto orientation_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, interface_names_[4], &orientation_values_[1]);
auto orientation_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, interface_names_[5], &orientation_values_[2]);
auto orientation_w = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, interface_names_[6], &orientation_values_[3]);

// Create state interface vector in jumbled order
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,17 +38,20 @@ TEST_F(SemanticCommandInterfaceTest, validate_command_interfaces)
std::vector<double> interface_values = {
std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
std::numeric_limits<double>::quiet_NaN()};
hardware_interface::CommandInterface cmd_interface_1{component_name_, "1", &interface_values[0]};
hardware_interface::CommandInterface cmd_interface_2{component_name_, "2", &interface_values[1]};
hardware_interface::CommandInterface cmd_interface_3{component_name_, "3", &interface_values[2]};
auto cmd_interface_1 = std::make_shared<hardware_interface::CommandInterface>(
component_name_, "1", &interface_values[0]);
auto cmd_interface_2 = std::make_shared<hardware_interface::CommandInterface>(
component_name_, "2", &interface_values[1]);
auto cmd_interface_3 = std::make_shared<hardware_interface::CommandInterface>(
component_name_, "3", &interface_values[2]);

// create local command interface vector
std::vector<hardware_interface::LoanedCommandInterface> temp_command_interfaces;
temp_command_interfaces.reserve(3);
// insert the interfaces in jumbled sequence
temp_command_interfaces.emplace_back(cmd_interface_1);
temp_command_interfaces.emplace_back(cmd_interface_3);
temp_command_interfaces.emplace_back(cmd_interface_2);
temp_command_interfaces.emplace_back(cmd_interface_1, nullptr);
temp_command_interfaces.emplace_back(cmd_interface_3, nullptr);
temp_command_interfaces.emplace_back(cmd_interface_2, nullptr);

// now call the function to make them in order like interface_names
EXPECT_TRUE(semantic_component_->assign_loaned_command_interfaces(temp_command_interfaces));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -95,9 +95,12 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces)
std::vector<double> interface_values = {1.1, 3.3, 5.5};

// assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5
hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]};
hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]};
hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]};
auto interface_1 = std::make_shared<hardware_interface::StateInterface>(
component_name_, "1", &interface_values[0]);
auto interface_3 = std::make_shared<hardware_interface::StateInterface>(
component_name_, "3", &interface_values[1]);
auto interface_5 = std::make_shared<hardware_interface::StateInterface>(
component_name_, "5", &interface_values[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down
Loading