#include <safety_controller.hpp>
Public Member Functions | |
bool | init () |
SafetyController (ros::NodeHandle &nh_priv, const std::string &name) | |
void | spinOnce () |
~SafetyController () | |
Public Member Functions inherited from yocs::Controller | |
Controller () | |
bool | disable () |
bool | enable () |
bool | getState () |
virtual void | spin () |
virtual | ~Controller () |
Protected Member Functions | |
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 | rangerCB (const sensor_msgs::RangeConstPtr msg) |
Keeps track of rangers. More... | |
Protected Attributes | |
geometry_msgs::TwistPtr | cmd_vel_msg_ |
ros::Publisher | controller_state_publisher_ |
ros::Subscriber | disable_controller_subscriber_ |
ros::Subscriber | enable_controller_subscriber_ |
std::string | name_ |
ros::NodeHandle | nh_priv_ |
bool | obstacle_detected_ |
ros::Subscriber | ranger_subscriber_ |
bool | reverse_ |
bool | reversing_ |
double | reversing_distance_ |
ros::Duration | reversing_duration_ |
ros::Time | reversing_start_ |
double | reversing_velocity_ |
ros::Publisher | velocity_command_publisher_ |
@ brief Keeps track of ranger readings to stop your robot, if necessary
The SafetyController keeps track of ranger readings. If obstacles get too close, your robot is stopped. You can also activate the move backward option, if you like your robot to reverse a bit after stopping.
Note: Currently only supports fixed-distance rangers.
TODO: Implement logic to handle variable-distance rangers.
This controller can be enabled/disabled.
Definition at line 35 of file safety_controller.hpp.
yocs_safety_controller::SafetyController::SafetyController | ( | ros::NodeHandle & | nh_priv, |
const std::string & | name | ||
) |
Definition at line 12 of file library.cpp.
|
inline |
Definition at line 39 of file safety_controller.hpp.
|
protected |
ROS logging output for disabling the controller.
msg | incoming topic message |
Definition at line 69 of file library.cpp.
|
protected |
ROS logging output for enabling the controller.
msg | incoming topic message |
Definition at line 57 of file library.cpp.
|
virtual |
Set-up necessary publishers/subscribers and variables
Implements yocs::Controller.
Definition at line 26 of file library.cpp.
|
protected |
Keeps track of rangers.
msg | incoming topic message |
Definition at line 81 of file library.cpp.
void yocs_safety_controller::SafetyController::spinOnce | ( | ) |
@ brief Checks safety states and publishes velocity commands when necessary
Definition at line 94 of file library.cpp.
|
protected |
Definition at line 58 of file safety_controller.hpp.
|
protected |
Definition at line 57 of file safety_controller.hpp.
|
protected |
Definition at line 56 of file safety_controller.hpp.
|
protected |
Definition at line 56 of file safety_controller.hpp.
|
protected |
Definition at line 55 of file safety_controller.hpp.
|
protected |
Definition at line 54 of file safety_controller.hpp.
|
protected |
Indicates whether an obstacle has been detected
Definition at line 63 of file safety_controller.hpp.
|
protected |
Definition at line 56 of file safety_controller.hpp.
|
protected |
Indicates whether to reverse after an obstacle has been detected
Definition at line 67 of file safety_controller.hpp.
|
protected |
Reversing state
Definition at line 71 of file safety_controller.hpp.
|
protected |
the total distance the robot will reverse in [m](configurable)
Definition at line 79 of file safety_controller.hpp.
|
protected |
max. duration the robot will reverse in [s] automatically calculated based on reversing_distance_ and reversing_speed_
Definition at line 88 of file safety_controller.hpp.
|
protected |
the time robot started reversing
Definition at line 75 of file safety_controller.hpp.
|
protected |
how fast the robot should move, when reversing in [m/s] (configurable)
Definition at line 83 of file safety_controller.hpp.
|
protected |
Definition at line 57 of file safety_controller.hpp.