#include <safety_controller.hpp>
Public Member Functions | |
bool | init () |
SafetyController (ros::NodeHandle &nh_priv, std::string &name) | |
void | spinOnce () |
~SafetyController () | |
Private Member Functions | |
void | disableCB (const std_msgs::EmptyConstPtr msg) |
ROS logging output for disabling the controller. | |
void | enableCB (const std_msgs::EmptyConstPtr msg) |
ROS logging output for enabling the controller. | |
void | rangerCB (const sensor_msgs::RangeConstPtr msg) |
Keeps track of rangers. | |
Private 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, |
std::string & | name | ||
) |
Definition at line 12 of file library.cpp.
Definition at line 39 of file safety_controller.hpp.
void yocs_safety_controller::SafetyController::disableCB | ( | const std_msgs::EmptyConstPtr | msg | ) | [private] |
ROS logging output for disabling the controller.
msg | incoming topic message |
Definition at line 69 of file library.cpp.
void yocs_safety_controller::SafetyController::enableCB | ( | const std_msgs::EmptyConstPtr | msg | ) | [private] |
ROS logging output for enabling the controller.
msg | incoming topic message |
Definition at line 57 of file library.cpp.
bool yocs_safety_controller::SafetyController::init | ( | ) | [virtual] |
Set-up necessary publishers/subscribers and variables
Implements yocs::Controller.
Definition at line 26 of file library.cpp.
void yocs_safety_controller::SafetyController::rangerCB | ( | const sensor_msgs::RangeConstPtr | msg | ) | [private] |
Keeps track of rangers.
msg | incoming topic message |
Definition at line 81 of file library.cpp.
@ brief Checks safety states and publishes velocity commands when necessary
Definition at line 94 of file library.cpp.
geometry_msgs::TwistPtr yocs_safety_controller::SafetyController::cmd_vel_msg_ [private] |
Definition at line 57 of file safety_controller.hpp.
Definition at line 56 of file safety_controller.hpp.
Definition at line 55 of file safety_controller.hpp.
Definition at line 55 of file safety_controller.hpp.
std::string yocs_safety_controller::SafetyController::name_ [private] |
Definition at line 54 of file safety_controller.hpp.
Definition at line 53 of file safety_controller.hpp.
bool yocs_safety_controller::SafetyController::obstacle_detected_ [private] |
Indicates whether an obstacle has been detected
Definition at line 62 of file safety_controller.hpp.
Definition at line 55 of file safety_controller.hpp.
bool yocs_safety_controller::SafetyController::reverse_ [private] |
Indicates whether to reverse after an obstacle has been detected
Definition at line 66 of file safety_controller.hpp.
bool yocs_safety_controller::SafetyController::reversing_ [private] |
Reversing state
Definition at line 70 of file safety_controller.hpp.
double yocs_safety_controller::SafetyController::reversing_distance_ [private] |
the total distance the robot will reverse in [m](configurable)
Definition at line 78 of file safety_controller.hpp.
max. duration the robot will reverse in [s] automatically calculated based on reversing_distance_ and reversing_speed_
Definition at line 87 of file safety_controller.hpp.
the time robot started reversing
Definition at line 74 of file safety_controller.hpp.
double yocs_safety_controller::SafetyController::reversing_velocity_ [private] |
how fast the robot should move, when reversing in [m/s] (configurable)
Definition at line 82 of file safety_controller.hpp.
Definition at line 56 of file safety_controller.hpp.