Skip to content

Commit 020ba51

Browse files
authored
Merge pull request #44 from stack-of-tasks/pre-commit-ci-update-config
[pre-commit.ci] pre-commit autoupdate
2 parents d20eeae + 6d52f29 commit 020ba51

File tree

3 files changed

+41
-41
lines changed

3 files changed

+41
-41
lines changed

.pre-commit-config.yaml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ repos:
1515
hooks:
1616
- id: pre-commit-sort
1717
- repo: https://github.com/pre-commit/mirrors-clang-format
18-
rev: v20.1.8
18+
rev: v21.1.2
1919
hooks:
2020
- id: clang-format
2121
args:

include/ros2_hardware_interface_odri/system_odri.hpp

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ class SystemOdriHardware : public hardware_interface::SystemInterface {
7878

7979
ROS2_CONTROL_ODRI_PUBLIC
8080
hardware_interface::CallbackReturn on_configure(
81-
const rclcpp_lifecycle::State &previous_state) override;
81+
const rclcpp_lifecycle::State& previous_state) override;
8282

8383
ROS2_CONTROL_ODRI_PUBLIC
8484
std::vector<hardware_interface::StateInterface> export_state_interfaces()
@@ -90,27 +90,27 @@ class SystemOdriHardware : public hardware_interface::SystemInterface {
9090

9191
ROS2_CONTROL_ODRI_PUBLIC
9292
return_type prepare_command_mode_switch(
93-
const std::vector<std::string> &start_interfaces,
94-
const std::vector<std::string> &stop_interfaces) override;
93+
const std::vector<std::string>& start_interfaces,
94+
const std::vector<std::string>& stop_interfaces) override;
9595

9696
ROS2_CONTROL_ODRI_PUBLIC
9797
return_type calibration();
9898

9999
ROS2_CONTROL_ODRI_PUBLIC
100100
hardware_interface::CallbackReturn on_activate(
101-
const rclcpp_lifecycle::State &previous_state) override;
101+
const rclcpp_lifecycle::State& previous_state) override;
102102

103103
ROS2_CONTROL_ODRI_PUBLIC
104104
hardware_interface::CallbackReturn on_deactivate(
105-
const rclcpp_lifecycle::State &previous_state) override;
105+
const rclcpp_lifecycle::State& previous_state) override;
106106

107107
ROS2_CONTROL_ODRI_PUBLIC
108-
hardware_interface::return_type read(const rclcpp::Time &,
109-
const rclcpp::Duration &) override;
108+
hardware_interface::return_type read(const rclcpp::Time&,
109+
const rclcpp::Duration&) override;
110110

111111
ROS2_CONTROL_ODRI_PUBLIC
112-
hardware_interface::return_type write(const rclcpp::Time &,
113-
const rclcpp::Duration &) override;
112+
hardware_interface::return_type write(const rclcpp::Time&,
113+
const rclcpp::Duration&) override;
114114

115115
ROS2_CONTROL_ODRI_PUBLIC
116116
return_type display();
@@ -128,7 +128,7 @@ class SystemOdriHardware : public hardware_interface::SystemInterface {
128128
return_type read_desired_starting_position();
129129

130130
// Read default joint cmd and state values
131-
return_type read_default_cmd_state_value(std::string &default_joint_cs);
131+
return_type read_default_cmd_state_value(std::string& default_joint_cs);
132132

133133
// Read default cmd or state value.
134134
// default_joint_cs: "default_joint_cmd" or "default_joint_state"

src/system_odri.cpp

Lines changed: 30 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero();
4444
Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero();
4545

4646
return_type SystemOdriHardware::read_default_cmd_state_value(
47-
std::string &default_joint_cs) {
47+
std::string& default_joint_cs) {
4848
// Hardware parameters provides a string
4949
if (info_.hardware_parameters.find(default_joint_cs) ==
5050
info_.hardware_parameters.end()) {
@@ -79,11 +79,11 @@ return_type SystemOdriHardware::read_default_cmd_state_value(
7979

8080
// Find the associate joint
8181
bool found_joint = false;
82-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
82+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
8383
if (joint.name == joint_name) {
84-
auto handle_dbl_and_msg = [](std::istringstream &iss_def_cmd_val,
85-
std::string &joint_name, double &adbl,
86-
std::string &msg) {
84+
auto handle_dbl_and_msg = [](std::istringstream& iss_def_cmd_val,
85+
std::string& joint_name, double& adbl,
86+
std::string& msg) {
8787
if (!iss_def_cmd_val.eof())
8888
iss_def_cmd_val >> adbl;
8989
else {
@@ -189,9 +189,9 @@ return_type SystemOdriHardware::read_desired_starting_position() {
189189
}
190190

191191
hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
192-
const rclcpp_lifecycle::State & /* previous_state */) {
192+
const rclcpp_lifecycle::State& /* previous_state */) {
193193
// For each sensor.
194-
for (const hardware_interface::ComponentInfo &sensor : info_.sensors) {
194+
for (const hardware_interface::ComponentInfo& sensor : info_.sensors) {
195195
imu_states_[sensor.name] = {std::numeric_limits<double>::quiet_NaN(),
196196
std::numeric_limits<double>::quiet_NaN(),
197197
std::numeric_limits<double>::quiet_NaN(),
@@ -210,7 +210,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
210210
std::numeric_limits<double>::quiet_NaN()};
211211
}
212212
// For each joint.
213-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
213+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
214214
// Initialize state of the joint by default to NaN
215215
// it allows to see which joints are not properly initialized
216216
// from the real hardware
@@ -237,7 +237,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
237237
}
238238

239239
// For each command interface of the joint
240-
for (const auto &a_joint_cmd_inter : joint.command_interfaces) {
240+
for (const auto& a_joint_cmd_inter : joint.command_interfaces) {
241241
// Check if the command interface is inside the list
242242
if (odri_list_of_cmd_inter.find(a_joint_cmd_inter.name) ==
243243
odri_list_of_cmd_inter.end()) {
@@ -246,7 +246,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
246246
"Joint '%s' have %s command interfaces found. One of the "
247247
"following values is expected",
248248
joint.name.c_str(), a_joint_cmd_inter.name.c_str());
249-
for (const auto &a_cmd_inter : odri_list_of_cmd_inter) {
249+
for (const auto& a_cmd_inter : odri_list_of_cmd_inter) {
250250
RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"),
251251
"'%s' expected.", a_cmd_inter.c_str());
252252
}
@@ -263,7 +263,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
263263
}
264264

265265
// For each state interface of the joint
266-
for (const auto &a_joint_state_inter : joint.state_interfaces) {
266+
for (const auto& a_joint_state_inter : joint.state_interfaces) {
267267
std::string joint_state_inter_name = a_joint_state_inter.name;
268268

269269
// Check if the state interface is inside the list
@@ -274,7 +274,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
274274
"was expected: ",
275275
joint.name.c_str(), a_joint_state_inter.name.c_str());
276276

277-
for (const auto &a_state_inter : odri_list_of_state_inter) {
277+
for (const auto& a_state_inter : odri_list_of_state_inter) {
278278
RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"),
279279
"'%s' expected.", a_state_inter.c_str());
280280
}
@@ -287,7 +287,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
287287
}
288288

289289
void SystemOdriHardware::display_robot_state() {
290-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
290+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
291291
std::cout << "joint " << joint.name << " "
292292
<< hw_commands_[joint.name].position << " "
293293
<< hw_commands_[joint.name].velocity << " "
@@ -299,17 +299,17 @@ void SystemOdriHardware::display_robot_state() {
299299
}
300300

301301
return_type SystemOdriHardware::prepare_command_mode_switch(
302-
const std::vector<std::string> &start_interfaces,
303-
const std::vector<std::string> &stop_interfaces) {
302+
const std::vector<std::string>& start_interfaces,
303+
const std::vector<std::string>& stop_interfaces) {
304304
// Initialize new modes.
305-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
305+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
306306
new_modes_[joint.name] = control_mode_t::NO_VALID_MODE;
307307
}
308308

309309
/// Check that the key interfaces are coherent
310-
for (auto &key : start_interfaces) {
310+
for (auto& key : start_interfaces) {
311311
/// For each joint
312-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
312+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
313313
if (key == joint.name + "/" + hardware_interface::HW_IF_POSITION) {
314314
new_modes_[joint.name] = control_mode_t::POSITION;
315315
}
@@ -332,7 +332,7 @@ return_type SystemOdriHardware::prepare_command_mode_switch(
332332
}
333333
// Stop motion on all relevant joints that are stopping
334334
for (std::string key : stop_interfaces) {
335-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
335+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
336336
if (key.find(joint.name) != std::string::npos) {
337337
hw_commands_[joint.name].velocity = 0.0;
338338
hw_commands_[joint.name].effort = 0.0;
@@ -342,7 +342,7 @@ return_type SystemOdriHardware::prepare_command_mode_switch(
342342
}
343343
}
344344
// Set the new command modes
345-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
345+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
346346
if ((control_mode_[joint.name] == control_mode_t::NO_VALID_MODE) &&
347347
(new_modes_[joint.name] == control_mode_t::NO_VALID_MODE)) {
348348
// Something else is using the joint! Abort!
@@ -363,7 +363,7 @@ return_type SystemOdriHardware::prepare_command_mode_switch(
363363
std::vector<hardware_interface::StateInterface>
364364
SystemOdriHardware::export_state_interfaces() {
365365
std::vector<hardware_interface::StateInterface> state_interfaces;
366-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
366+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
367367
state_interfaces.emplace_back(hardware_interface::StateInterface(
368368
joint.name, hardware_interface::HW_IF_POSITION,
369369
&hw_states_[joint.name].position));
@@ -379,7 +379,7 @@ SystemOdriHardware::export_state_interfaces() {
379379
joint.name, HW_IF_GAIN_KD, &hw_states_[joint.name].Kd));
380380
}
381381

382-
for (const hardware_interface::ComponentInfo &sensor : info_.sensors) {
382+
for (const hardware_interface::ComponentInfo& sensor : info_.sensors) {
383383
state_interfaces.emplace_back(hardware_interface::StateInterface(
384384
sensor.name, "gyroscope_x", &imu_states_[sensor.name].gyro_x));
385385
state_interfaces.emplace_back(hardware_interface::StateInterface(
@@ -428,7 +428,7 @@ std::vector<hardware_interface::CommandInterface>
428428
SystemOdriHardware::export_command_interfaces() {
429429
std::vector<hardware_interface::CommandInterface> command_interfaces;
430430

431-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
431+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
432432
command_interfaces.emplace_back(hardware_interface::CommandInterface(
433433
joint.name, hardware_interface::HW_IF_POSITION,
434434
&hw_commands_[joint.name].position));
@@ -448,7 +448,7 @@ SystemOdriHardware::export_command_interfaces() {
448448
}
449449

450450
hardware_interface::CallbackReturn SystemOdriHardware::on_activate(
451-
const rclcpp_lifecycle::State & /* previous_state */) {
451+
const rclcpp_lifecycle::State& /* previous_state */) {
452452
//// Read Parameters ////
453453

454454
/// Read odri_config_yaml
@@ -476,7 +476,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_activate(
476476
robot_->Initialize(eig_des_start_pos_);
477477

478478
/// Build the map from name to index.
479-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
479+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
480480
// First set the key
481481
joint_name_to_array_index_[joint.name] = 0;
482482
}
@@ -492,15 +492,15 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_activate(
492492
}
493493

494494
hardware_interface::CallbackReturn SystemOdriHardware::on_deactivate(
495-
const rclcpp_lifecycle::State & /* previous_state */) {
495+
const rclcpp_lifecycle::State& /* previous_state */) {
496496
// Stop the MasterBoard
497497
main_board_ptr_->MasterBoardInterface::Stop();
498498

499499
return hardware_interface::CallbackReturn::SUCCESS;
500500
}
501501

502502
hardware_interface::return_type SystemOdriHardware::read(
503-
const rclcpp::Time &, const rclcpp::Duration &) {
503+
const rclcpp::Time&, const rclcpp::Duration&) {
504504
// Data acquisition (with ODRI)
505505
robot_->ParseSensorData();
506506

@@ -515,7 +515,7 @@ hardware_interface::return_type SystemOdriHardware::read(
515515
auto imu_quater = robot_->imu->GetAttitudeQuaternion();
516516

517517
// Assignment of sensor data to ros2_control variables (URDF)
518-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
518+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
519519
hw_states_[joint.name].position =
520520
sensor_positions[joint_name_to_array_index_[joint.name]];
521521
hw_states_[joint.name].velocity =
@@ -553,15 +553,15 @@ hardware_interface::return_type SystemOdriHardware::read(
553553
}
554554

555555
hardware_interface::return_type SystemOdriHardware::write(
556-
const rclcpp::Time &, const rclcpp::Duration &) {
556+
const rclcpp::Time&, const rclcpp::Duration&) {
557557
Eigen::Vector6d positions;
558558
Eigen::Vector6d velocities;
559559
Eigen::Vector6d torques;
560560

561561
Eigen::Vector6d gain_KP;
562562
Eigen::Vector6d gain_KD;
563563

564-
for (const hardware_interface::ComponentInfo &joint : info_.joints) {
564+
for (const hardware_interface::ComponentInfo& joint : info_.joints) {
565565
if ((control_mode_[joint.name] == control_mode_t::POS_VEL_EFF_GAINS) ||
566566
(control_mode_[joint.name] == control_mode_t::POSITION)) {
567567
positions[joint_name_to_array_index_[joint.name]] =

0 commit comments

Comments
 (0)