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.

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

Final variant support #25

Draft
wants to merge 7 commits into
base: master
Choose a base branch
from
Draft
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 @@ -63,11 +63,13 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
else
{
const double traction_right_wheel_value =
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value<double>();
const double traction_left_wheel_value =
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value<double>();
const double steering_right_position =
state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value<double>();
const double steering_left_position =
state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value<double>();
if (
std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
Expand Down
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
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
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
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::InterfaceDescription> export_reference_interface_descriptions()
override;

controller_interface::return_type update_reference_from_subscribers(
const rclcpp::Time & time, const rclcpp::Duration & period) override;
Expand Down Expand Up @@ -121,8 +124,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
// internal reference values
const std::vector<std::string> allowed_reference_interfaces_types_ = {
hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY};
std::vector<std::reference_wrapper<double>> position_reference_;
std::vector<std::reference_wrapper<double>> velocity_reference_;
std::vector<std::string> position_reference_names_;
std::vector<std::string> velocity_reference_names_;

// Admittance rule and dependent variables;
std::unique_ptr<admittance_controller::AdmittanceRule> admittance_;
Expand Down
Loading
Loading