45 #ifndef SAFETY_CONTROLLER_HPP_ 46 #define SAFETY_CONTROLLER_HPP_ 55 #include <std_msgs/Empty.h> 56 #include <geometry_msgs/Twist.h> 57 #include <xbot_msgs/InfraRed.h> 58 #include <xbot_msgs/Echo.h> 85 msg_(new geometry_msgs::Twist()){};
95 double time_to_extend_bump_cliff_events;
96 nh_.
param(
"time_to_extend_bump_cliff_events", time_to_extend_bump_cliff_events, 0.0);
130 void enableCB(
const std_msgs::EmptyConstPtr msg);
136 void disableCB(
const std_msgs::EmptyConstPtr msg);
148 void cliffEventCB(
const xbot_msgs::InfraRedConstPtr msg);
189 if (msg->front_hanged)
198 if (msg->rear_hanged)
252 msg_.reset(
new geometry_msgs::Twist());
253 msg_->linear.x = -0.1;
254 msg_->linear.y = 0.0;
255 msg_->linear.z = 0.0;
256 msg_->angular.x = 0.0;
257 msg_->angular.y = 0.0;
258 msg_->angular.z = 0.0;
263 msg_.reset(
new geometry_msgs::Twist());
264 msg_->linear.x = 0.1;
265 msg_->linear.y = 0.0;
266 msg_->linear.z = 0.0;
267 msg_->angular.x = 0.0;
268 msg_->angular.y = 0.0;
ros::Subscriber bumper_event_subscriber_
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void enableCB(const std_msgs::EmptyConstPtr msg)
ROS logging output for enabling the controller.
bool cliff_rear_detected_
ros::Subscriber enable_controller_subscriber_
SafetyController(ros::NodeHandle &nh, std::string &name)
ros::Duration time_to_extend_bump_cliff_events_
void disableCB(const std_msgs::EmptyConstPtr msg)
ROS logging output for disabling the controller.
ros::Publisher controller_state_publisher_
void cliffEventCB(const xbot_msgs::InfraRedConstPtr msg)
Keeps track of cliff detection.
ros::Subscriber reset_safety_states_subscriber_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void bumperEventCB(const xbot_msgs::EchoConstPtr msg)
Keeps track of bumps.
geometry_msgs::TwistPtr msg_
ros::Publisher velocity_command_publisher_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
void resetSafetyStatesCB(const std_msgs::EmptyConstPtr msg)
Callback for resetting safety variables.
bool bumper_rear_pressed_
#define ROS_INFO_STREAM(args)
bool cliff_front_detected_
ros::Subscriber cliff_event_subscriber_
ros::Time last_event_time_
ros::Subscriber disable_controller_subscriber_
bool bumper_front_pressed_