Skip to content

Commit b19cf3c

Browse files
saikishorsaikishor
committed
Add logic to have a functional AUTO mode
1 parent f6d9c7a commit b19cf3c

File tree

2 files changed

+83
-65
lines changed

2 files changed

+83
-65
lines changed

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 74 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -1467,22 +1467,14 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
14671467
get_logger(),
14681468
"Controller Manager: to switch controllers you need to specify a "
14691469
"strictness level of controller_manager_msgs::SwitchController::STRICT "
1470-
"(%d) or ::BEST_EFFORT (%d). When unspecified, the default is %s",
1470+
"(%d) or ::BEST_EFFORT (%d) or ::AUTO (%d). When unspecified, the default is %s",
14711471
controller_manager_msgs::srv::SwitchController::Request::STRICT,
14721472
controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT,
1473-
default_strictness.c_str());
1473+
controller_manager_msgs::srv::SwitchController::Request::AUTO, default_strictness.c_str());
14741474
strictness = params_->defaults.switch_controller.strictness == "strict"
14751475
? controller_manager_msgs::srv::SwitchController::Request::STRICT
14761476
: controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
14771477
}
1478-
else if (strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO)
1479-
{
1480-
RCLCPP_WARN(
1481-
get_logger(),
1482-
"Controller Manager: AUTO is not currently implemented. "
1483-
"Defaulting to BEST_EFFORT");
1484-
strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
1485-
}
14861478
else if (strictness == controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO)
14871479
{
14881480
RCLCPP_DEBUG(
@@ -1491,63 +1483,61 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
14911483
"Defaulting to BEST_EFFORT");
14921484
strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
14931485
}
1494-
1495-
if (!activate_controllers.empty())
1486+
else if (strictness > controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO)
14961487
{
1497-
for (const auto & controller : activate_controllers)
1498-
{
1499-
const auto x = controller_chain_dependency_graph_.get_dependencies_to_activate(controller);
1500-
RCLCPP_INFO(
1501-
get_logger(),
1502-
fmt::format(
1503-
"Controller {} has '{}' dependencies to activate", controller, fmt::join(x, ", "))
1504-
.c_str());
1505-
}
1506-
}
1507-
1508-
if (!deactivate_controllers.empty())
1509-
{
1510-
for (const auto & controller : deactivate_controllers)
1511-
{
1512-
const auto x = controller_chain_dependency_graph_.get_dependencies_to_deactivate(controller);
1513-
RCLCPP_INFO(
1514-
get_logger(),
1515-
fmt::format(
1516-
"Controller {} has '{}' dependencies to deactivate", controller, fmt::join(x, ", "))
1517-
.c_str());
1518-
}
1488+
message = fmt::format(
1489+
FMT_COMPILE(
1490+
"Unknown strictness level '%d'. Please use controller_manager_msgs::SwitchController::"
1491+
"STRICT (%d) or ::BEST_EFFORT (%d) or ::AUTO (%d) or ::FORCE_AUTO (%d)."),
1492+
strictness, controller_manager_msgs::srv::SwitchController::Request::STRICT,
1493+
controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT,
1494+
controller_manager_msgs::srv::SwitchController::Request::AUTO,
1495+
controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO);
1496+
RCLCPP_ERROR(get_logger(), "%s", message.c_str());
1497+
return controller_interface::return_type::ERROR;
15191498
}
15201499

1521-
std::string activate_list, deactivate_list;
1522-
activate_list.reserve(500);
1523-
deactivate_list.reserve(500);
1524-
for (const auto & controller : activate_controllers)
1525-
{
1526-
activate_list.append(controller);
1527-
activate_list.append(" ");
1528-
}
1529-
for (const auto & controller : deactivate_controllers)
1530-
{
1531-
deactivate_list.append(controller);
1532-
deactivate_list.append(" ");
1533-
}
1534-
RCLCPP_INFO_EXPRESSION(
1535-
get_logger(), !activate_list.empty(), "Activating controllers: [ %s]", activate_list.c_str());
1536-
RCLCPP_INFO_EXPRESSION(
1537-
get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]",
1538-
deactivate_list.c_str());
1500+
const std::string strictness_string =
1501+
strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT
1502+
? "STRICT"
1503+
: (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT
1504+
? "BEST_EFFORT"
1505+
: (strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO
1506+
? "AUTO"
1507+
: "FORCE_AUTO"));
15391508

15401509
const auto list_controllers =
1541-
[this, strictness](
1510+
[this, strictness, &strictness_string](
15421511
const std::vector<std::string> & controller_list, std::vector<std::string> & request_list,
15431512
const std::string & action, std::string & msg) -> controller_interface::return_type
15441513
{
15451514
// lock controllers
15461515
std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
15471516
auto result = controller_interface::return_type::OK;
1517+
std::vector<std::string> new_controller_list = controller_list;
1518+
1519+
if (strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO)
1520+
{
1521+
const bool is_activate = (action == "activate");
1522+
std::vector<std::string> dependencies = {};
1523+
for (const auto & controller : controller_list)
1524+
{
1525+
const auto ctrl_dependencies =
1526+
is_activate
1527+
? controller_chain_dependency_graph_.get_dependencies_to_activate(controller)
1528+
: controller_chain_dependency_graph_.get_dependencies_to_deactivate(controller);
1529+
RCLCPP_INFO(
1530+
get_logger(), fmt::format(
1531+
"Controller {} has '{}' dependencies to {}", controller,
1532+
fmt::join(ctrl_dependencies, ", "), action)
1533+
.c_str());
1534+
ros2_control::add_unique_items(dependencies, ctrl_dependencies);
1535+
}
1536+
ros2_control::add_unique_items(new_controller_list, dependencies);
1537+
}
15481538

15491539
// list all controllers to (de)activate
1550-
for (const auto & controller : controller_list)
1540+
for (const auto & controller : new_controller_list)
15511541
{
15521542
const auto & updated_controllers = rt_controllers_wrapper_.get_updated_list(guard);
15531543

@@ -1567,10 +1557,12 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
15671557
// not a critical error
15681558
result = request_list.empty() ? controller_interface::return_type::ERROR
15691559
: controller_interface::return_type::OK;
1570-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
1560+
if (strictness != controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
15711561
{
15721562
msg = error_msg;
1573-
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! ('STRICT' switch)");
1563+
RCLCPP_ERROR(
1564+
get_logger(), "Aborting, no controller is switched! ('%s' switch)",
1565+
strictness_string.c_str());
15741566
return controller_interface::return_type::ERROR;
15751567
}
15761568
}
@@ -1605,6 +1597,14 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
16051597
activate_request_.clear();
16061598
return ret;
16071599
}
1600+
1601+
RCLCPP_INFO_EXPRESSION(
1602+
get_logger(), !activate_request_.empty(),
1603+
fmt::format("Activating controllers: [ {} ]", fmt::join(activate_request_, " ")).c_str());
1604+
RCLCPP_INFO_EXPRESSION(
1605+
get_logger(), !deactivate_request_.empty(),
1606+
fmt::format("Deactivating controllers: [ {} ]", fmt::join(deactivate_request_, " ")).c_str());
1607+
16081608
// If it is a best effort switch, we can remove the controllers log that could not be activated
16091609
message.clear();
16101610

@@ -1646,7 +1646,6 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
16461646
message = fmt::format(
16471647
FMT_COMPILE("Controller with name '{}' is already active."), controller_it->info.name);
16481648
RCLCPP_WARN(get_logger(), "%s", message.c_str());
1649-
RCLCPP_WARN(get_logger(), "%s", message.c_str());
16501649
status = controller_interface::return_type::ERROR;
16511650
}
16521651
}
@@ -1679,7 +1678,9 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
16791678
"Check the state of the controllers and their required interfaces using "
16801679
"`ros2 control list_controllers -v` CLI to get more information.",
16811680
(*ctrl_it).c_str());
1682-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
1681+
if (
1682+
strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT ||
1683+
strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO)
16831684
{
16841685
// TODO(destogl): automatic manipulation of the chain:
16851686
// || strictness ==
@@ -1690,9 +1691,11 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
16901691
message.clear();
16911692
--ctrl_it;
16921693
}
1693-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
1694+
else
16941695
{
1695-
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
1696+
RCLCPP_ERROR(
1697+
get_logger(), "Aborting, no controller is switched! (::%s switch)",
1698+
strictness_string.c_str());
16961699
// reset all lists
16971700
clear_requests();
16981701
return controller_interface::return_type::ERROR;
@@ -1731,17 +1734,21 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
17311734
"Check the state of the controllers and their required interfaces using "
17321735
"`ros2 control list_controllers -v` CLI to get more information.",
17331736
(*ctrl_it).c_str());
1734-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
1737+
if (
1738+
strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT ||
1739+
strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO)
17351740
{
17361741
// remove controller that can not be activated from the activation request and step-back
17371742
// iterator to correctly step to the next element in the list in the loop
17381743
deactivate_request_.erase(ctrl_it);
17391744
message.clear();
17401745
--ctrl_it;
17411746
}
1742-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
1747+
else
17431748
{
1744-
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
1749+
RCLCPP_ERROR(
1750+
get_logger(), "Aborting, no controller is switched! (::%s switch)",
1751+
strictness_string.c_str());
17451752
// reset all lists
17461753
clear_requests();
17471754
return controller_interface::return_type::ERROR;
@@ -1797,10 +1804,10 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
17971804

17981805
auto handle_conflict = [&](const std::string & msg)
17991806
{
1800-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
1807+
if (strictness != controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
18011808
{
18021809
message = msg;
1803-
RCLCPP_ERROR(get_logger(), "%s", msg.c_str());
1810+
RCLCPP_ERROR(get_logger(), "%s (::%s switch)", message.c_str(), strictness_string.c_str());
18041811
deactivate_request_.clear();
18051812
deactivate_command_interface_request_.clear();
18061813
activate_request_.clear();
@@ -2112,6 +2119,8 @@ void ControllerManager::deactivate_controllers(
21122119
{
21132120
resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name);
21142121
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
2122+
// TODO(saikishor): Review this part later
2123+
controller->set_chained_mode(false);
21152124
}
21162125
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
21172126
{

‎hardware_interface/include/hardware_interface/helpers.hpp‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,15 @@ void add_item(std::vector<T> & vector, const T & item)
8888
}
8989
}
9090

91+
template <typename T>
92+
void add_unique_items(std::vector<T> & vector, const std::vector<T> & items)
93+
{
94+
for (const auto & item : items)
95+
{
96+
add_item(vector, item);
97+
}
98+
}
99+
91100
/**
92101
* @brief Remove the item from the container if it is in it.
93102
* @param container The container to remove the item from.

0 commit comments

Comments
 (0)