Skip to content

Commit

Permalink
formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
ARK3r committed Oct 3, 2023
1 parent eb66756 commit 9d37c17
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 18 deletions.
27 changes: 10 additions & 17 deletions example_11/hardware/carlikebot_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,14 +146,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
// // END: This part here is for exemplary purposes - Please do not copy to your production code

// joint name, state, command
hw_interfaces_.resize(4, std::make_tuple(
std::string(),
std::numeric_limits<double>::quiet_NaN(),
std::numeric_limits<double>::quiet_NaN()
)
);


hw_interfaces_.resize(
4, std::make_tuple(
std::string(), std::numeric_limits<double>::quiet_NaN(),
std::numeric_limits<double>::quiet_NaN()));

// hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
// hw_velocities_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
Expand All @@ -165,7 +161,6 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
// hw_position_states_.resize(2, std::numeric_limits<double>::queit_NaN());
// hw_velocity_states_.resize(2, std::numeric_limits<double>::queit_NaN());


return hardware_interface::CallbackReturn::SUCCESS;
}

Expand All @@ -186,9 +181,8 @@ std::vector<hardware_interface::StateInterface> CarlikeBotSystemHardware::export
// get the index of the joint name in hw_interfaces_
auto it = std::find_if(
hw_interfaces_.begin(), hw_interfaces_.end(),
[this, i](const std::tuple<std::string, double, double> & element) {
return std::get<0>(element) == info_.joints[i].name;
});
[this, i](const std::tuple<std::string, double, double> & element)
{ return std::get<0>(element) == info_.joints[i].name; });

if (joint_is_steering)
{
Expand Down Expand Up @@ -234,9 +228,8 @@ CarlikeBotSystemHardware::export_command_interfaces()
// get the index of the joint name in hw_interfaces_
auto it = std::find_if(
hw_interfaces_.begin(), hw_interfaces_.end(),
[this, i](const std::tuple<std::string, double, double> & element) {
return std::get<0>(element) == info_.joints[i].name;
});
[this, i](const std::tuple<std::string, double, double> & element)
{ return std::get<0>(element) == info_.joints[i].name; });

if (joint_is_steering)
{
Expand Down Expand Up @@ -346,8 +339,8 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH
std::get<2>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]);

// RCLCPP_INFO(
// rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[i],
// info_.joints[i].name.c_str());
// rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'",
// hw_commands_[i], info_.joints[i].name.c_str());
}

return hardware_interface::return_type::OK;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,8 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
// std::vector<double> hw_commands_;
// std::vector<double> hw_states_;

std::vector<std::tuple<std::string, double, double>> hw_interfaces_; // name of joint, state, command
std::vector<std::tuple<std::string, double, double>>
hw_interfaces_; // name of joint, state, command

// std::vector<std::tuple<unsigned int, std::string, int>> commands_counterpart_;

Expand Down

0 comments on commit 9d37c17

Please sign in to comment.