00001 00012 #ifndef TIPPING_SAFETY_H_ 00013 #define TIPPING_SAFETY_H_ 00014 00015 #include <ros/ros.h> 00016 #include <carl_safety/Error.h> 00017 #include <sensor_msgs/JointState.h> 00018 #include <std_srvs/Empty.h> 00019 #include <wpi_jaco_msgs/EStop.h> 00020 00021 //Tipping thresholds 00022 #define BASE_PITCH_THRESHOLD 0.08 00023 #define BASE_ROLL_THRESHOLD 0.035 00024 00025 class TippingSafety 00026 { 00027 public: 00031 TippingSafety(); 00032 00033 private: 00038 void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg); 00039 00040 ros::NodeHandle node; 00042 ros::Publisher safetyErrorPublisher; 00043 ros::Subscriber jointStateSubscriber; 00045 ros::ServiceClient jacoEStopClient; 00046 ros::ServiceClient safeNavStopClient; 00047 00048 bool enableAudibleWarnings; 00049 }; 00050 00051 #endif