File tree Expand file tree Collapse file tree 3 files changed +3
-6
lines changed
ros2_control_demo_hardware
include/ros2_control_demo_hardware
ros2_control_demo_robot/description/diffbot Expand file tree Collapse file tree 3 files changed +3
-6
lines changed Original file line number Diff line number Diff line change @@ -62,7 +62,6 @@ class DiffBotSystemHardware : public
6262 // Parameters for the DiffBot simulation
6363 double hw_start_sec_;
6464 double hw_stop_sec_;
65- double hw_slowdown_;
6665
6766 // Store the command for the simulated robot
6867 std::vector<double > hw_commands_;
Original file line number Diff line number Diff line change @@ -35,7 +35,6 @@ hardware_interface::return_type DiffBotSystemHardware::configure(
3535
3636 hw_start_sec_ = stod (info_.hardware_parameters [" example_param_hw_start_duration_sec" ]);
3737 hw_stop_sec_ = stod (info_.hardware_parameters [" example_param_hw_stop_duration_sec" ]);
38- hw_slowdown_ = stod (info_.hardware_parameters [" example_param_hw_slowdown" ]);
3938 hw_states_.resize (info_.joints .size (), std::numeric_limits<double >::quiet_NaN ());
4039 hw_commands_.resize (info_.joints .size (), std::numeric_limits<double >::quiet_NaN ());
4140
@@ -177,11 +176,11 @@ hardware_interface::return_type DiffBotSystemHardware::read()
177176 " Reading..." );
178177
179178 double tau = 0.2 ;
180- double a = tau/ (tau+ 1 );
181- double b = 1 / (tau+ 1 );
179+ double a = tau / (tau + 1 );
180+ double b = 1 / (tau + 1 );
182181 for (uint i = 0 ; i < hw_commands_.size (); i++) {
183182 // Simulate DiffBot wheels's movement as a first-order system
184- hw_states_[1 ] = a* hw_states_[1 ] + b* hw_commands_[i];
183+ hw_states_[1 ] = a * hw_states_[1 ] + b * hw_commands_[i];
185184 hw_states_[0 ] += hw_states_[1 ] / 2 ;
186185 RCLCPP_INFO (
187186 rclcpp::get_logger (" DiffBotSystemHardware" ),
Original file line number Diff line number Diff line change 88 <plugin >ros2_control_demo_hardware/DiffBotSystemHardware</plugin >
99 <param name =" example_param_hw_start_duration_sec" >2.0</param >
1010 <param name =" example_param_hw_stop_duration_sec" >3.0</param >
11- <param name =" example_param_hw_slowdown" >2.0</param >
1211 </hardware >
1312 <joint name =" left_wheel_joint" >
1413 <command_interface name =" velocity" >
You can’t perform that action at this time.
0 commit comments