00001 00013 #ifndef TELEOP_SAFETY_H_ 00014 #define TELEOP_SAFETY_H_ 00015 00016 #include <ros/ros.h> 00017 #include <sensor_msgs/LaserScan.h> 00018 #include <std_msgs/Float64.h> 00019 #include <geometry_msgs/PointStamped.h> 00020 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00021 #include <tf/transform_listener.h> 00022 #include <nav_msgs/OccupancyGrid.h> 00023 #include <math.h> 00024 #include <unistd.h> 00025 00031 #define START_FORWARD_SAFETY_THROTTLE_DIST 1.25 00032 00038 #define MIN_FORWARD_SAFE_DIST 0.55 00039 00045 #define START_REVERSE_SAFETY_THROTTLE_DIST 1.4 00046 00052 #define MIN_REVERSE_SAFE_DIST 0.65 00053 00054 00062 class teleop_safety 00063 { 00064 public: 00068 teleop_safety(); 00069 00073 ~teleop_safety(); 00074 00075 private: 00081 void scan_cback(const sensor_msgs::LaserScan::ConstPtr& scan); 00082 00088 void map_cback(const nav_msgs::OccupancyGrid::ConstPtr& map); 00089 00095 void amcl_pose_cback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose); 00096 00097 00098 00099 void cmd_vel_safety_check_cback(const geometry_msgs::Twist::ConstPtr& cmd); 00100 00101 ros::NodeHandle node; 00102 tf::TransformListener* pListener; 00103 00104 ros::Publisher cmd_vel_pub; 00105 ros::Subscriber cmd_vel_safety_check; 00106 ros::Subscriber scan_sub; 00107 ros::Subscriber map_sub; 00108 ros::Subscriber amcl_pose_sub; 00110 geometry_msgs::Twist twist; 00111 nav_msgs::OccupancyGrid savedMap; 00113 double forward_throttle_safety_factor_base; 00114 double reverse_throttle_safety_factor_base; 00115 }; 00116 00124 int main(int argc, char **argv); 00125 00126 #endif