#include <safety_controller.hpp>
Public Member Functions | |
bool | init () |
SafetyController (ros::NodeHandle &nh, std::string &name) | |
void | spin () |
~SafetyController () | |
Public Member Functions inherited from yocs::Controller | |
Controller () | |
bool | disable () |
bool | enable () |
bool | getState () |
virtual | ~Controller () |
Private Member Functions | |
void | bumperEventCB (const xbot_msgs::EchoConstPtr msg) |
Keeps track of bumps. More... | |
void | cliffEventCB (const xbot_msgs::InfraRedConstPtr msg) |
Keeps track of cliff detection. More... | |
void | disableCB (const std_msgs::EmptyConstPtr msg) |
ROS logging output for disabling the controller. More... | |
void | enableCB (const std_msgs::EmptyConstPtr msg) |
ROS logging output for enabling the controller. More... | |
void | resetSafetyStatesCB (const std_msgs::EmptyConstPtr msg) |
Callback for resetting safety variables. More... | |
@ brief Keeps track of safety-related events and commands Xbot to move accordingly
The SafetyController keeps track of bumper, cliff and wheel drop events. In case of the first two, Xbot is commanded to move back. In the latter case, Xbot is stopped. All commands stop when the event condition disappears. In the case of lateral bump/cliff, robot also spins a bit, what makes easier to escape from the risk.
This controller can be enabled/disabled. The safety states (bumper pressed etc.) can be reset. WARNING: Dangerous!
Definition at line 73 of file safety_controller.hpp.
|
inline |
Definition at line 76 of file safety_controller.hpp.
|
inline |
Definition at line 86 of file safety_controller.hpp.
|
private |
Keeps track of bumps.
msg | incoming topic message |
Definition at line 211 of file safety_controller.hpp.
|
private |
Keeps track of cliff detection.
msg | incoming topic message |
Definition at line 187 of file safety_controller.hpp.
|
private |
ROS logging output for disabling the controller.
msg | incoming topic message |
Definition at line 175 of file safety_controller.hpp.
|
private |
ROS logging output for enabling the controller.
msg | incoming topic message |
Definition at line 163 of file safety_controller.hpp.
|
inlinevirtual |
Set-up necessary publishers/subscribers and variables
Implements yocs::Controller.
Definition at line 92 of file safety_controller.hpp.
|
private |
Callback for resetting safety variables.
Allows resetting bumper, cliff and wheel drop states. DANGEROUS!
msg | incoming topic message |
Definition at line 237 of file safety_controller.hpp.
|
virtual |
@ brief Checks safety states and publishes velocity commands when necessary
Reimplemented from yocs::Controller.
Definition at line 246 of file safety_controller.hpp.
|
private |
Definition at line 116 of file safety_controller.hpp.
|
private |
Definition at line 119 of file safety_controller.hpp.
|
private |
Definition at line 119 of file safety_controller.hpp.
|
private |
Definition at line 116 of file safety_controller.hpp.
|
private |
Definition at line 120 of file safety_controller.hpp.
|
private |
Definition at line 120 of file safety_controller.hpp.
|
private |
Definition at line 118 of file safety_controller.hpp.
|
private |
Definition at line 115 of file safety_controller.hpp.
|
private |
Definition at line 115 of file safety_controller.hpp.
|
private |
Definition at line 122 of file safety_controller.hpp.
|
private |
Definition at line 124 of file safety_controller.hpp.
|
private |
Definition at line 114 of file safety_controller.hpp.
|
private |
Definition at line 113 of file safety_controller.hpp.
|
private |
Definition at line 117 of file safety_controller.hpp.
|
private |
Definition at line 121 of file safety_controller.hpp.
|
private |
Definition at line 118 of file safety_controller.hpp.