00001 00013 #ifndef NAV_SAFETY_H_ 00014 #define NAV_SAFETY_H_ 00015 00016 #include <ros/ros.h> 00017 #include <actionlib/client/simple_action_client.h> 00018 #include <actionlib/server/simple_action_server.h> 00019 #include <carl_safety/Error.h> 00020 #include <geometry_msgs/Pose.h> 00021 #include <geometry_msgs/Twist.h> 00022 #include <move_base_msgs/MoveBaseAction.h> 00023 #include <sensor_msgs/JointState.h> 00024 #include <sensor_msgs/Joy.h> 00025 #include <std_srvs/Empty.h> 00026 #include <wpi_jaco_msgs/GetAngularPosition.h> 00027 #include <wpi_jaco_msgs/GetCartesianPosition.h> 00028 #include <wpi_jaco_msgs/HomeArmAction.h> 00029 00030 //controller types 00031 #define ANALOG 0 //analog triggers 00032 #define DIGITAL 1 //digital triggers 00033 00034 //Boundary 00035 #define BOUNDARY_X 4.4 00036 #define BOUNDARY_Y 1.0 00037 #define PI 3.14159 00038 00047 class NavSafety 00048 { 00049 public: 00053 NavSafety(); 00054 00058 void cancelNavGoals(); 00059 00060 private: 00065 void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); 00066 00071 void safeBaseCommandCallback(const geometry_msgs::Twist::ConstPtr& msg); 00072 00077 void poseCallback(const geometry_msgs::Pose::ConstPtr& msg); 00078 00086 void safeMoveCallback(const move_base_msgs::MoveBaseGoalConstPtr &goal); 00087 00095 bool navStopCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); 00096 00097 bool isArmRetracted(); 00098 00099 bool isArmContained(); 00100 00101 void publishArmNotContainedError(); 00102 00103 void publishStoppedError(); 00104 00105 void publishClearError(); 00106 00107 ros::NodeHandle node; 00109 ros::Publisher baseCommandPublisher; 00110 ros::Publisher safetyErrorPublisher; 00111 ros::Subscriber safeBaseCommandSubscriber; 00112 ros::Subscriber joySubscriber; 00113 ros::Subscriber robotPoseSubscriber; 00115 ros::ServiceClient jacoPosClient; 00116 ros::ServiceClient jacoCartesianClient; 00117 ros::ServiceServer stopBaseNavServer; 00118 00119 ros::Time baseFeedbackLastPublished; 00120 00121 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> acMoveBase; 00122 actionlib::SimpleActionClient<wpi_jaco_msgs::HomeArmAction> acHome; 00123 00124 actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> asSafeMove; 00125 00126 int controllerType; 00127 bool stopped; 00128 float x; 00129 float y; 00130 float theta; 00131 std::vector<float> retractPos; //jaco arm retracted joint positions 00132 00133 bool use_teleop_safety; 00134 }; 00135 00136 #endif