diff --git a/fake_joint_driver/README.md b/fake_joint_driver/README.md index c922e40..ff3e40a 100644 --- a/fake_joint_driver/README.md +++ b/fake_joint_driver/README.md @@ -48,6 +48,12 @@ sliders to set the joint trajectory goals. # Parameters +- `main_loop_rate` + + The internal loop rate at which the robot controls are updated. + This rate is a hard limit for joint state publishers. The default + value is set to 100 Hz. + - `use_robot_description` When it is true, the driver read the parameter `/robot_description` diff --git a/fake_joint_driver/src/fake_joint_driver_node.cpp b/fake_joint_driver/src/fake_joint_driver_node.cpp index 86aa6e7..9efc7f9 100644 --- a/fake_joint_driver/src/fake_joint_driver_node.cpp +++ b/fake_joint_driver/src/fake_joint_driver_node.cpp @@ -24,15 +24,18 @@ int main(int argc, char **argv) // Connect to controller manager controller_manager::ControllerManager cm(&robot, nh); - // Set spin ratge - ros::Rate rate(1.0 / ros::Duration(0.010).toSec()); + // Set loop rate + ros::NodeHandle pnh("~"); + int loop_rate; + pnh.param("main_loop_rate", loop_rate, 100); + ros::Rate rate(loop_rate); ros::AsyncSpinner spinner(1); spinner.start(); while (ros::ok()) { robot.update(); - cm.update(ros::Time::now(), ros::Duration(0.010)); + cm.update(ros::Time::now(), rate.expectedCycleTime()); rate.sleep(); } spinner.stop();