@@ -8,7 +8,7 @@ namespace robotiq_hande_driver {
88RobotiqHandeHardwareInterface::RobotiqHandeHardwareInterface () {}
99
1010HWI::CallbackReturn RobotiqHandeHardwareInterface::on_init (const HWI::HardwareInfo& info) {
11- if (HWI::SystemInterface::on_init (info) != CallbackReturn::SUCCESS) {
11+ if (HWI::SystemInterface::on_init (info) != CallbackReturn::SUCCESS) {
1212 return HWI::CallbackReturn::ERROR;
1313 }
1414
@@ -18,22 +18,25 @@ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_init(const HWI::HardwareIn
1818 tty_port_ = info_.hardware_parameters [" tty" ];
1919
2020 logger_ = std::make_shared<rclcpp::Logger>(
21- rclcpp::get_logger (" controller_manager.resource_manager.hardware_component.system.RobotiqHandeHardwareInterface" ));
21+ rclcpp::get_logger (" controller_manager.resource_manager.hardware_"
22+ " component.system.RobotiqHandeHardwareInterface" ));
2223
23- // TODO(modbus integration): Set parameters for the modbus communication
24+ // TODO(modbus integration): Set parameters for the modbus communication
2425
2526 RCLCPP_INFO (get_logger (), " Initialized ModbusRTU for %s" , tty_port_.c_str ());
2627 return HWI::CallbackReturn::SUCCESS;
2728}
2829
29- HWI::CallbackReturn RobotiqHandeHardwareInterface::on_configure (const rlccp_lc::State& /* previous_state*/ ){
30- // TODO(modbus integration): Initialize the ModbusRTU communication session
30+ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_configure (
31+ const rlccp_lc::State& /* previous_state*/ ) {
32+ // TODO(modbus integration): Initialize the ModbusRTU communication session
3133 RCLCPP_INFO (get_logger (), " configure()" );
3234 return HWI::CallbackReturn::SUCCESS;
3335}
3436
35- HWI::CallbackReturn RobotiqHandeHardwareInterface::on_cleanup (const rlccp_lc::State& /* previous_state*/ ){
36- // TODO(modbus integration): Deinitalize the ModbusRTU session
37+ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_cleanup (
38+ const rlccp_lc::State& /* previous_state*/ ) {
39+ // TODO(modbus integration): Deinitalize the ModbusRTU session
3740 RCLCPP_INFO (get_logger (), " cleanup()" );
3841 return HWI::CallbackReturn::SUCCESS;
3942}
@@ -42,9 +45,13 @@ std::vector<HWI::StateInterface> RobotiqHandeHardwareInterface::export_state_int
4245 std::vector<HWI::StateInterface> state_interfaces;
4346
4447 state_interfaces.emplace_back (hardware_interface::StateInterface (
45- info_.joints [LEFT_FINGER_JOINT_ID].name , hardware_interface::HW_IF_POSITION, &state_position_));
48+ info_.joints [LEFT_FINGER_JOINT_ID].name ,
49+ hardware_interface::HW_IF_POSITION,
50+ &state_position_));
4651 state_interfaces.emplace_back (hardware_interface::StateInterface (
47- info_.joints [LEFT_FINGER_JOINT_ID].name , hardware_interface::HW_IF_VELOCITY, &state_velocity_));
52+ info_.joints [LEFT_FINGER_JOINT_ID].name ,
53+ hardware_interface::HW_IF_VELOCITY,
54+ &state_velocity_));
4855
4956 RCLCPP_INFO (get_logger (), " export_state_interfaces()" );
5057 return state_interfaces;
@@ -54,50 +61,56 @@ std::vector<HWI::CommandInterface> RobotiqHandeHardwareInterface::export_command
5461 std::vector<hardware_interface::CommandInterface> command_interfaces;
5562
5663 command_interfaces.emplace_back (hardware_interface::CommandInterface (
57- info_.joints [LEFT_FINGER_JOINT_ID].name , hardware_interface::HW_IF_POSITION, &cmd_position_));
64+ info_.joints [LEFT_FINGER_JOINT_ID].name ,
65+ hardware_interface::HW_IF_POSITION,
66+ &cmd_position_));
5867
5968 RCLCPP_INFO (get_logger (), " export_command_interfaces()" );
6069 return command_interfaces;
6170}
6271
63- HWI::CallbackReturn RobotiqHandeHardwareInterface::on_activate (const rlccp_lc::State& /* previous_state*/ ){
64- // TODO(modbus integration): Power on the gripper (activation)
72+ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_activate (
73+ const rlccp_lc::State& /* previous_state*/ ) {
74+ // TODO(modbus integration): Power on the gripper (activation)
6575 RCLCPP_INFO (get_logger (), " activate()" );
6676 return HWI::CallbackReturn::SUCCESS;
6777}
6878
69- HWI::CallbackReturn RobotiqHandeHardwareInterface::on_deactivate (const rlccp_lc::State& /* previous_state*/ ){
70- // TODO(modbus integration): Deactiavte the gripper (set the reset flag)
79+ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_deactivate (
80+ const rlccp_lc::State& /* previous_state*/ ) {
81+ // TODO(modbus integration): Deactiavte the gripper (set the reset flag)
7182 RCLCPP_INFO (get_logger (), " deactivate()" );
7283 return HWI::CallbackReturn::SUCCESS;
7384}
7485
75- HWI::CallbackReturn RobotiqHandeHardwareInterface::on_shutdown (const rlccp_lc::State& /* previous_state*/ ){
76- // TODO(modbus integration): Deactivate the gripper, deinitalize the modbus RTU session,
86+ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_shutdown (
87+ const rlccp_lc::State& /* previous_state*/ ) {
88+ // TODO(modbus integration): Deactivate the gripper, deinitalize the modbus
89+ // RTU session,
7790 RCLCPP_INFO (get_logger (), " shutdown()" );
7891 return HWI::CallbackReturn::SUCCESS;
7992}
8093
81- HWI::CallbackReturn RobotiqHandeHardwareInterface::on_error (const rlccp_lc::State& /* previous_state*/ ){
82- // TODO(modbus integration): Placeholder for error handling
94+ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_error (
95+ const rlccp_lc::State& /* previous_state*/ ) {
96+ // TODO(modbus integration): Placeholder for error handling
8397 RCLCPP_INFO (get_logger (), " Handled error" );
8498 return HWI::CallbackReturn::SUCCESS;
8599}
86100
87- HWI::return_type RobotiqHandeHardwareInterface::read (const rclcpp::Time& /* time */ ,
88- const rclcpp::Duration& /* period*/ ) {
89- // TODO(modbus integration): data = driver_->receive_data();
101+ HWI::return_type RobotiqHandeHardwareInterface::read (
102+ const rclcpp::Time& /* time */ , const rclcpp::Duration& /* period*/ ) {
103+ // TODO(modbus integration): data = driver_->receive_data();
90104 return hardware_interface::return_type::OK;
91105}
92- HWI::return_type RobotiqHandeHardwareInterface::write (const rclcpp::Time& /* time */ ,
93- const rclcpp::Duration& /* period*/ ) {
94- // TODO(modbus integration): driver_->send_data(cmd_position_);
106+ HWI::return_type RobotiqHandeHardwareInterface::write (
107+ const rclcpp::Time& /* time */ , const rclcpp::Duration& /* period*/ ) {
108+ // TODO(modbus integration): driver_->send_data(cmd_position_);
95109 return hardware_interface::return_type::OK;
96110}
97111
98-
99- } // namespace robotiq_hande_driver
112+ } // namespace robotiq_hande_driver
100113
101114#include " pluginlib/class_list_macros.hpp"
102115PLUGINLIB_EXPORT_CLASS (
103- robotiq_hande_driver::RobotiqHandeHardwareInterface, hardware_interface::SystemInterface)
116+ robotiq_hande_driver::RobotiqHandeHardwareInterface, hardware_interface::SystemInterface)
0 commit comments