Skip to content

Commit

Permalink
Update to clang format 12 (#731)
Browse files Browse the repository at this point in the history
* Update to clang-format-12

* Re-run clang-format

* adjust joint limits file too
  • Loading branch information
bmagyar authored Jun 8, 2022
1 parent 8fd48f7 commit 6f5ee7c
Show file tree
Hide file tree
Showing 15 changed files with 162 additions and 130 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/ci-format.yml
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ jobs:
with:
python-version: 3.9.7
- name: Install system hooks
run: sudo apt install -qq clang-format-11 cppcheck
run: sudo apt install -qq clang-format-12 cppcheck
- uses: pre-commit/[email protected]
with:
extra_args: --all-files --hook-stage manual
2 changes: 1 addition & 1 deletion .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ repos:
- id: clang-format
name: clang-format
description: Format files with ClangFormat.
entry: clang-format-10
entry: clang-format-12
language: system
files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
args: ['-fallback-style=none', '-i']
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,8 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
const std::string & interface_torque_y, const std::string & interface_torque_z)
: SemanticComponentInterface("", 6)
{
auto check_and_add_interface = [this](const std::string & interface_name, const int index) {
auto check_and_add_interface = [this](const std::string & interface_name, const int index)
{
if (!interface_name.empty())
{
interface_names_.emplace_back(interface_name);
Expand Down
12 changes: 7 additions & 5 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -464,7 +464,8 @@ controller_interface::return_type ControllerManager::switch_controller(
const auto list_controllers = [this, strictness](
const std::vector<std::string> & controller_list,
std::vector<std::string> & request_list,
const std::string & action) {
const std::string & action)
{
// lock controllers
std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);

Expand Down Expand Up @@ -524,9 +525,9 @@ controller_interface::return_type ControllerManager::switch_controller(
return ret;
}

const auto list_interfaces = [this](
const ControllerSpec controller,
std::vector<std::string> & request_interface_list) {
const auto list_interfaces =
[this](const ControllerSpec controller, std::vector<std::string> & request_interface_list)
{
auto command_interface_config = controller.c->command_interface_configuration();
std::vector<std::string> command_interface_names = {};
if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
Expand Down Expand Up @@ -560,7 +561,8 @@ controller_interface::return_type ControllerManager::switch_controller(
const bool is_active = is_controller_active(*controller.c);
const bool is_inactive = is_controller_inactive(*controller.c);

auto handle_conflict = [&](const std::string & msg) {
auto handle_conflict = [&](const std::string & msg)
{
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
{
RCLCPP_ERROR(get_logger(), "%s", msg.c_str());
Expand Down
45 changes: 24 additions & 21 deletions controller_manager/src/ros2_control_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,31 +37,34 @@ int main(int argc, char ** argv)
// the executor (see issue #260).
// When the MutliThreadedExecutor issues are fixed (ros2/rclcpp#1168), this loop should be
// converted back to a timer.
std::thread cm_thread([cm]() {
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
std::thread cm_thread(
[cm]()
{
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());

rclcpp::Time current_time = cm->now();
rclcpp::Time previous_time = current_time;
rclcpp::Time end_period = current_time;
rclcpp::Time current_time = cm->now();
rclcpp::Time previous_time = current_time;
rclcpp::Time end_period = current_time;

// Use nanoseconds to avoid chrono's rounding
rclcpp::Duration period(std::chrono::nanoseconds(1000000000 / cm->get_update_rate()));
// Use nanoseconds to avoid chrono's rounding
rclcpp::Duration period(std::chrono::nanoseconds(1000000000 / cm->get_update_rate()));

while (rclcpp::ok())
{
// wait until we hit the end of the period
end_period += period;
std::this_thread::sleep_for(std::chrono::nanoseconds((end_period - cm->now()).nanoseconds()));
while (rclcpp::ok())
{
// wait until we hit the end of the period
end_period += period;
std::this_thread::sleep_for(
std::chrono::nanoseconds((end_period - cm->now()).nanoseconds()));

// execute update loop
auto period = current_time - previous_time;
cm->read(current_time, period);
current_time = cm->now();
cm->update(current_time, period);
previous_time = current_time;
cm->write(current_time, period);
}
});
// execute update loop
auto period = current_time - previous_time;
cm->read(current_time, period);
current_time = cm->now();
cm->update(current_time, period);
previous_time = current_time;
cm->write(current_time, period);
}
});

executor->add_node(cm);
executor->spin();
Expand Down
27 changes: 16 additions & 11 deletions controller_manager/test/controller_manager_test_common.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,13 +79,15 @@ class ControllerManagerFixture : public ::testing::Test
void startCmUpdater()
{
run_updater_ = true;
updater_ = std::thread([&](void) -> void {
while (run_updater_)
updater_ = std::thread(
[&](void) -> void
{
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
});
while (run_updater_)
{
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
});
}

void stopCmUpdater()
Expand Down Expand Up @@ -135,11 +137,14 @@ class TestControllerManagerSrvs

void SetUpSrvsCMExecutor()
{
update_timer_ = cm_->create_wall_timer(std::chrono::milliseconds(10), [&]() {
cm_->read(TIME, PERIOD);
cm_->update(TIME, PERIOD);
cm_->write(TIME, PERIOD);
});
update_timer_ = cm_->create_wall_timer(
std::chrono::milliseconds(10),
[&]()
{
cm_->read(TIME, PERIOD);
cm_->update(TIME, PERIOD);
cm_->write(TIME, PERIOD);
});

executor_->add_node(cm_);

Expand Down
19 changes: 10 additions & 9 deletions controller_manager/test/test_hardware_management_srvs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -120,15 +120,16 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
[](
const std::vector<controller_manager_msgs::msg::HardwareInterface> & interfaces,
const std::vector<const char *> & interface_names,
const std::vector<bool> is_available_status, const std::vector<bool> is_claimed_status) {
for (auto i = 0ul; i < interfaces.size(); ++i)
{
auto it = std::find(interface_names.begin(), interface_names.end(), interfaces[i].name);
EXPECT_NE(it, interface_names.end());
EXPECT_EQ(interfaces[i].is_available, is_available_status[i]);
EXPECT_EQ(interfaces[i].is_claimed, is_claimed_status[i]);
}
};
const std::vector<bool> is_available_status, const std::vector<bool> is_claimed_status)
{
for (auto i = 0ul; i < interfaces.size(); ++i)
{
auto it = std::find(interface_names.begin(), interface_names.end(), interfaces[i].name);
EXPECT_NE(it, interface_names.end());
EXPECT_EQ(interfaces[i].is_available, is_available_status[i]);
EXPECT_EQ(interfaces[i].is_claimed, is_claimed_status[i]);
}
};

for (const auto & component : result->component)
{
Expand Down
13 changes: 8 additions & 5 deletions controller_manager/test/test_spawner_unspawner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,14 @@ class TestLoadController : public ControllerManagerFixture<controller_manager::C
{
ControllerManagerFixture::SetUp();

update_timer_ = cm_->create_wall_timer(std::chrono::milliseconds(10), [&]() {
cm_->read(TIME, PERIOD);
cm_->update(TIME, PERIOD);
cm_->write(TIME, PERIOD);
});
update_timer_ = cm_->create_wall_timer(
std::chrono::milliseconds(10),
[&]()
{
cm_->read(TIME, PERIOD);
cm_->update(TIME, PERIOD);
cm_->write(TIME, PERIOD);
});

update_executor_ =
std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(), 2);
Expand Down
13 changes: 7 additions & 6 deletions hardware_interface/src/fake_components/generic_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,9 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
return CallbackReturn::ERROR;
}

auto populate_non_standard_interfaces = [this](
auto interface_list, auto & non_standard_interfaces) {
auto populate_non_standard_interfaces =
[this](auto interface_list, auto & non_standard_interfaces)
{
for (const auto & interface : interface_list)
{
// add to list if non-standard interface
Expand Down Expand Up @@ -120,9 +121,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
const auto mimicked_joint_it = std::find_if(
info_.joints.begin(), info_.joints.end(),
[&mimicked_joint =
joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) {
return joint_info.name == mimicked_joint;
});
joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info)
{ return joint_info.name == mimicked_joint; });
if (mimicked_joint_it == info_.joints.cend())
{
throw std::runtime_error(
Expand Down Expand Up @@ -320,7 +320,8 @@ std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_

return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0) {
auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0)
{
for (size_t i = start_index; i < states_.size(); ++i)
{
for (size_t j = 0; j < states_[i].size(); ++j)
Expand Down
45 changes: 24 additions & 21 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,28 +40,29 @@ namespace hardware_interface
auto trigger_and_print_hardware_state_transition =
[](
auto transition, const std::string transition_name, const std::string & hardware_name,
const lifecycle_msgs::msg::State::_id_type & target_state) {
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "'%s' hardware '%s' ", transition_name.c_str(), hardware_name.c_str());
const lifecycle_msgs::msg::State::_id_type & target_state)
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "'%s' hardware '%s' ", transition_name.c_str(), hardware_name.c_str());

const rclcpp_lifecycle::State new_state = transition();
const rclcpp_lifecycle::State new_state = transition();

bool result = new_state.id() == target_state;
bool result = new_state.id() == target_state;

if (result)
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Successful '%s' of hardware '%s'", transition_name.c_str(),
hardware_name.c_str());
}
else
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str(),
hardware_name.c_str());
}
return result;
};
if (result)
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Successful '%s' of hardware '%s'", transition_name.c_str(),
hardware_name.c_str());
}
else
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str(),
hardware_name.c_str());
}
return result;
};

class ResourceStorage
{
Expand Down Expand Up @@ -843,7 +844,8 @@ bool ResourceManager::prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces)
{
auto interfaces_to_string = [&]() {
auto interfaces_to_string = [&]()
{
std::stringstream ss;
ss << "Start interfaces: " << std::endl << "[" << std::endl;
for (const auto & start_if : start_interfaces)
Expand Down Expand Up @@ -953,7 +955,8 @@ return_type ResourceManager::set_component_state(
}
}

auto find_set_component_state = [&](auto action, auto & components) {
auto find_set_component_state = [&](auto action, auto & components)
{
auto found_component_it = std::find_if(
components.begin(), components.end(),
[&](const auto & component) { return component.get_name() == component_name; });
Expand Down
22 changes: 12 additions & 10 deletions hardware_interface/test/fake_components/test_generic_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -421,28 +421,30 @@ void set_components_state(

auto configure_components = [](
hardware_interface::ResourceManager & rm,
const std::vector<std::string> & components = {"GenericSystem2dof"}) {
const std::vector<std::string> & components = {"GenericSystem2dof"})
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
hardware_interface::lifecycle_state_names::INACTIVE);
};

auto activate_components = [](
hardware_interface::ResourceManager & rm,
const std::vector<std::string> & components = {"GenericSystem2dof"}) {
const std::vector<std::string> & components = {"GenericSystem2dof"})
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE);
};

auto deactivate_components =
[](
hardware_interface::ResourceManager & rm,
const std::vector<std::string> & components = {"GenericSystem2dof"}) {
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
hardware_interface::lifecycle_state_names::INACTIVE);
};
auto deactivate_components = [](
hardware_interface::ResourceManager & rm,
const std::vector<std::string> & components = {"GenericSystem2dof"})
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
hardware_interface::lifecycle_state_names::INACTIVE);
};

TEST_F(TestGenericSystem, load_generic_system_2dof)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,9 @@ class TestForceTorqueSensor : public SensorInterface
{
if (
std::find_if(
state_interfaces.begin(), state_interfaces.end(), [&ft_key](const auto & interface_info) {
return interface_info.name == ft_key;
}) == state_interfaces.end())
state_interfaces.begin(), state_interfaces.end(),
[&ft_key](const auto & interface_info)
{ return interface_info.name == ft_key; }) == state_interfaces.end())
{
return CallbackReturn::ERROR;
}
Expand Down
Loading

0 comments on commit 6f5ee7c

Please sign in to comment.