Skip to content

Commit b60f45d

Browse files
saikishorsaikishor
committed
Add logic to have a functional AUTO mode
1 parent 853a28b commit b60f45d

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
}
@@ -1606,6 +1598,14 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
16061598
activate_request_.clear();
16071599
return ret;
16081600
}
1601+
1602+
RCLCPP_INFO_EXPRESSION(
1603+
get_logger(), !activate_request_.empty(),
1604+
fmt::format("Activating controllers: [ {} ]", fmt::join(activate_request_, " ")).c_str());
1605+
RCLCPP_INFO_EXPRESSION(
1606+
get_logger(), !deactivate_request_.empty(),
1607+
fmt::format("Deactivating controllers: [ {} ]", fmt::join(deactivate_request_, " ")).c_str());
1608+
16091609
// If it is a best effort switch, we can remove the controllers log that could not be activated
16101610
message.clear();
16111611

@@ -1647,7 +1647,6 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
16471647
message = fmt::format(
16481648
FMT_COMPILE("Controller with name '{}' is already active."), controller_it->info.name);
16491649
RCLCPP_WARN(get_logger(), "%s", message.c_str());
1650-
RCLCPP_WARN(get_logger(), "%s", message.c_str());
16511650
status = controller_interface::return_type::ERROR;
16521651
}
16531652
}
@@ -1680,7 +1679,9 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
16801679
"Check the state of the controllers and their required interfaces using "
16811680
"`ros2 control list_controllers -v` CLI to get more information.",
16821681
(*ctrl_it).c_str());
1683-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
1682+
if (
1683+
strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT ||
1684+
strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO)
16841685
{
16851686
// TODO(destogl): automatic manipulation of the chain:
16861687
// || strictness ==
@@ -1691,9 +1692,11 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
16911692
message.clear();
16921693
--ctrl_it;
16931694
}
1694-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
1695+
else
16951696
{
1696-
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
1697+
RCLCPP_ERROR(
1698+
get_logger(), "Aborting, no controller is switched! (::%s switch)",
1699+
strictness_string.c_str());
16971700
// reset all lists
16981701
clear_requests();
16991702
return controller_interface::return_type::ERROR;
@@ -1732,17 +1735,21 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
17321735
"Check the state of the controllers and their required interfaces using "
17331736
"`ros2 control list_controllers -v` CLI to get more information.",
17341737
(*ctrl_it).c_str());
1735-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
1738+
if (
1739+
strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT ||
1740+
strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO)
17361741
{
17371742
// remove controller that can not be activated from the activation request and step-back
17381743
// iterator to correctly step to the next element in the list in the loop
17391744
deactivate_request_.erase(ctrl_it);
17401745
message.clear();
17411746
--ctrl_it;
17421747
}
1743-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
1748+
else
17441749
{
1745-
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
1750+
RCLCPP_ERROR(
1751+
get_logger(), "Aborting, no controller is switched! (::%s switch)",
1752+
strictness_string.c_str());
17461753
// reset all lists
17471754
clear_requests();
17481755
return controller_interface::return_type::ERROR;
@@ -1798,10 +1805,10 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
17981805

17991806
auto handle_conflict = [&](const std::string & msg)
18001807
{
1801-
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
1808+
if (strictness != controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
18021809
{
18031810
message = msg;
1804-
RCLCPP_ERROR(get_logger(), "%s", msg.c_str());
1811+
RCLCPP_ERROR(get_logger(), "%s (::%s switch)", message.c_str(), strictness_string.c_str());
18051812
deactivate_request_.clear();
18061813
deactivate_command_interface_request_.clear();
18071814
activate_request_.clear();
@@ -2113,6 +2120,8 @@ void ControllerManager::deactivate_controllers(
21132120
{
21142121
resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name);
21152122
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
2123+
// TODO(saikishor): Review this part later
2124+
controller->set_chained_mode(false);
21162125
}
21172126
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
21182127
{

‎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)