Skip to content

Commit

Permalink
continue adapting controllers
Browse files Browse the repository at this point in the history
  • Loading branch information
@mamueluth
mamueluth committed Aug 29, 2024
1 parent f47a13c commit c7c2f52
Show file tree
Hide file tree
Showing 15 changed files with 508 additions and 421 deletions.
70 changes: 37 additions & 33 deletions ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);

auto cmd_if_conf = controller_->command_interface_configuration();
ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_size_);
EXPECT_EQ(
cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL],
rear_wheels_names_[0] + "/" + traction_interface_name_);
Expand All @@ -67,7 +67,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

auto state_if_conf = controller_->state_interface_configuration();
ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size());
ASSERT_EQ(state_if_conf.names.size(), joint_state_values_size_);
EXPECT_EQ(
state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL],
controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_);
Expand All @@ -83,7 +83,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
auto reference_interfaces = controller_->export_reference_interface_descriptions();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
{
Expand Down Expand Up @@ -140,9 +140,9 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value<double>()));
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value<double>()));

ASSERT_EQ(
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
Expand Down Expand Up @@ -173,23 +173,25 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)

// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value<double>(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value<double>(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value<double>(), 1.4179821977774734,
COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value<double>(), 1.4179821977774734,
COMMON_THRESHOLD);

EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
for (const auto & interface : controller_->reference_interfaces_)
EXPECT_EQ(controller_->ordered_reference_interfaces_.size(), joint_reference_interfaces_.size());
for (const auto & interface : controller_->ordered_reference_interfaces_)
{
EXPECT_TRUE(std::isnan(interface));
EXPECT_TRUE(interface->holds_value());
EXPECT_TRUE(interface->operator bool());
EXPECT_TRUE(std::isnan(interface->get_value<double>()));
}
}

Expand All @@ -204,32 +206,34 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_TRUE(controller_->is_in_chained_mode());

controller_->reference_interfaces_[0] = 0.1;
controller_->reference_interfaces_[1] = 0.2;
controller_->ordered_reference_interfaces_[0]->set_value(0.1);
controller_->ordered_reference_interfaces_[1]->set_value(0.2);

ASSERT_EQ(
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
EXPECT_NEAR(
controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value<double>(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value<double>(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
COMMON_THRESHOLD);
controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value<double>(),
1.4179821977774734, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
COMMON_THRESHOLD);
controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value<double>(),
1.4179821977774734, COMMON_THRESHOLD);

EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
for (const auto & interface : controller_->reference_interfaces_)
EXPECT_EQ(controller_->ordered_reference_interfaces_.size(), joint_reference_interfaces_.size());
for (const auto & interface : controller_->ordered_reference_interfaces_)
{
EXPECT_TRUE(std::isnan(interface));
EXPECT_TRUE(interface->holds_value());
EXPECT_TRUE(interface->operator bool());
EXPECT_TRUE(std::isnan(interface->get_value<double>()));
}
}

Expand Down Expand Up @@ -263,16 +267,16 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat

// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value<double>(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value<double>(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value<double>(), 1.4179821977774734,
COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value<double>(), 1.4179821977774734,
COMMON_THRESHOLD);

subscribe_and_get_messages(msg);
Expand Down
66 changes: 35 additions & 31 deletions ackermann_steering_controller/test/test_ackermann_steering_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class TestableAckermannSteeringController
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override
{
auto ref_itfs = on_export_reference_interfaces();
auto ref_itfs = export_reference_interface_descriptions();
return ackermann_steering_controller::AckermannSteeringController::on_activate(previous_state);
}

Expand Down Expand Up @@ -147,51 +147,55 @@ class AckermannSteeringControllerFixture : public ::testing::Test
}

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

command_itfs_.emplace_back(hardware_interface::CommandInterface(
rear_wheels_names_[0], traction_interface_name_,
&joint_command_values_[CMD_TRACTION_RIGHT_WHEEL]));
command_itfs_.emplace_back(
hardware_interface::CommandInterface(hardware_interface::InterfaceDescription(
rear_wheels_names_[0],
hardware_interface::InterfaceInfo(traction_interface_name_, "double", "1.1"))));
command_ifs.emplace_back(command_itfs_.back());

command_itfs_.emplace_back(hardware_interface::CommandInterface(
rear_wheels_names_[1], steering_interface_name_,
&joint_command_values_[CMD_TRACTION_LEFT_WHEEL]));
command_itfs_.emplace_back(
hardware_interface::CommandInterface(hardware_interface::InterfaceDescription(
rear_wheels_names_[1],
hardware_interface::InterfaceInfo(steering_interface_name_, "double", "3.3"))));
command_ifs.emplace_back(command_itfs_.back());

command_itfs_.emplace_back(hardware_interface::CommandInterface(
front_wheels_names_[0], steering_interface_name_,
&joint_command_values_[CMD_STEER_RIGHT_WHEEL]));
command_itfs_.emplace_back(
hardware_interface::CommandInterface(hardware_interface::InterfaceDescription(
front_wheels_names_[0],
hardware_interface::InterfaceInfo(steering_interface_name_, "double", "2.2"))));
command_ifs.emplace_back(command_itfs_.back());

command_itfs_.emplace_back(hardware_interface::CommandInterface(
front_wheels_names_[1], steering_interface_name_,
&joint_command_values_[CMD_STEER_LEFT_WHEEL]));
command_itfs_.emplace_back(
hardware_interface::CommandInterface(hardware_interface::InterfaceDescription(
front_wheels_names_[1],
hardware_interface::InterfaceInfo(steering_interface_name_, "double", "4.4"))));
command_ifs.emplace_back(command_itfs_.back());

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

state_itfs_.emplace_back(hardware_interface::StateInterface(
rear_wheels_names_[0], traction_interface_name_,
&joint_state_values_[STATE_TRACTION_RIGHT_WHEEL]));
state_itfs_.emplace_back(
hardware_interface::StateInterface(hardware_interface::InterfaceDescription(
rear_wheels_names_[0],
hardware_interface::InterfaceInfo(traction_interface_name_, "double", "0.5"))));
state_ifs.emplace_back(state_itfs_.back());

state_itfs_.emplace_back(hardware_interface::StateInterface(
rear_wheels_names_[1], traction_interface_name_,
&joint_state_values_[STATE_TRACTION_LEFT_WHEEL]));
state_itfs_.emplace_back(
hardware_interface::StateInterface(hardware_interface::InterfaceDescription(
rear_wheels_names_[1],
hardware_interface::InterfaceInfo(traction_interface_name_, "double", "0.5"))));
state_ifs.emplace_back(state_itfs_.back());

state_itfs_.emplace_back(hardware_interface::StateInterface(
front_wheels_names_[0], steering_interface_name_,
&joint_state_values_[STATE_STEER_RIGHT_WHEEL]));
state_itfs_.emplace_back(
hardware_interface::StateInterface(hardware_interface::InterfaceDescription(
front_wheels_names_[0],
hardware_interface::InterfaceInfo(steering_interface_name_, "double", "0.0"))));
state_ifs.emplace_back(state_itfs_.back());

state_itfs_.emplace_back(hardware_interface::StateInterface(
front_wheels_names_[1], steering_interface_name_,
&joint_state_values_[STATE_STEER_LEFT_WHEEL]));
state_itfs_.emplace_back(
hardware_interface::StateInterface(hardware_interface::InterfaceDescription(
front_wheels_names_[1],
hardware_interface::InterfaceInfo(steering_interface_name_, "double", "0.0"))));
state_ifs.emplace_back(state_itfs_.back());

controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
Expand Down Expand Up @@ -293,8 +297,8 @@ class AckermannSteeringControllerFixture : public ::testing::Test
double front_wheels_radius_ = 0.45;
double rear_wheels_radius_ = 0.45;

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};
const size_t joint_command_values_size_ = 4;
const size_t joint_state_values_size_ = 4;
std::array<std::string, 2> joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"};
std::string steering_interface_name_ = "position";
// defined in setup
Expand Down
6 changes: 3 additions & 3 deletions ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);

auto cmd_if_conf = controller_->command_interface_configuration();
ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_size_);
EXPECT_EQ(
cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL],
preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_);
Expand All @@ -69,7 +69,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

auto state_if_conf = controller_->state_interface_configuration();
ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size());
ASSERT_EQ(state_if_conf.names.size(), joint_state_values_size_);
EXPECT_EQ(
state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL],
controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_);
Expand All @@ -85,7 +85,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

// check ref itfs
auto reference_interfaces = controller_->export_reference_interfaces();
auto reference_interfaces = controller_->export_reference_interface_descriptions();
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
{
Expand Down
5 changes: 4 additions & 1 deletion admittance_controller/include/admittance_controller/admittance_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,10 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
const rclcpp::Time & time, const rclcpp::Duration & period) override;

protected:
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
std::vector<hardware_interface::InterfaceDescription> export_state_interface_descriptions()
override;
std::vector<hardware_interface::CommandInterface> export_reference_interface_descriptions()
override;

controller_interface::return_type update_reference_from_subscribers(
const rclcpp::Time & time, const rclcpp::Duration & period) override;
Expand Down
Loading

0 comments on commit c7c2f52

Please sign in to comment.