00001 00006 #ifndef YOCS_SAFETY_CONTROLLER_HPP_ 00007 #define YOCS_SAFETY_CONTROLLER_HPP_ 00008 00009 /***************************************************************************** 00010 ** Includes 00011 *****************************************************************************/ 00012 00013 #include <string> 00014 #include <geometry_msgs/Twist.h> 00015 #include <ros/ros.h> 00016 #include <std_msgs/Empty.h> 00017 #include <sensor_msgs/Range.h> 00018 #include <yocs_controllers/default_controller.hpp> 00019 00020 namespace yocs_safety_controller 00021 { 00022 00035 class SafetyController : public yocs::Controller 00036 { 00037 public: 00038 SafetyController(ros::NodeHandle& nh_priv, std::string& name); 00039 ~SafetyController(){}; 00040 00045 bool init(); 00046 00050 void spinOnce(); 00051 00052 private: 00053 ros::NodeHandle nh_priv_; 00054 std::string name_; 00055 ros::Subscriber enable_controller_subscriber_, disable_controller_subscriber_, ranger_subscriber_; 00056 ros::Publisher controller_state_publisher_, velocity_command_publisher_; 00057 geometry_msgs::TwistPtr cmd_vel_msg_; // velocity command 00058 00062 bool obstacle_detected_; 00066 bool reverse_; 00070 bool reversing_; 00074 ros::Time reversing_start_; 00078 double reversing_distance_; 00082 double reversing_velocity_; 00087 ros::Duration reversing_duration_; 00088 00093 void enableCB(const std_msgs::EmptyConstPtr msg); 00094 00099 void disableCB(const std_msgs::EmptyConstPtr msg); 00100 00105 void rangerCB(const sensor_msgs::RangeConstPtr msg); 00106 }; 00107 00108 } // namespace yocs_safety_controller 00109 00110 #endif // YOCS_SAFETY_CONTROLLER_HPP_