2525
2626namespace ros2_control_demo_hardware
2727{
28-
2928hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure (
3029 const hardware_interface::HardwareInfo & info)
3130{
32- if (configure_default (info) != hardware_interface::return_type::OK) {
31+ if (configure_default (info) != hardware_interface::return_type::OK)
32+ {
3333 return hardware_interface::return_type::ERROR;
3434 }
3535
@@ -39,39 +39,42 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure(
3939 hw_states_.resize (info_.joints .size (), std::numeric_limits<double >::quiet_NaN ());
4040 hw_commands_.resize (info_.joints .size (), std::numeric_limits<double >::quiet_NaN ());
4141
42- for (const hardware_interface::ComponentInfo & joint : info_.joints ) {
42+ for (const hardware_interface::ComponentInfo & joint : info_.joints )
43+ {
4344 // RRBotSystemPositionOnly has exactly one state and command interface on each joint
44- if (joint.command_interfaces .size () != 1 ) {
45+ if (joint.command_interfaces .size () != 1 )
46+ {
4547 RCLCPP_FATAL (
4648 rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
47- " Joint '%s' has %d command interfaces found. 1 expected." ,
48- joint.name . c_str (), joint. command_interfaces .size ());
49+ " Joint '%s' has %d command interfaces found. 1 expected." , joint. name . c_str (),
50+ joint.command_interfaces .size ());
4951 return hardware_interface::return_type::ERROR;
5052 }
5153
52- if (joint.command_interfaces [0 ].name != hardware_interface::HW_IF_POSITION) {
54+ if (joint.command_interfaces [0 ].name != hardware_interface::HW_IF_POSITION)
55+ {
5356 RCLCPP_FATAL (
5457 rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
55- " Joint '%s' have %s command interfaces found. '%s' expected." ,
56- joint.name .c_str (), joint.command_interfaces [0 ].name .c_str (),
57- hardware_interface::HW_IF_POSITION);
58+ " Joint '%s' have %s command interfaces found. '%s' expected." , joint.name .c_str (),
59+ joint.command_interfaces [0 ].name .c_str (), hardware_interface::HW_IF_POSITION);
5860 return hardware_interface::return_type::ERROR;
5961 }
6062
61- if (joint.state_interfaces .size () != 1 ) {
63+ if (joint.state_interfaces .size () != 1 )
64+ {
6265 RCLCPP_FATAL (
6366 rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
64- " Joint '%s' has %d state interface. 1 expected." ,
65- joint.name . c_str (), joint. state_interfaces .size ());
67+ " Joint '%s' has %d state interface. 1 expected." , joint. name . c_str (),
68+ joint.state_interfaces .size ());
6669 return hardware_interface::return_type::ERROR;
6770 }
6871
69- if (joint.state_interfaces [0 ].name != hardware_interface::HW_IF_POSITION) {
72+ if (joint.state_interfaces [0 ].name != hardware_interface::HW_IF_POSITION)
73+ {
7074 RCLCPP_FATAL (
7175 rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
72- " Joint '%s' have %s state interface. '%s' expected." ,
73- joint.name .c_str (), joint.state_interfaces [0 ].name .c_str (),
74- hardware_interface::HW_IF_POSITION);
76+ " Joint '%s' have %s state interface. '%s' expected." , joint.name .c_str (),
77+ joint.state_interfaces [0 ].name .c_str (), hardware_interface::HW_IF_POSITION);
7578 return hardware_interface::return_type::ERROR;
7679 }
7780 }
@@ -84,10 +87,10 @@ std::vector<hardware_interface::StateInterface>
8487RRBotSystemPositionOnlyHardware::export_state_interfaces ()
8588{
8689 std::vector<hardware_interface::StateInterface> state_interfaces;
87- for (uint i = 0 ; i < info_.joints .size (); i++) {
88- state_interfaces. emplace_back (
89- hardware_interface::StateInterface (
90- info_.joints [i].name , hardware_interface::HW_IF_POSITION, &hw_states_[i]));
90+ for (uint i = 0 ; i < info_.joints .size (); i++)
91+ {
92+ state_interfaces. emplace_back ( hardware_interface::StateInterface (
93+ info_.joints [i].name , hardware_interface::HW_IF_POSITION, &hw_states_[i]));
9194 }
9295
9396 return state_interfaces;
@@ -97,105 +100,99 @@ std::vector<hardware_interface::CommandInterface>
97100RRBotSystemPositionOnlyHardware::export_command_interfaces ()
98101{
99102 std::vector<hardware_interface::CommandInterface> command_interfaces;
100- for (uint i = 0 ; i < info_.joints .size (); i++) {
101- command_interfaces. emplace_back (
102- hardware_interface::CommandInterface (
103- info_.joints [i].name , hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
103+ for (uint i = 0 ; i < info_.joints .size (); i++)
104+ {
105+ command_interfaces. emplace_back ( hardware_interface::CommandInterface (
106+ info_.joints [i].name , hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
104107 }
105108
106109 return command_interfaces;
107110}
108111
109-
110112hardware_interface::return_type RRBotSystemPositionOnlyHardware::start ()
111113{
112- RCLCPP_INFO (
113- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
114- " Starting ...please wait..." );
114+ RCLCPP_INFO (rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " Starting ...please wait..." );
115115
116- for (int i = 0 ; i <= hw_start_sec_; i++) {
116+ for (int i = 0 ; i <= hw_start_sec_; i++)
117+ {
117118 rclcpp::sleep_for (std::chrono::seconds (1 ));
118119 RCLCPP_INFO (
119- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
120- " %.1f seconds left... " , hw_start_sec_ - i);
120+ rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " %.1f seconds left... " ,
121+ hw_start_sec_ - i);
121122 }
122123
123124 // set some default values when starting the first time
124- for (uint i = 0 ; i < hw_states_.size (); i++) {
125- if (std::isnan (hw_states_[i])) {
125+ for (uint i = 0 ; i < hw_states_.size (); i++)
126+ {
127+ if (std::isnan (hw_states_[i]))
128+ {
126129 hw_states_[i] = 0 ;
127130 hw_commands_[i] = 0 ;
128- } else {
131+ }
132+ else
133+ {
129134 hw_commands_[i] = hw_states_[i];
130135 }
131136 }
132137
133138 status_ = hardware_interface::status::STARTED;
134139
135140 RCLCPP_INFO (
136- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
137- " System Sucessfully started!" );
141+ rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " System Successfully started!" );
138142
139143 return hardware_interface::return_type::OK;
140144}
141145
142146hardware_interface::return_type RRBotSystemPositionOnlyHardware::stop ()
143147{
144- RCLCPP_INFO (
145- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
146- " Stopping ...please wait..." );
148+ RCLCPP_INFO (rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " Stopping ...please wait..." );
147149
148- for (int i = 0 ; i <= hw_stop_sec_; i++) {
150+ for (int i = 0 ; i <= hw_stop_sec_; i++)
151+ {
149152 rclcpp::sleep_for (std::chrono::seconds (1 ));
150153 RCLCPP_INFO (
151- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
152- " %.1f seconds left... " , hw_stop_sec_ - i);
154+ rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " %.1f seconds left... " ,
155+ hw_stop_sec_ - i);
153156 }
154157
155158 status_ = hardware_interface::status::STOPPED;
156159
157160 RCLCPP_INFO (
158- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
159- " System sucessfully stopped!" );
161+ rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " System successfully stopped!" );
160162
161163 return hardware_interface::return_type::OK;
162164}
163165
164166hardware_interface::return_type RRBotSystemPositionOnlyHardware::read ()
165167{
166- RCLCPP_INFO (
167- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
168- " Reading..." );
168+ RCLCPP_INFO (rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " Reading..." );
169169
170- for (uint i = 0 ; i < hw_states_.size (); i++) {
170+ for (uint i = 0 ; i < hw_states_.size (); i++)
171+ {
171172 // Simulate RRBot's movement
172173 hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
173174 RCLCPP_INFO (
174- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
175- " Got state %.5f for joint %d! " , hw_states_[i], i);
175+ rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " Got state %.5f for joint %d! " ,
176+ hw_states_[i], i);
176177 }
177- RCLCPP_INFO (
178- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
179- " Joints sucessfully read!" );
178+ RCLCPP_INFO (rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " Joints successfully read!" );
180179
181180 return hardware_interface::return_type::OK;
182181}
183182
184183hardware_interface::return_type RRBotSystemPositionOnlyHardware::write ()
185184{
186- RCLCPP_INFO (
187- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
188- " Writing..." );
185+ RCLCPP_INFO (rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " Writing..." );
189186
190- for (uint i = 0 ; i < hw_commands_.size (); i++) {
187+ for (uint i = 0 ; i < hw_commands_.size (); i++)
188+ {
191189 // Simulate sending commands to the hardware
192190 RCLCPP_INFO (
193- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
194- " Got command %.5f for joint %d! " , hw_commands_[i], i);
191+ rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " Got command %.5f for joint %d! " ,
192+ hw_commands_[i], i);
195193 }
196194 RCLCPP_INFO (
197- rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ),
198- " Joints sucessfully written!" );
195+ rclcpp::get_logger (" RRBotSystemPositionOnlyHardware" ), " Joints successfully written!" );
199196
200197 return hardware_interface::return_type::OK;
201198}
@@ -205,6 +202,4 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write()
205202#include " pluginlib/class_list_macros.hpp"
206203
207204PLUGINLIB_EXPORT_CLASS (
208- ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware,
209- hardware_interface::SystemInterface
210- )
205+ ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface)
0 commit comments