Skip to content

Commit

Permalink
Merge branch 'master' into change/to/realtime_helpers
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor authored Nov 5, 2024
2 parents 1e3aecc + 814137d commit c5a86e4
Show file tree
Hide file tree
Showing 10 changed files with 95 additions and 60 deletions.
3 changes: 2 additions & 1 deletion .github/mergify.yml
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,15 @@ pull_request_rules:
- author=mergify[bot]
actions:
comment:
message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich?
message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor?

- name: development targets master branch
conditions:
- base!=master
- author!=bmagyar
- author!=destogl
- author!=christophfroehlich
- author!=saikishor
- author!=mergify[bot]
- author!=dependabot[bot]
actions:
Expand Down
10 changes: 5 additions & 5 deletions .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.6.0
rev: v5.0.0
hooks:
- id: check-added-large-files
- id: check-ast
Expand All @@ -37,7 +37,7 @@ repos:

# Python hooks
- repo: https://github.com/asottile/pyupgrade
rev: v3.17.0
rev: v3.19.0
hooks:
- id: pyupgrade
args: [--py36-plus]
Expand All @@ -50,7 +50,7 @@ repos:
args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]

- repo: https://github.com/psf/black
rev: 24.8.0
rev: 24.10.0
hooks:
- id: black
args: ["--line-length=99"]
Expand All @@ -63,7 +63,7 @@ repos:

# CPP hooks
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v19.1.0
rev: v19.1.3
hooks:
- id: clang-format
args: ['-fallback-style=none', '-i']
Expand Down Expand Up @@ -133,7 +133,7 @@ repos:
exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$

- repo: https://github.com/python-jsonschema/check-jsonschema
rev: 0.29.3
rev: 0.29.4
hooks:
- id: check-github-workflows
args: ["--verbose"]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
* **The method called in the (real-time) control loop.**
*
* \param[in] time The time at the start of this control loop iteration
* \param[in] period The measured time taken by the last control loop iteration
* \param[in] period The measured time since the last control loop iteration
* \returns return_type::OK if update is successfully, otherwise return_type::ERROR.
*/
CONTROLLER_INTERFACE_PUBLIC
Expand Down
3 changes: 2 additions & 1 deletion controller_manager/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,7 @@ if(BUILD_TESTING)

ament_add_gmock(test_controller_manager
test/test_controller_manager.cpp
TIMEOUT 180
)
target_link_libraries(test_controller_manager
controller_manager
Expand Down Expand Up @@ -195,7 +196,7 @@ if(BUILD_TESTING)
)

ament_add_gmock(test_hardware_spawner
test/test_hardware_spawner
test/test_hardware_spawner.cpp
TIMEOUT 120
)
target_link_libraries(test_hardware_spawner
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ class ControllerManager : public rclcpp::Node
controller_spec.c = controller;
controller_spec.info.name = controller_name;
controller_spec.info.type = controller_type;
controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
return add_controller_impl(controller_spec);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ struct ControllerSpec
{
hardware_interface::ControllerInfo info;
controller_interface::ControllerInterfaceBaseSharedPtr c;
std::shared_ptr<rclcpp::Time> next_update_cycle_time;
std::shared_ptr<rclcpp::Time> last_update_cycle_time;
};

struct ControllerChainSpec
Expand Down
34 changes: 21 additions & 13 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -543,7 +543,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
controller_spec.c = controller;
controller_spec.info.name = controller_name;
controller_spec.info.type = controller_type;
controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(
controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(
0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());

// We have to fetch the parameters_file at the time of loading the controller, because this way we
Expand Down Expand Up @@ -1668,8 +1668,8 @@ void ControllerManager::activate_controllers(
continue;
}
auto controller = found_it->c;
// reset the next update cycle time for newly activated controllers
*found_it->next_update_cycle_time =
// reset the last update cycle time for newly activated controllers
*found_it->last_update_cycle_time =
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());

bool assignment_successful = true;
Expand Down Expand Up @@ -2354,21 +2354,31 @@ controller_interface::return_type ControllerManager::update(
run_controller_at_cm_rate ? period
: rclcpp::Duration::from_seconds((1.0 / controller_update_rate));

const rclcpp::Time current_time = get_clock()->now();
if (
*loaded_controller.next_update_cycle_time ==
*loaded_controller.last_update_cycle_time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
{
// it is zero after activation
*loaded_controller.last_update_cycle_time = current_time - controller_period;
RCLCPP_DEBUG(
get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s",
time.seconds(), loaded_controller.info.name.c_str());
*loaded_controller.next_update_cycle_time = time;
get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s",
loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str());
}

bool controller_go =
const auto controller_actual_period =
(current_time - *loaded_controller.last_update_cycle_time);

/// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the
/// jitter in the system sleep cycles.
// For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have
// an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we
// wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue
// to keep up with the controller update rate (see issue #1769).
const bool controller_go =
run_controller_at_cm_rate ||
(time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
(time.seconds() >= loaded_controller.next_update_cycle_time->seconds());
(controller_actual_period.seconds() * controller_update_rate >= 0.99);

RCLCPP_DEBUG(
get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
Expand All @@ -2377,8 +2387,6 @@ controller_interface::return_type ControllerManager::update(

if (controller_go)
{
const auto controller_actual_period =
(time - *loaded_controller.next_update_cycle_time) + controller_period;
auto controller_ret = controller_interface::return_type::OK;
bool trigger_status = true;
// Catch exceptions thrown by the controller update function
Expand All @@ -2402,7 +2410,7 @@ controller_interface::return_type ControllerManager::update(
controller_ret = controller_interface::return_type::ERROR;
}

*loaded_controller.next_update_cycle_time += controller_period;
*loaded_controller.last_update_cycle_time = current_time;

if (controller_ret != controller_interface::return_type::OK)
{
Expand Down
20 changes: 19 additions & 1 deletion controller_manager/src/ros2_control_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,24 @@ int main(int argc, char ** argv)
auto cm = std::make_shared<controller_manager::ControllerManager>(
executor, manager_node_name, "", cm_node_options);

const bool lock_memory = cm->get_parameter_or<bool>("lock_memory", true);
std::string message;
if (lock_memory && !realtime_tools::lock_memory(message))
{
RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str());
}

const int cpu_affinity = cm->get_parameter_or<int>("cpu_affinity", -1);
if (cpu_affinity >= 0)
{
const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity);
if (!affinity_result.first)
{
RCLCPP_WARN(
cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str());
}
}

RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
RCLCPP_INFO(
Expand All @@ -79,7 +97,7 @@ int main(int argc, char ** argv)
{
RCLCPP_INFO(
cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.",
kSchedPriority);
thread_priority);
}

// for calculating sleep time
Expand Down
Loading

0 comments on commit c5a86e4

Please sign in to comment.