00001 00008 #include <geometry_msgs/Twist.h> 00009 #include <ros/ros.h> 00010 #include <ctime> 00011 00012 #include <youbot_overhead_localization/CostmapCost.h> 00013 #include <youbot_overhead_localization/Pose2d.h> 00014 #include <youbot_overhead_localization/GetCost.h> 00015 00016 #define PI 3.14159 00017 00026 class keyNavSafety 00027 { 00028 public: 00029 //ROS subscribers, publishers, and services 00030 ros::NodeHandle n; 00031 ros::Publisher baseCommandPublisher; 00032 ros::Subscriber baseCommandSubscriber; 00033 ros::Subscriber costSubscriber; 00034 ros::Subscriber localizationSubscriber; 00035 ros::ServiceClient costClient; 00036 00037 //the current cost 00038 youbot_overhead_localization::CostmapCost currentCost; 00039 00040 //the current move command 00041 geometry_msgs::Twist moveCommand; 00042 00043 //localization 00044 youbot_overhead_localization::Pose2d robotPose; 00045 00046 bool moveInitialized; 00047 bool localizationInitialized; 00048 00049 //Variables used to stop keyNavSafety from pumping stop commands when not receiving new move commands. This is necessary because if the path planner is causing movement, the robot is moving while the keyNavSafety is pumping out stop commands, causing a jerky behavior 00050 bool stopCommandFound; 00051 time_t lastStopCmdTime; 00052 int timeBetweenStopCmds; 00053 00058 void costCallback(const youbot_overhead_localization::CostmapCost& newCost); 00059 00065 void baseCommandCallback(const geometry_msgs::Twist& requestedCommand); 00066 00071 void localizationCallback(const youbot_overhead_localization::Pose2d& pose); 00072 00076 keyNavSafety(); 00077 };