ROS-aware controller to manage a differential drive mobile base. This subcribes to cmd_vel topic, publishes odom and tf, and manages the two wheel joints. More...
#include <diff_drive_base.h>
Public Member Functions | |
void | command (const geometry_msgs::TwistConstPtr &msg) |
Command callback from either a ROS topic, or a higher controller. | |
DiffDriveBaseController () | |
virtual std::vector< std::string > | getClaimedNames () |
Get the names of joints/controllers which this controller exclusively claims. | |
virtual std::vector< std::string > | getCommandedNames () |
Get the names of joints/controllers which this controller commands. | |
virtual std::string | getType () |
Get the type of this controller. | |
virtual int | init (ros::NodeHandle &nh, ControllerManager *manager) |
Initialize the controller and any required data structures. | |
virtual bool | reset () |
Cleanly reset the controller to it's initial state. Some controllers may choose to stop themselves. This is mainly used in the case of the the robot exiting some fault condition. | |
virtual bool | start () |
Attempt to start the controller. This should be called only by the ControllerManager instance. | |
virtual bool | stop (bool force) |
Attempt to stop the controller. This should be called only by the ControllerManager instance. | |
virtual void | update (const ros::Time &now, const ros::Duration &dt) |
This is the update loop for the controller. | |
virtual | ~DiffDriveBaseController () |
Private Member Functions | |
void | publishCallback (const ros::TimerEvent &event) |
void | scanCallback (const sensor_msgs::LaserScanConstPtr &scan) |
void | setCommand (float left, float right) |
Private Attributes | |
boost::shared_ptr < tf::TransformBroadcaster > | broadcaster_ |
ros::Subscriber | cmd_sub_ |
boost::mutex | command_mutex_ |
double | desired_r_ |
double | desired_x_ |
bool | enabled_ |
bool | initialized_ |
ros::Time | last_command_ |
ros::Time | last_laser_scan_ |
float | last_sent_r_ |
float | last_sent_x_ |
ros::Time | last_update_ |
JointHandlePtr | left_ |
float | left_last_position_ |
double | left_last_timestamp_ |
ControllerManager * | manager_ |
double | max_acceleration_r_ |
double | max_acceleration_x_ |
double | max_velocity_r_ |
double | max_velocity_x_ |
Threshold for dx to be considered "moving". | |
double | moving_threshold_ |
Threshold for dr to be considered "moving". | |
nav_msgs::Odometry | odom_ |
boost::mutex | odom_mutex_ |
ros::Publisher | odom_pub_ |
ros::Timer | odom_timer_ |
bool | publish_tf_ |
double | radians_per_meter_ |
bool | ready_ |
JointHandlePtr | right_ |
float | right_last_position_ |
double | right_last_timestamp_ |
double | robot_width_ |
double | rotating_threshold_ |
Threshold for wheel velocity to be "moving". | |
double | safety_scaling_ |
double | safety_scaling_distance_ |
ros::Subscriber | scan_sub_ |
double | theta_ |
ros::Duration | timeout_ |
double | track_width_ |
double | wheel_rotating_threshold_ |
ROS-aware controller to manage a differential drive mobile base. This subcribes to cmd_vel topic, publishes odom and tf, and manages the two wheel joints.
Definition at line 63 of file diff_drive_base.h.
Definition at line 47 of file diff_drive_base.cpp.
virtual robot_controllers::DiffDriveBaseController::~DiffDriveBaseController | ( | ) | [inline, virtual] |
Definition at line 67 of file diff_drive_base.h.
void robot_controllers::DiffDriveBaseController::command | ( | const geometry_msgs::TwistConstPtr & | msg | ) |
Command callback from either a ROS topic, or a higher controller.
Definition at line 158 of file diff_drive_base.cpp.
std::vector< std::string > robot_controllers::DiffDriveBaseController::getClaimedNames | ( | ) | [virtual] |
Get the names of joints/controllers which this controller exclusively claims.
Implements robot_controllers::Controller.
Definition at line 348 of file diff_drive_base.cpp.
std::vector< std::string > robot_controllers::DiffDriveBaseController::getCommandedNames | ( | ) | [virtual] |
Get the names of joints/controllers which this controller commands.
Implements robot_controllers::Controller.
Definition at line 338 of file diff_drive_base.cpp.
virtual std::string robot_controllers::DiffDriveBaseController::getType | ( | ) | [inline, virtual] |
Get the type of this controller.
Reimplemented from robot_controllers::Controller.
Definition at line 110 of file diff_drive_base.h.
int robot_controllers::DiffDriveBaseController::init | ( | ros::NodeHandle & | nh, |
ControllerManager * | manager | ||
) | [virtual] |
Initialize the controller and any required data structures.
nh | Node handle for this controller. |
manager | The controller manager instance, this is needed for the controller to get information about joints, etc. |
Reimplemented from robot_controllers::Controller.
Definition at line 63 of file diff_drive_base.cpp.
void robot_controllers::DiffDriveBaseController::publishCallback | ( | const ros::TimerEvent & | event | ) | [private] |
Definition at line 354 of file diff_drive_base.cpp.
bool robot_controllers::DiffDriveBaseController::reset | ( | ) | [virtual] |
Cleanly reset the controller to it's initial state. Some controllers may choose to stop themselves. This is mainly used in the case of the the robot exiting some fault condition.
Implements robot_controllers::Controller.
Definition at line 210 of file diff_drive_base.cpp.
void robot_controllers::DiffDriveBaseController::scanCallback | ( | const sensor_msgs::LaserScanConstPtr & | scan | ) | [private] |
Definition at line 383 of file diff_drive_base.cpp.
void robot_controllers::DiffDriveBaseController::setCommand | ( | float | left, |
float | right | ||
) | [private] |
Definition at line 411 of file diff_drive_base.cpp.
bool robot_controllers::DiffDriveBaseController::start | ( | ) | [virtual] |
Attempt to start the controller. This should be called only by the ControllerManager instance.
Implements robot_controllers::Controller.
Definition at line 182 of file diff_drive_base.cpp.
bool robot_controllers::DiffDriveBaseController::stop | ( | bool | force | ) | [virtual] |
Attempt to stop the controller. This should be called only by the ControllerManager instance.
force | Should we force the controller to stop? Some controllers may wish to continue running until they absolutely have to stop. |
Implements robot_controllers::Controller.
Definition at line 193 of file diff_drive_base.cpp.
void robot_controllers::DiffDriveBaseController::update | ( | const ros::Time & | now, |
const ros::Duration & | dt | ||
) | [virtual] |
This is the update loop for the controller.
time | The system time. |
dt | The timestep since last call to update. |
Implements robot_controllers::Controller.
Definition at line 217 of file diff_drive_base.cpp.
boost::shared_ptr<tf::TransformBroadcaster> robot_controllers::DiffDriveBaseController::broadcaster_ [private] |
Definition at line 180 of file diff_drive_base.h.
Definition at line 178 of file diff_drive_base.h.
boost::mutex robot_controllers::DiffDriveBaseController::command_mutex_ [private] |
Definition at line 157 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::desired_r_ [private] |
Definition at line 159 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::desired_x_ [private] |
Definition at line 158 of file diff_drive_base.h.
bool robot_controllers::DiffDriveBaseController::enabled_ [private] |
Definition at line 183 of file diff_drive_base.h.
bool robot_controllers::DiffDriveBaseController::initialized_ [private] |
Definition at line 125 of file diff_drive_base.h.
Definition at line 170 of file diff_drive_base.h.
Definition at line 154 of file diff_drive_base.h.
float robot_controllers::DiffDriveBaseController::last_sent_r_ [private] |
Definition at line 163 of file diff_drive_base.h.
float robot_controllers::DiffDriveBaseController::last_sent_x_ [private] |
Definition at line 162 of file diff_drive_base.h.
Definition at line 171 of file diff_drive_base.h.
Definition at line 134 of file diff_drive_base.h.
float robot_controllers::DiffDriveBaseController::left_last_position_ [private] |
Definition at line 165 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::left_last_timestamp_ [private] |
Definition at line 167 of file diff_drive_base.h.
Definition at line 126 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::max_acceleration_r_ [private] |
Definition at line 148 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::max_acceleration_x_ [private] |
Definition at line 147 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::max_velocity_r_ [private] |
Definition at line 146 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::max_velocity_x_ [private] |
Threshold for dx to be considered "moving".
Definition at line 145 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::moving_threshold_ [private] |
Threshold for dr to be considered "moving".
Definition at line 143 of file diff_drive_base.h.
nav_msgs::Odometry robot_controllers::DiffDriveBaseController::odom_ [private] |
Definition at line 175 of file diff_drive_base.h.
boost::mutex robot_controllers::DiffDriveBaseController::odom_mutex_ [private] |
Definition at line 174 of file diff_drive_base.h.
Definition at line 176 of file diff_drive_base.h.
Definition at line 177 of file diff_drive_base.h.
bool robot_controllers::DiffDriveBaseController::publish_tf_ [private] |
Definition at line 181 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::radians_per_meter_ [private] |
Definition at line 138 of file diff_drive_base.h.
bool robot_controllers::DiffDriveBaseController::ready_ [private] |
Definition at line 184 of file diff_drive_base.h.
Definition at line 135 of file diff_drive_base.h.
float robot_controllers::DiffDriveBaseController::right_last_position_ [private] |
Definition at line 166 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::right_last_timestamp_ [private] |
Definition at line 168 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::robot_width_ [private] |
Definition at line 153 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::rotating_threshold_ [private] |
Threshold for wheel velocity to be "moving".
Definition at line 142 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::safety_scaling_ [private] |
Definition at line 151 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::safety_scaling_distance_ [private] |
Definition at line 152 of file diff_drive_base.h.
Definition at line 178 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::theta_ [private] |
Definition at line 139 of file diff_drive_base.h.
Definition at line 172 of file diff_drive_base.h.
double robot_controllers::DiffDriveBaseController::track_width_ [private] |
Definition at line 137 of file diff_drive_base.h.
Definition at line 141 of file diff_drive_base.h.