Skip to content

Commit de2d3c9

Browse files
committed
bug fixes
1 parent adcba1e commit de2d3c9

File tree

2 files changed

+20
-28
lines changed

2 files changed

+20
-28
lines changed

example_11/hardware/carlikebot_system.cpp

+20-26
Original file line numberDiff line numberDiff line change
@@ -233,23 +233,20 @@ CarlikeBotSystemHardware::export_command_interfaces()
233233
hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
234234
const rclcpp_lifecycle::State & /*previous_state*/)
235235
{
236-
if (m_running_simulation)
237-
{
238-
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait...");
236+
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait...");
239237

240-
for (auto i = 0; i < hw_start_sec_; i++)
241-
{
242-
rclcpp::sleep_for(std::chrono::seconds(1));
243-
RCLCPP_INFO(
244-
rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i);
245-
}
238+
for (auto i = 0; i < hw_start_sec_; i++)
239+
{
240+
rclcpp::sleep_for(std::chrono::seconds(1));
241+
RCLCPP_INFO(
242+
rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i);
243+
}
246244

247-
for (uint i = 0; i < hw_positions_.size(); i++)
248-
{
249-
hw_positions_[i] = 0.0;
250-
hw_velocities_[i] = 0.0;
251-
hw_commands_[i] = 0.0;
252-
}
245+
for (uint i = 0; i < hw_positions_.size(); i++)
246+
{
247+
hw_positions_[i] = 0.0;
248+
hw_velocities_[i] = 0.0;
249+
hw_commands_[i] = 0.0;
253250
}
254251

255252
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully activated!");
@@ -260,19 +257,16 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
260257
hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate(
261258
const rclcpp_lifecycle::State & /*previous_state*/)
262259
{
263-
if (m_running_simulation)
264-
{
265-
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
266-
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait...");
260+
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
261+
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait...");
267262

268-
for (auto i = 0; i < hw_stop_sec_; i++)
269-
{
270-
rclcpp::sleep_for(std::chrono::seconds(1));
271-
RCLCPP_INFO(
272-
rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i);
273-
}
274-
// END: This part here is for exemplary purposes - Please do not copy to your production code
263+
for (auto i = 0; i < hw_stop_sec_; i++)
264+
{
265+
rclcpp::sleep_for(std::chrono::seconds(1));
266+
RCLCPP_INFO(
267+
rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i);
275268
}
269+
// END: This part here is for exemplary purposes - Please do not copy to your production code
276270
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully deactivated!");
277271

278272
return hardware_interface::CallbackReturn::SUCCESS;

example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,6 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
6868
const rclcpp::Time & time, const rclcpp::Duration & period) override;
6969

7070
private:
71-
// Parameter disnguishing between simulation and physical robot
72-
bool m_running_simulation;
7371

7472
// Parameters for the CarlikeBot simulation
7573
double hw_start_sec_;

0 commit comments

Comments
 (0)