Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Distributed control base #10

Draft
wants to merge 4 commits into
base: master
Choose a base branch
from
Draft

Distributed control base #10

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
822dcbe
adapt to changes made to loandedInterface
mamueluth Nov 21, 2022
1cc40be
adjust tests to reflect changes in handles
mamueluth Dec 5, 2022
af6fdb5
joint_state_broadcaster if no local topic us "/" as prefix
mamueluth Jan 11, 2023
8d2e023
adapt admittance_controller tests
mamueluth Jan 11, 2023
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
10 changes: 5 additions & 5 deletions admittance_controller/test/test_admittance_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -201,7 +201,7 @@ class AdmittanceControllerTest : public ::testing::Test

for (auto i = 0u; i < joint_command_values_.size(); ++i)
{
command_itfs_.emplace_back(hardware_interface::CommandInterface(
command_itfs_.emplace_back(std::make_shared<hardware_interface::CommandInterface>(
joint_names_[i], command_interface_types_[0], &joint_command_values_[i]));
command_ifs.emplace_back(command_itfs_.back());
}
Expand All @@ -216,7 +216,7 @@ class AdmittanceControllerTest : public ::testing::Test

for (auto i = 0u; i < joint_state_values_.size(); ++i)
{
state_itfs_.emplace_back(hardware_interface::StateInterface(
state_itfs_.emplace_back(std::make_shared<hardware_interface::StateInterface>(
joint_names_[i], state_interface_types_[0], &joint_state_values_[i]));
state_ifs.emplace_back(state_itfs_.back());
}
Expand All @@ -226,7 +226,7 @@ class AdmittanceControllerTest : public ::testing::Test

for (auto i = 0u; i < fts_state_names_.size(); ++i)
{
state_itfs_.emplace_back(hardware_interface::StateInterface(
state_itfs_.emplace_back(std::make_shared<hardware_interface::StateInterface>(
ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i]));
state_ifs.emplace_back(state_itfs_.back());
}
Expand Down Expand Up @@ -397,8 +397,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<std::shared_ptr<hardware_interface::StateInterface>> state_itfs_;
std::vector<std::shared_ptr<hardware_interface::CommandInterface>> command_itfs_;

// Test related parameters
std::unique_ptr<TestableAdmittanceController> controller_;
Expand Down
50 changes: 28 additions & 22 deletions diff_drive_controller/test/test_diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,18 +163,24 @@ class TestDiffDriveController : public ::testing::Test
std::vector<double> position_values_ = {0.1, 0.2};
std::vector<double> velocity_values_ = {0.01, 0.02};

hardware_interface::StateInterface left_wheel_pos_state_{
left_wheel_names[0], HW_IF_POSITION, &position_values_[0]};
hardware_interface::StateInterface right_wheel_pos_state_{
right_wheel_names[0], HW_IF_POSITION, &position_values_[1]};
hardware_interface::StateInterface left_wheel_vel_state_{
left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]};
hardware_interface::StateInterface right_wheel_vel_state_{
right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]};
hardware_interface::CommandInterface left_wheel_vel_cmd_{
left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]};
hardware_interface::CommandInterface right_wheel_vel_cmd_{
right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]};
std::shared_ptr<hardware_interface::StateInterface> left_wheel_pos_state_ =
std::make_shared<hardware_interface::StateInterface>(
left_wheel_names[0], HW_IF_POSITION, &position_values_[0]);
std::shared_ptr<hardware_interface::StateInterface> right_wheel_pos_state_ =
std::make_shared<hardware_interface::StateInterface>(
right_wheel_names[0], HW_IF_POSITION, &position_values_[1]);
std::shared_ptr<hardware_interface::StateInterface> left_wheel_vel_state_ =
std::make_shared<hardware_interface::StateInterface>(
left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]);
std::shared_ptr<hardware_interface::StateInterface> right_wheel_vel_state_ =
std::make_shared<hardware_interface::StateInterface>(
right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]);
std::shared_ptr<hardware_interface::CommandInterface> left_wheel_vel_cmd_ =
std::make_shared<hardware_interface::CommandInterface>(
left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]);
std::shared_ptr<hardware_interface::CommandInterface> right_wheel_vel_cmd_ =
std::make_shared<hardware_interface::CommandInterface>(
right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]);

rclcpp::Node::SharedPtr pub_node;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_publisher;
Expand Down Expand Up @@ -368,8 +374,8 @@ TEST_F(TestDiffDriveController, cleanup)
ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());

// should be stopped
EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value());
EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value());
EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value());
EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value());

executor.cancel();
}
Expand All @@ -393,8 +399,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
assignResourcesPosFeedback();

ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value());
EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value());
EXPECT_EQ(0.01, left_wheel_vel_cmd_->get_value());
EXPECT_EQ(0.02, right_wheel_vel_cmd_->get_value());

state = controller_->get_node()->activate();
ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());
Expand All @@ -409,8 +415,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
ASSERT_EQ(
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);
EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value());
EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value());
EXPECT_EQ(1.0, left_wheel_vel_cmd_->get_value());
EXPECT_EQ(1.0, right_wheel_vel_cmd_->get_value());

// deactivated
// wait so controller process the second point when deactivated
Expand All @@ -421,14 +427,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()";
EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()";
EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()) << "Wheels are halted on deactivate()";
EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()) << "Wheels are halted on deactivate()";

// cleanup
state = controller_->get_node()->cleanup();
ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value());
EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value());
EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value());
EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value());

state = controller_->get_node()->configure();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
Expand Down
48 changes: 24 additions & 24 deletions effort_controllers/test/test_joint_group_effort_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,9 +119,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
controller_interface::return_type::OK);

// check joint commands are still the default ones
ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
ASSERT_EQ(joint_1_cmd_->get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_->get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_->get_value(), 3.1);

// send command
auto command_ptr = std::make_shared<forward_command_controller::CmdType>();
Expand All @@ -134,9 +134,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
controller_interface::return_type::OK);

// check joint commands have been modified
ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);
ASSERT_EQ(joint_2_cmd_.get_value(), 20.0);
ASSERT_EQ(joint_3_cmd_.get_value(), 30.0);
ASSERT_EQ(joint_1_cmd_->get_value(), 10.0);
ASSERT_EQ(joint_2_cmd_->get_value(), 20.0);
ASSERT_EQ(joint_3_cmd_->get_value(), 30.0);
}

TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
Expand All @@ -156,9 +156,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
controller_interface::return_type::ERROR);

// check joint commands are still the default ones
ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
ASSERT_EQ(joint_1_cmd_->get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_->get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_->get_value(), 3.1);
}

TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
Expand All @@ -173,9 +173,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
controller_interface::return_type::OK);

// check joint commands are still the default ones
ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
ASSERT_EQ(joint_1_cmd_->get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_->get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_->get_value(), 3.1);
}

TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
Expand All @@ -184,9 +184,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
controller_->get_node()->set_parameter({"joints", joint_names_});

// default values
ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
ASSERT_EQ(joint_1_cmd_->get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_->get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_->get_value(), 3.1);

auto node_state = controller_->get_node()->configure();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
Expand Down Expand Up @@ -214,9 +214,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
controller_interface::return_type::OK);

// check command in handle was set
ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);
ASSERT_EQ(joint_2_cmd_.get_value(), 20.0);
ASSERT_EQ(joint_3_cmd_.get_value(), 30.0);
ASSERT_EQ(joint_1_cmd_->get_value(), 10.0);
ASSERT_EQ(joint_2_cmd_->get_value(), 20.0);
ASSERT_EQ(joint_3_cmd_->get_value(), 30.0);
}

TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest)
Expand All @@ -228,15 +228,15 @@ TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

// check joint commands are still the default ones
ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
ASSERT_EQ(joint_1_cmd_->get_value(), 1.1);
ASSERT_EQ(joint_2_cmd_->get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_->get_value(), 3.1);

// stop the controller
ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

// check joint commands are now zero
ASSERT_EQ(joint_1_cmd_.get_value(), 0.0);
ASSERT_EQ(joint_2_cmd_.get_value(), 0.0);
ASSERT_EQ(joint_3_cmd_.get_value(), 0.0);
ASSERT_EQ(joint_1_cmd_->get_value(), 0.0);
ASSERT_EQ(joint_2_cmd_->get_value(), 0.0);
ASSERT_EQ(joint_3_cmd_->get_value(), 0.0);
}
9 changes: 6 additions & 3 deletions effort_controllers/test/test_joint_group_effort_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,12 @@ class JointGroupEffortControllerTest : public ::testing::Test
const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};
std::vector<double> joint_commands_ = {1.1, 2.1, 3.1};

CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]};
CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]};
CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]};
std::shared_ptr<CommandInterface> joint_1_cmd_ =
std::make_shared<CommandInterface>(joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]);
std::shared_ptr<CommandInterface> joint_2_cmd_ =
std::make_shared<CommandInterface>(joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]);
std::shared_ptr<CommandInterface> joint_3_cmd_ =
std::make_shared<CommandInterface>(joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]);
};

#endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_
24 changes: 18 additions & 6 deletions force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,12 +57,24 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test
const std::string frame_id_ = "fts_sensor_frame";
std::array<double, 6> sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6};

hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]};
hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]};
hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]};
hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]};
hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]};
hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]};
std::shared_ptr<hardware_interface::StateInterface> fts_force_x_ =
std::make_shared<hardware_interface::StateInterface>(
sensor_name_, "force.x", &sensor_values_[0]);
std::shared_ptr<hardware_interface::StateInterface> fts_force_y_ =
std::make_shared<hardware_interface::StateInterface>(
sensor_name_, "force.y", &sensor_values_[1]);
std::shared_ptr<hardware_interface::StateInterface> fts_force_z_ =
std::make_shared<hardware_interface::StateInterface>(
sensor_name_, "force.z", &sensor_values_[2]);
std::shared_ptr<hardware_interface::StateInterface> fts_torque_x_ =
std::make_shared<hardware_interface::StateInterface>(
sensor_name_, "torque.x", &sensor_values_[3]);
std::shared_ptr<hardware_interface::StateInterface> fts_torque_y_ =
std::make_shared<hardware_interface::StateInterface>(
sensor_name_, "torque.y", &sensor_values_[4]);
std::shared_ptr<hardware_interface::StateInterface> fts_torque_z_ =
std::make_shared<hardware_interface::StateInterface>(
sensor_name_, "torque.z", &sensor_values_[5]);

std::unique_ptr<FriendForceTorqueSensorBroadcaster> fts_broadcaster_;

Expand Down
Loading