Communicates with the high level hardware_interface::RobotHW and interacts with the robot hardware sensors (e.g. encoders) and actuators (e.g. motor driver). More...
#include <base_controller.h>
Classes | |
| struct | LastUpdateTime |
| Keeps track of the last update times. More... | |
| struct | UpdateRate |
| Stores update rate frequencies (Hz) for the main control loop, (optinal) imu and debug output. More... | |
Public Member Functions | |
| BaseController (ros::NodeHandle &nh, TMotorController *motor_controller_left, TMotorController *motor_controller_right) | |
| Construct a new Base Controller object using two generic motor controllers. More... | |
| void | commandCallback (const diffbot_msgs::WheelsCmdStamped &cmd_msg) |
| Callback method when a new diffbot_msgs::WheelsCmdStamped is received on the wheel_cmd_velocities topic. More... | |
| bool | debug () |
| Getter if the firmware should log debug output. More... | |
| void | eStop () |
| Stops the motors, in case no wheel commands are received over a longer time period. More... | |
| void | init () |
| Reads parameters from the parameter server. More... | |
| LastUpdateTime & | lastUpdateTime () |
| Returns a reference to last_update_times_. More... | |
| int | period (double frequency) |
Calculates the period (s) from a given frequency (Hz). More... | |
| void | pidLeftCallback (const diffbot_msgs::PIDStamped &pid_msg) |
| Callback method to update the PID constants for the left motor. More... | |
| void | pidRightCallback (const diffbot_msgs::PIDStamped &pid_msg) |
| Callback method to update the PID constants for the right motor. More... | |
| void | printDebug () |
| UpdateRate & | publishRate () |
| void | read () |
| Reads the current encoder tick counts and joint states (angular position (rad) and angular velocity (rad/s) from both encoders left encoder_left_ and right encoder_right_ and publishes sensor_msgs::JointState on the "measured_joint_states" topic, using the pub_measured_joint_states_. More... | |
| void | resetEncodersCallback (const std_msgs::Empty &reset_msg) |
| Callback method to reset both encoder's tick count to zero. More... | |
| void | setup () |
| Initializes the main node handle and setup publisher and subscriber. More... | |
| void | write () |
| Uses the PIDs to compute capped PWM signals for the left and right motor. More... | |
Public Attributes | |
| struct diffbot::BaseController::LastUpdateTime | last_update_time_ |
| struct diffbot::BaseController::UpdateRate | update_rate_ |
Communicates with the high level hardware_interface::RobotHW and interacts with the robot hardware sensors (e.g. encoders) and actuators (e.g. motor driver).
The BaseController communicates with the high level interface diffbot_base::hardware_interface::RobotHW using ROS publishers and subscribers. In the main loop (see main.cpp) this class is operated at specific update rates update_rate_ for different sensors and acutators. There exist three update rates:
The control update rate is used to read from the encoders and write computed pwm signals to the motors at a certain rate. This is important to avoid running into syncronization errors because of too many running calculations. Similarly the imu update rate reads the latest inertial measurements at a specific rate. To keep track when a new update is needed, the last_update_time_ member is used, which stores the time stamps when the last update happened. Additionally the LastUpdateTime::command_received time stamp is used to check for connection losses to the high level hardware interface, which would then stop the motors (see eStop), if no command message was received for a specified amount of time.
The BaseController subscribes to the target wheel command angular velocities (diffbot_msgs::WheelsCmdStamped on the "wheel_cmd_velocities" topic) with the sub_wheel_cmd_velocities_ and keeps pointers to both motors in p_motor_controller_right_ and p_motor_controller_left_ using a generic motor driver agnostic interface diffbot::MotorControllerIntf. Each time a new diffbot_msgs::WheelsCmdStamped is received on the "wheel_cmd_velocities" topic the commandCallback is called and the two target velocities wheel_cmd_velocity_left_ and wheel_cmd_velocity_right_ for each wheel are updated.
To measure the angular wheel positions (absolut) and the angular velocities, two diffbot::Encoder objects (encoder_left_ encoder_right_) are used to read() the diffbot::JointStates, stored in joint_state_left_ and joint_state_left_. After reading the latest states (position and velocity), the values are published with pub_measured_joint_states_ on "measured_joint_states" topic of type sensor_msgs::JointState. The diffbot_base::hardware_interface::RobotHW subscribes to these joint states and passes them on to the diff_drive_controller. Also inside the read() method, the current encoder ticks read from the encoder_left_ and encoder_right_ and stored in ticks_left_ and ticks_right_.
The measured angular velocities are important to compute the pwm signals for each motor using two separate PID controllers (motor_pid_left_ and motor_pid_right_), which calculate the error between measured and commanded angular wheel velocity for each wheel joint.
For initialization the following parameters are read from the ROS parameter server.
| TMotorController | |
| TMotorDriver |
Definition at line 77 of file base_controller.h.
| diffbot::BaseController< TMotorController, TMotorDriver >::BaseController | ( | ros::NodeHandle & | nh, |
| TMotorController * | motor_controller_left, | ||
| TMotorController * | motor_controller_right | ||
| ) |
Construct a new Base Controller object using two generic motor controllers.
Requires two initialized generic motor controllers, which are defined in a parent scope (e.g. main.cpp) The two motor controllers for the left and right motor are kept generic. The only requirement is to implement the diffbot::MotorControllerIntf, which is composed of the motor driver and has therefore the ability to control one motor.
| nh | Reference to the global ROS node handle |
| motor_controller_left | Pointer to the generic motor controller for the left motor |
| motor_controller_right | Pointer to the generic motor controller for the right motor |
Definition at line 385 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::commandCallback | ( | const diffbot_msgs::WheelsCmdStamped & | cmd_msg | ) |
Callback method when a new diffbot_msgs::WheelsCmdStamped is received on the wheel_cmd_velocities topic.
Callback method every time the angular wheel commands for each wheel joint are received from 'wheel_cmd_velocities' topic. This topic is published from the high level hardware_interface::RobotHW::write() method.
This callback method receives diffbot_msgs::WheelsCmdStamped message object (cmd_msg) consisting of diffbot_msgs::AngularVelocities, where commanded wheel joints for the left and right wheel are stored. After receiving the message the wheel_cmd_velocity_left_ and wheel_cmd_velocity_right_ members are updated, which are used in the next write() of the control loop.
In this method the lastUpdateTime::command_received time stamp is set to the current time. This is used for the eStop functionality. In case no diffbot_msgs::WheelsCmdStamped messages are received on the wheel_cmd_velocities topic, the eStop method is called (see main loop in main.cpp)
| cmd_msg | Message containing the commanded wheel velocities. |
Definition at line 465 of file base_controller.h.
|
inline |
Getter if the firmware should log debug output.
Definition at line 211 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::eStop |
Stops the motors, in case no wheel commands are received over a longer time period.
Sets the wheel_cmd_velocity_left_ and wheel_cmd_velocity_right_ to zero. This method is called when the commandCallback wasn't called within the LastUpdateTime::command_received period on the /diffbot/wheel_cmd_velocities topic.
Definition at line 558 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::init |
Reads parameters from the parameter server.
Definition at line 435 of file base_controller.h.
|
inline |
Returns a reference to last_update_times_.
Used inside the main loop inside main.cpp to compare with the current time if an update of a specific function (command_received, control, imu, debug) is needed.
Definition at line 202 of file base_controller.h.
|
inline |
Calculates the period (s) from a given frequency (Hz).
| frequency | Input frequency (Hz) to be converted to period. |
Definition at line 154 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::pidLeftCallback | ( | const diffbot_msgs::PIDStamped & | pid_msg | ) |
Callback method to update the PID constants for the left motor.
Publish to pid_left topic to update the PID constants of the left motor.
| pid_msg | message containing the new PID constants |
Definition at line 490 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::pidRightCallback | ( | const diffbot_msgs::PIDStamped & | pid_msg | ) |
Callback method to update the PID constants for the right motor.
Publish to pid_right topic to update the PID constants of the right motor.
| pid_msg | message containing the new PID constants |
Definition at line 500 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::printDebug |
Definition at line 565 of file base_controller.h.
|
inline |
Definition at line 156 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::read |
Reads the current encoder tick counts and joint states (angular position (rad) and angular velocity (rad/s) from both encoders left encoder_left_ and right encoder_right_ and publishes sensor_msgs::JointState on the "measured_joint_states" topic, using the pub_measured_joint_states_.
Definition at line 511 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::resetEncodersCallback | ( | const std_msgs::Empty & | reset_msg | ) |
Callback method to reset both encoder's tick count to zero.
For initializing the the BaseController the encoder tick counts are set back to zero. Every time the diffbot_bringup/launch/bringup.launch is launched, the high level hardware_interface::RobotHW publishes an empty message on the /reset topic, which invokes this callback and sets the encoders to zero.
| reset_msg | empty message (unused) |
Definition at line 480 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::setup |
Initializes the main node handle and setup publisher and subscriber.
Waits until the connection to the ros master is established.
Definition at line 406 of file base_controller.h.
| void diffbot::BaseController< TMotorController, TMotorDriver >::write |
Uses the PIDs to compute capped PWM signals for the left and right motor.
The values for both motors sent to the motor driver are calculated by the two PIDs pid_motor_left_ and pid_motor_right_, based on the error between commanded angular velocity vs measured angular velocity (e.g. diffbot::PID::error_ = wheel_cmd_velocity_left_ - measured_angular_velocity_left_)
The calculated PID ouput values are capped at -/+ MAX_RPM to prevent the PID from having too much error The computed and capped PID values are then set using the diffbot::MotorControllerIntf::setSpeed method for the left and right motors p_motor_controller_left_ and p_motor_controller_right_.
Definition at line 536 of file base_controller.h.
|
private |
Definition at line 373 of file base_controller.h.
|
private |
Definition at line 338 of file base_controller.h.
|
private |
Definition at line 351 of file base_controller.h.
|
private |
Definition at line 345 of file base_controller.h.
|
private |
Definition at line 339 of file base_controller.h.
|
private |
Definition at line 343 of file base_controller.h.
|
private |
Definition at line 343 of file base_controller.h.
| struct diffbot::BaseController::LastUpdateTime diffbot::BaseController< TMotorController, TMotorDriver >::last_update_time_ |
|
private |
Definition at line 330 of file base_controller.h.
|
private |
Definition at line 329 of file base_controller.h.
|
private |
Definition at line 364 of file base_controller.h.
|
private |
Definition at line 365 of file base_controller.h.
|
private |
Definition at line 369 of file base_controller.h.
|
private |
Definition at line 370 of file base_controller.h.
|
private |
Definition at line 354 of file base_controller.h.
|
private |
Definition at line 325 of file base_controller.h.
|
private |
Definition at line 358 of file base_controller.h.
|
private |
Definition at line 357 of file base_controller.h.
|
private |
Definition at line 352 of file base_controller.h.
|
private |
Definition at line 355 of file base_controller.h.
|
private |
Definition at line 367 of file base_controller.h.
|
private |
Definition at line 368 of file base_controller.h.
|
private |
Definition at line 347 of file base_controller.h.
|
private |
Definition at line 360 of file base_controller.h.
|
private |
Definition at line 340 of file base_controller.h.
|
private |
Definition at line 340 of file base_controller.h.
| struct diffbot::BaseController::UpdateRate diffbot::BaseController< TMotorController, TMotorDriver >::update_rate_ |
|
private |
Definition at line 361 of file base_controller.h.
|
private |
Definition at line 362 of file base_controller.h.
|
private |
Definition at line 328 of file base_controller.h.