@@ -44,7 +44,7 @@ Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero();
4444Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero();
4545
4646return_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
191191hardware_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
289289void 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
301301return_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(
363363std::vector<hardware_interface::StateInterface>
364364SystemOdriHardware::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>
428428SystemOdriHardware::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
450450hardware_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
494494hardware_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
502502hardware_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
555555hardware_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