Skip to content

State inferface changes for kilted #1

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

Open
ipa-dmo wants to merge 1 commit into
base: kilted
Choose a base branch
from
+24 −22
Open

State inferface changes for kilted #1

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
46 changes: 24 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 @@ -14,6 +14,8 @@

#include <gmock/gmock.h>

#include <functional>
Copy link
Member

@ottojo ottojo Jan 22, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
#include <functional>

unused?

#include <hardware_interface/handle.hpp>
#include <memory>
#include <string>
#include <thread>
Expand Down Expand Up @@ -196,18 +198,18 @@ 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]};
hardware_interface::StateInterface::ConstSharedPtr left_wheel_pos_state_ = std::make_shared<const hardware_interface::StateInterface>(
left_wheel_names[0], HW_IF_POSITION, &position_values_[0]);
hardware_interface::StateInterface::ConstSharedPtr right_wheel_pos_state_ = std::make_shared<const hardware_interface::StateInterface>(
right_wheel_names[0], HW_IF_POSITION, &position_values_[1]);
hardware_interface::StateInterface::ConstSharedPtr left_wheel_vel_state_ = std::make_shared<const hardware_interface::StateInterface>(
left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]);
hardware_interface::StateInterface::ConstSharedPtr right_wheel_vel_state_ = std::make_shared<const hardware_interface::StateInterface>(
right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]);
hardware_interface::CommandInterface::SharedPtr left_wheel_vel_cmd_ = std::make_shared<hardware_interface::CommandInterface>(
left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]);
hardware_interface::CommandInterface::SharedPtr 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 @@ -513,8 +515,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_optional().value_or(1.0));
EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value_or(1.0));
EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value_or(1.0));
EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value_or(1.0));

executor.cancel();
}
Expand All @@ -534,8 +536,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_optional().value_or(0.0));
EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_optional().value_or(0.0));
EXPECT_EQ(0.01, left_wheel_vel_cmd_->get_optional().value_or(0.0));
EXPECT_EQ(0.02, right_wheel_vel_cmd_->get_optional().value_or(0.0));

state = controller_->get_node()->activate();
ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());
Expand All @@ -550,8 +552,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_optional().value_or(0.0));
EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_optional().value_or(0.0));
EXPECT_EQ(1.0, left_wheel_vel_cmd_->get_optional().value_or(0.0));
EXPECT_EQ(1.0, right_wheel_vel_cmd_->get_optional().value_or(0.0));

// deactivated
// wait so controller process the second point when deactivated
Expand All @@ -562,14 +564,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_optional().value_or(1.0)) << "Wheels are halted on deactivate()";
EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value_or(1.0)) << "Wheels are halted on deactivate()";
EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value_or(1.0)) << "Wheels are halted on deactivate()";
EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value_or(1.0)) << "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_optional().value_or(1.0));
EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value_or(1.0));
EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value_or(1.0));
EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value_or(1.0));

state = controller_->get_node()->configure();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
Expand Down