Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef HLMANAGER_HPP_
00038 #define HLMANAGER_HPP_
00039 #include <navcon_msgs/SetHLMode.h>
00040 #include <navcon_msgs/HLMessage.h>
00041
00042 #include <std_msgs/Bool.h>
00043 #include <geometry_msgs/PointStamped.h>
00044 #include <auv_msgs/NavSts.h>
00045 #include <sensor_msgs/NavSatFix.h>
00046 #include <sensor_msgs/Imu.h>
00047 #include <tf/transform_broadcaster.h>
00048
00049 #include <ros/ros.h>
00050 #include <boost/thread/mutex.hpp>
00051
00052 #include <map>
00053 #include <string>
00054
00055 namespace labust
00056 {
00057 namespace control
00058 {
00068 class HLManager
00069 {
00070 enum {bArt=0, cArt};
00071 enum {stop=0, manual,
00072 gotoPoint, stationKeeping, circle, heading, headingSurge, vtManual, lastMode};
00073
00074 typedef std::map<std::string,bool> ControllerMap;
00075
00076 public:
00080 HLManager();
00084 void onInit();
00088 void start();
00089
00090 private:
00094 void onVehicleEstimates(const auv_msgs::NavSts::ConstPtr& estimate);
00098 bool setHLMode(navcon_msgs::SetHLMode::Request& req, navcon_msgs::SetHLMode::Response& resp);
00102 void onLaunch(const std_msgs::Bool::ConstPtr& isLaunched);
00106 void onGPSData(const sensor_msgs::NavSatFix::ConstPtr& fix);
00110 void onVTTwist(const geometry_msgs::TwistStamped::ConstPtr& twist);
00114 void onImuMeas(const sensor_msgs::Imu::ConstPtr& imu);
00118 void safetyTest();
00122 void bArtStep();
00126 void step();
00130 void calculateBArtPoint(double heading);
00134 void disableControllerMap();
00138 bool fullStop();
00142 bool configureControllers();
00143
00147 ros::NodeHandle nh,ph;
00151 ros::Time lastEst, launchTime, lastFix;
00155 bool launchDetected;
00159 double timeout;
00163 int32_t mode, type;
00167 geometry_msgs::PointStamped point;
00171 auv_msgs::NavSts stateHat, trackPoint;
00175 sensor_msgs::NavSatFix fix;
00179 bool fixValidated;
00180
00184 double safetyRadius, safetyDistance, safetyTime, safetyTime2, gyroYaw;
00188 double circleRadius, turnDir, s;
00192 tf::TransformBroadcaster broadcaster;
00196 double originLat, originLon;
00197
00201 ros::Publisher refPoint, refTrack, openLoopSurge, curMode, refHeading,
00202 hlMessagePub, sfPub;
00206 ros::Subscriber state, launch, gpsData, virtualTargetTwist, imuMeas;
00210 ros::ServiceServer modeServer;
00214 ControllerMap controllers;
00218 navcon_msgs::HLMessage hlDiagnostics;
00219 };
00220 }
00221 }
00222
00223 #endif