Skip to content

Commit

Permalink
Merge branch 'master' into hardware_interfaces_getter_setter
Browse files Browse the repository at this point in the history
  • Loading branch information
mamueluth authored Aug 27, 2024
2 parents 70c674c + 8f7374f commit 7f24010
Show file tree
Hide file tree
Showing 56 changed files with 845 additions and 513 deletions.
3 changes: 3 additions & 0 deletions controller_interface/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,9 @@
Changelog for package controller_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

4.16.1 (2024-08-24)
-------------------

4.16.0 (2024-08-22)
-------------------
* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 <https://github.com/ros-controls/ros2_control/issues/1701>`_)
Expand Down
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
2 changes: 1 addition & 1 deletion controller_interface/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>controller_interface</name>
<version>4.16.0</version>
<version>4.16.1</version>
<description>Description of controller_interface</description>
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
<maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>
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
5 changes: 5 additions & 0 deletions controller_manager/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,11 @@
Changelog for package controller_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

4.16.1 (2024-08-24)
-------------------
* propage a portion of global args to the controller nodes (`#1712 <https://github.com/ros-controls/ros2_control/issues/1712>`_)
* Contributors: Sai Kishor Kothakota

4.16.0 (2024-08-22)
-------------------
* inform user what reason is for not setting rt policy, inform is policy (`#1705 <https://github.com/ros-controls/ros2_control/issues/1705>`_)
Expand Down
27 changes: 18 additions & 9 deletions controller_manager/controller_manager/spawner.py
Original file line number Diff line number Diff line change
Expand Up @@ -234,7 +234,7 @@ def main(args=None):

node.get_logger().info(
bcolors.OKGREEN
+ "Configured and activated all the parsed controllers list!"
+ f"Configured and activated all the parsed controllers list : {controller_names}!"
+ bcolors.ENDC
)

Expand All @@ -258,16 +258,25 @@ def main(args=None):
)
return 1

node.get_logger().info("Deactivated controller")

ret = unload_controller(node, controller_manager_name, controller_name)
if not ret.ok:
node.get_logger().error(
bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC
node.get_logger().info(
f"Successfully deactivated controllers : {controller_names}"
)
return 1

node.get_logger().info("Unloaded controller")
unload_status = True
for controller_name in controller_names:
ret = unload_controller(node, controller_manager_name, controller_name)
if not ret.ok:
unload_status = False
node.get_logger().error(
bcolors.FAIL
+ f"Failed to unload controller : {controller_name}"
+ bcolors.ENDC
)

if unload_status:
node.get_logger().info(f"Successfully unloaded controllers : {controller_names}")
else:
return 1
return 0
except KeyboardInterrupt:
pass
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -577,6 +577,7 @@ class ControllerManager : public rclcpp::Node
std::map<std::string, std::vector<std::string>> controller_chained_reference_interfaces_cache_;
std::map<std::string, std::vector<std::string>> controller_chained_state_interfaces_cache_;

rclcpp::NodeOptions cm_node_options_;
std::string robot_description_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
Expand Down
2 changes: 1 addition & 1 deletion controller_manager/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>controller_manager</name>
<version>4.16.0</version>
<version>4.16.1</version>
<description>Description of controller_manager</description>
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
<maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>
Expand Down
64 changes: 48 additions & 16 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include "controller_manager_msgs/msg/hardware_component_state.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rcl/arguments.h"
#include "rclcpp/version.h"
#include "rclcpp_lifecycle/state.hpp"

Expand Down Expand Up @@ -57,7 +58,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 +70,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 @@ -197,7 +199,8 @@ ControllerManager::ControllerManager(
kControllerInterfaceNamespace, kControllerInterfaceClassName)),
chainable_loader_(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
cm_node_options_(options)
{
init_controller_manager();
}
Expand All @@ -217,7 +220,8 @@ ControllerManager::ControllerManager(
kControllerInterfaceNamespace, kControllerInterfaceClassName)),
chainable_loader_(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
cm_node_options_(options)
{
init_controller_manager();
}
Expand All @@ -234,7 +238,8 @@ ControllerManager::ControllerManager(
kControllerInterfaceNamespace, kControllerInterfaceClassName)),
chainable_loader_(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
cm_node_options_(options)
{
init_controller_manager();
}
Expand Down Expand Up @@ -655,7 +660,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 +671,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 +1745,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 +2729,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 Expand Up @@ -2819,29 +2824,56 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(

rclcpp::NodeOptions controller_node_options = controller.c->define_custom_node_options();
std::vector<std::string> node_options_arguments = controller_node_options.arguments();
const std::string ros_args_arg = "--ros-args";

for (const std::string & arg : cm_node_options_.arguments())
{
if (arg.find("__ns") != std::string::npos || arg.find("__node") != std::string::npos)
{
if (
node_options_arguments.back() == RCL_REMAP_FLAG ||
node_options_arguments.back() == RCL_SHORT_REMAP_FLAG)
{
node_options_arguments.pop_back();
}
continue;
}

node_options_arguments.push_back(arg);
}

if (controller.info.parameters_file.has_value())
{
if (!check_for_element(node_options_arguments, ros_args_arg))
if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG))
{
node_options_arguments.push_back(ros_args_arg);
node_options_arguments.push_back(RCL_ROS_ARGS_FLAG);
}
node_options_arguments.push_back("--params-file");
node_options_arguments.push_back(RCL_PARAM_FILE_FLAG);
node_options_arguments.push_back(controller.info.parameters_file.value());
}

// ensure controller's `use_sim_time` parameter matches controller_manager's
const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time");
if (use_sim_time.as_bool())
{
if (!check_for_element(node_options_arguments, ros_args_arg))
if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG))
{
node_options_arguments.push_back(ros_args_arg);
node_options_arguments.push_back(RCL_ROS_ARGS_FLAG);
}
node_options_arguments.push_back("-p");
node_options_arguments.push_back(RCL_PARAM_FLAG);
node_options_arguments.push_back("use_sim_time:=true");
}

std::string arguments;
arguments.reserve(1000);
for (const auto & arg : node_options_arguments)
{
arguments.append(arg);
arguments.append(" ");
}
RCLCPP_INFO(
get_logger(), "Controller '%s' node arguments: %s", controller.info.name.c_str(),
arguments.c_str());

controller_node_options = controller_node_options.arguments(node_options_arguments);
controller_node_options.use_global_arguments(false);
return controller_node_options;
Expand Down
17 changes: 16 additions & 1 deletion controller_manager/src/ros2_control_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,22 @@ int main(int argc, char ** argv)
std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
std::string manager_node_name = "controller_manager";

auto cm = std::make_shared<controller_manager::ControllerManager>(executor, manager_node_name);
rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options();
std::vector<std::string> node_arguments = cm_node_options.arguments();
for (int i = 1; i < argc; ++i)
{
if (node_arguments.empty() && std::string(argv[i]) != "--ros-args")
{
// A simple way to reject non ros args
continue;
}
RCLCPP_INFO(rclcpp::get_logger("CM args"), "Adding argument: %s", argv[i]);
node_arguments.push_back(argv[i]);
}
cm_node_options.arguments(node_arguments);

auto cm = std::make_shared<controller_manager::ControllerManager>(
executor, manager_node_name, "", cm_node_options);

RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());

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
Loading

0 comments on commit 7f24010

Please sign in to comment.