Skip to content

Commit

Permalink
Merge branch 'master' into add/sw_joint_limiter
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor authored Jul 19, 2024
2 parents ece0933 + 0792a35 commit 82107d2
Show file tree
Hide file tree
Showing 14 changed files with 292 additions and 186 deletions.
6 changes: 3 additions & 3 deletions controller_interface/include/controller_interface/controller_interface_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,12 +258,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
protected:
std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
unsigned int update_rate_ = 0;
bool is_async_ = false;
std::string urdf_ = "";

private:
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
unsigned int update_rate_ = 0;
bool is_async_ = false;
std::string urdf_ = "";
};

using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;
Expand Down
1 change: 1 addition & 0 deletions controller_manager/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,7 @@ if(BUILD_TESTING)
)
target_link_libraries(test_controller_manager_urdf_passing
controller_manager
test_controller
ros2_control_test_assets::ros2_control_test_assets
)
ament_target_dependencies(test_controller_manager_urdf_passing
Expand Down
2 changes: 1 addition & 1 deletion controller_manager/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ jitter, using a lowlatency kernel can improve things a lot with being really eas
Subscribers
-----------

~/robot_description [std_msgs::msg::String]
robot_description [std_msgs::msg::String]
String with the URDF xml, e.g., from ``robot_state_publisher``.
Reloading of the URDF is not supported yet.
All joints defined in the ``<ros2_control>``-tag have to be present in the URDF.
Expand Down
3 changes: 2 additions & 1 deletion controller_manager/include/controller_manager/controller_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -356,7 +356,7 @@ class ControllerManager : public rclcpp::Node
std::vector<std::string> get_controller_names();
std::pair<std::string, std::string> split_command_interface(
const std::string & command_interface);
void subscribe_to_robot_description_topic();
void init_controller_manager();

/**
* Clear request lists used when switching controllers. The lists are shared between "callback"
Expand Down Expand Up @@ -585,6 +585,7 @@ class ControllerManager : public rclcpp::Node

std::string robot_description_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;

struct SwitchParams
{
Expand Down
95 changes: 38 additions & 57 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,38 +200,7 @@ ControllerManager::ControllerManager(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
{
if (!get_parameter("update_rate", update_rate_))
{
RCLCPP_WARN(
get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
}

robot_description_ = "";
// TODO(destogl): remove support at the end of 2023
get_parameter("robot_description", robot_description_);
if (robot_description_.empty())
{
subscribe_to_robot_description_topic();
}
else
{
RCLCPP_WARN(
get_logger(),
"[Deprecated] Passing the robot description parameter directly to the control_manager node "
"is deprecated. Use the 'robot_description' topic from 'robot_state_publisher' instead.");
init_resource_manager(robot_description_);
if (is_resource_manager_initialized())
{
RCLCPP_WARN(
get_logger(),
"You have to restart the framework when using robot description from parameter!");
init_services();
}
}

diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
init_controller_manager();
}

ControllerManager::ControllerManager(
Expand All @@ -251,21 +220,7 @@ ControllerManager::ControllerManager(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
{
if (!get_parameter("update_rate", update_rate_))
{
RCLCPP_WARN(
get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
}

if (is_resource_manager_initialized())
{
init_services();
}
subscribe_to_robot_description_topic();

diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
init_controller_manager();
}

ControllerManager::ControllerManager(
Expand All @@ -282,6 +237,12 @@ ControllerManager::ControllerManager(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
{
init_controller_manager();
}

void ControllerManager::init_controller_manager()
{
// Get parameters needed for RT "update" loop to work
if (!get_parameter("update_rate", update_rate_))
{
RCLCPP_WARN(
Expand All @@ -292,15 +253,17 @@ ControllerManager::ControllerManager(
{
init_services();
}
subscribe_to_robot_description_topic();

diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
}
else
{
robot_description_notification_timer_ = create_wall_timer(
std::chrono::seconds(1),
[&]()
{
RCLCPP_WARN(
get_logger(), "Waiting for data on 'robot_description' topic to finish initialization");
});
}

void ControllerManager::subscribe_to_robot_description_topic()
{
// set QoS to transient local to get messages that have already been published
// (if robot state publisher starts before controller manager)
robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
Expand All @@ -309,6 +272,11 @@ void ControllerManager::subscribe_to_robot_description_topic()
RCLCPP_INFO(
get_logger(), "Subscribing to '%s' topic for robot description.",
robot_description_subscription_->get_topic_name());

// Setup diagnostics
diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
}

void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
Expand All @@ -321,13 +289,16 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
{
RCLCPP_WARN(
get_logger(),
"ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
"description file.");
"ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description.");
return;
}
init_resource_manager(robot_description_);
if (is_resource_manager_initialized())
{
RCLCPP_INFO(
get_logger(),
"Resource Manager has been successfully initialized. Starting Controller Manager "
"services...");
init_services();
}
}
Expand Down Expand Up @@ -397,6 +368,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
resource_manager_->set_component_state(component, active_state);
}
robot_description_notification_timer_->cancel();
}

void ControllerManager::init_services()
Expand Down Expand Up @@ -898,6 +870,15 @@ controller_interface::return_type ControllerManager::switch_controller(
const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
const rclcpp::Duration & timeout)
{
if (!is_resource_manager_initialized())
{
RCLCPP_ERROR(
get_logger(),
"Resource Manager is not initialized yet! Please provide robot description on "
"'robot_description' topic before trying to switch controllers.");
return controller_interface::return_type::ERROR;
}

switch_params_ = SwitchParams();

if (!deactivate_request_.empty() || !activate_request_.empty())
Expand Down
58 changes: 20 additions & 38 deletions controller_manager/test/controller_manager_test_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,46 +65,18 @@ class ControllerManagerFixture : public ::testing::Test
{
public:
explicit ControllerManagerFixture(
const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf,
const bool & pass_urdf_as_parameter = false)
: robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter)
const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf)
: robot_description_(robot_description)
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
// We want to be able to create a ResourceManager where no urdf file has been passed to
if (robot_description_.empty())
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(
rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
executor_, TEST_CM_NAME);
// We want to be able to not pass robot description immediately
if (!robot_description_.empty())
{
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(
rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
executor_, TEST_CM_NAME);
}
else
{
// can be removed later, needed if we want to have the deprecated way of passing the robot
// description file to the controller manager covered by tests
if (pass_urdf_as_parameter_)
{
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(
robot_description_, rm_node_->get_node_clock_interface(),
rm_node_->get_node_logging_interface(), true, 100),
executor_, TEST_CM_NAME);
}
else
{
// TODO(mamueluth) : passing via topic not working in test setup, tested cm does
// not receive msg. Have to check this...

// this is just a workaround to skip passing
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(
rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
executor_, TEST_CM_NAME);
// mimic topic call
auto msg = std_msgs::msg::String();
msg.data = robot_description_;
cm_->robot_description_callback(msg);
}
pass_robot_description_to_cm_and_rm(robot_description_);
}
time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type());
}
Expand Down Expand Up @@ -140,6 +112,17 @@ class ControllerManagerFixture : public ::testing::Test
}
}

void pass_robot_description_to_cm_and_rm(
const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf)
{
// TODO(Manuel) : passing via topic not working in test setup, tested cm does
// not receive msg. Have to check this...
// this is just a workaround to skip passing - mimic topic call
auto msg = std_msgs::msg::String();
msg.data = robot_description;
cm_->robot_description_callback(msg);
}

void switch_test_controllers(
const std::vector<std::string> & start_controllers,
const std::vector<std::string> & stop_controllers, const int strictness,
Expand All @@ -162,7 +145,6 @@ class ControllerManagerFixture : public ::testing::Test
std::thread updater_;
bool run_updater_;
const std::string robot_description_;
const bool pass_urdf_as_parameter_;
rclcpp::Time time_;

protected:
Expand Down
Loading

0 comments on commit 82107d2

Please sign in to comment.