Skip to content

Commit

Permalink
change get/set_state to get/set_lifcycle_state in controllers as well to
Browse files Browse the repository at this point in the history
have unified interface for setting getting lifecylce state
  • Loading branch information
mamueluth committed Aug 15, 2024
1 parent f2a5053 commit 5a997e6
Show file tree
Hide file tree
Showing 15 changed files with 507 additions and 311 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,8 @@ class AsyncControllerThread
TimePoint next_iteration_time =
TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds()));

if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
if (
controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
auto const current_time = controller_->get_node()->now();
auto const measured_period = current_time - previous_time;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> get_node() const;

CONTROLLER_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & get_state() const;
const rclcpp_lifecycle::State & get_lifecycle_state() const;

CONTROLLER_INTERFACE_PUBLIC
unsigned int get_update_rate() const;
Expand Down
5 changes: 3 additions & 2 deletions controller_interface/src/chainable_controller_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
{
bool result = false;

if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
result = on_set_chained_mode(chained_mode);

Expand All @@ -112,7 +112,8 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
get_node()->get_logger(),
"Can not change controller's chained mode because it is no in '%s' state. "
"Current state is '%s'.",
hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str());
hardware_interface::lifecycle_state_names::UNCONFIGURED,
get_lifecycle_state().label().c_str());
}

return result;
Expand Down
4 changes: 2 additions & 2 deletions controller_interface/src/controller_interface_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
// Then we don't need to do state-machine related checks.
//
// Other solution is to add check into the LifecycleNode if a transition is valid to trigger
if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
{
update_rate_ = static_cast<unsigned int>(get_node()->get_parameter("update_rate").as_int());
is_async_ = get_node()->get_parameter("is_async").as_bool();
Expand All @@ -106,7 +106,7 @@ void ControllerInterfaceBase::release_interfaces()
state_interfaces_.clear();
}

const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const
const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const
{
return node_->get_current_state();
}
Expand Down
13 changes: 7 additions & 6 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,8 @@ static const rmw_qos_profile_t qos_services = {

inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller)
{
return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
return controller.get_lifecycle_state().id() ==
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
}

inline bool is_controller_inactive(
Expand All @@ -68,7 +69,7 @@ inline bool is_controller_inactive(

inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller)
{
return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
}

inline bool is_controller_active(
Expand Down Expand Up @@ -655,7 +656,7 @@ controller_interface::return_type ControllerManager::configure_controller(
}
auto controller = found_it->c;

auto state = controller->get_state();
auto state = controller->get_lifecycle_state();
if (
state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand All @@ -666,7 +667,7 @@ controller_interface::return_type ControllerManager::configure_controller(
return controller_interface::return_type::ERROR;
}

auto new_state = controller->get_state();
auto new_state = controller->get_lifecycle_state();
if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
RCLCPP_DEBUG(
Expand Down Expand Up @@ -1740,7 +1741,7 @@ void ControllerManager::list_controllers_srv_cb(
controller_state.name = controllers[i].info.name;
controller_state.type = controllers[i].info.type;
controller_state.claimed_interfaces = controllers[i].info.claimed_interfaces;
controller_state.state = controllers[i].c->get_state().label();
controller_state.state = controllers[i].c->get_lifecycle_state().label();
controller_state.is_chainable = controllers[i].c->is_chainable();
controller_state.is_chained = controllers[i].c->is_in_chained_mode();

Expand Down Expand Up @@ -2724,7 +2725,7 @@ void ControllerManager::controller_activity_diagnostic_callback(
{
all_active = false;
}
stat.add(controllers[i].info.name, controllers[i].c->get_state().label());
stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label());
}

if (all_active)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,8 @@ controller_interface::InterfaceConfiguration
TestChainableController::command_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return cmd_iface_cfg_;
}
Expand All @@ -49,8 +49,8 @@ controller_interface::InterfaceConfiguration
TestChainableController::state_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
auto state_iface_cfg = state_iface_cfg_;
if (imu_sensor_)
Expand Down
8 changes: 4 additions & 4 deletions controller_manager/test/test_controller/test_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@ TestController::TestController()
controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return cmd_iface_cfg_;
}
Expand All @@ -46,8 +46,8 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c
controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return state_iface_cfg_;
}
Expand Down
47 changes: 33 additions & 14 deletions controller_manager/test/test_controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
<< "Update should not reach an unconfigured controller";

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_state().id());

// configure controller
{
Expand All @@ -141,7 +142,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());

// Start controller, will take effect at the end of the update function
std::vector<std::string> start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME};
Expand Down Expand Up @@ -182,7 +185,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
}
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());

EXPECT_EQ(
controller_interface::return_type::OK,
Expand Down Expand Up @@ -210,7 +214,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
}

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());
auto unload_future = std::async(
std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
test_controller::TEST_CONTROLLER_NAME);
Expand All @@ -221,7 +227,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_state().id());
EXPECT_EQ(1, test_controller.use_count());
}

Expand All @@ -242,7 +249,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
<< "Update should not reach an unconfigured controller";

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_state().id());

test_controller->get_node()->set_parameter({"update_rate", 4});
// configure controller
Expand All @@ -255,7 +263,9 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());

// Start controller, will take effect at the end of the update function
std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
Expand All @@ -276,7 +286,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
}

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());

// As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle
for (size_t i = 0; i < 25; i++)
Expand Down Expand Up @@ -323,7 +334,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
<< "Update should not reach an unconfigured controller";

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_state().id());

rclcpp::Parameter update_rate_parameter("update_rate", static_cast<int>(ctrl_update_rate));
test_controller->get_node()->set_parameter(update_rate_parameter);
Expand All @@ -337,7 +349,9 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
<< "Controller is not started";
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());

// Start controller, will take effect at the end of the update function
std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
Expand All @@ -360,7 +374,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
}

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());

const auto pre_internal_counter = test_controller->internal_counter;
rclcpp::Rate loop_rate(cm_->get_update_rate());
Expand Down Expand Up @@ -430,7 +445,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
EXPECT_EQ(2, test_controller.use_count());

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_state().id());

test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
// configure controller
Expand All @@ -443,7 +459,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());

// Start controller, will take effect at the end of the update function
time_ = test_controller->get_node()->now(); // set to something nonzero
Expand All @@ -467,7 +485,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
}

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());

EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
const auto cm_update_rate = cm_->get_update_rate();
Expand Down
Loading

0 comments on commit 5a997e6

Please sign in to comment.