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 aio #11

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

Distributed control aio #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
4d09c75
minimal changes jtc inherits from chainable interface, compiling not
mamueluth May 10, 2023
2577680
first poc for chainable jtc
mamueluth May 13, 2023
ee73ab3
roughly working chainedable jtc.
mamueluth May 16, 2023
f20397b
adapt to changes made to loandedInterface
mamueluth Nov 21, 2022
0868154
adjust tests to reflect changes in handles
mamueluth Dec 5, 2022
dc1d3b5
joint_state_broadcaster if no local topic us "/" as prefix
mamueluth Jan 11, 2023
372789f
adapt admittance_controller tests
mamueluth Jan 11, 2023
470ea50
first working publishing of trajectory generated by moveit
mamueluth Jun 28, 2023
553dcdd
add reverting of movement
mamueluth Jun 30, 2023
8bac327
add duplicating
mamueluth Jun 30, 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