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 #include <boost/asio.hpp>
00038 #include <labust/tools/GeoUtilities.hpp>
00039 #include <labust/tools/StringUtilities.hpp>
00040 #include <ros/ros.h>
00041 #include <auv_msgs/NavSts.h>
00042 #include <geometry_msgs/PointStamped.h>
00043 #include <tf/transform_listener.h>
00044
00045 #include <string>
00046
00047 struct SharedData
00048 {
00049 SharedData():port(io){};
00050 boost::asio::io_service io;
00051 boost::asio::serial_port port;
00052 ros::Publisher trackPoint;
00053 tf::TransformListener listener;
00054 uint8_t buffer[10000];
00055 };
00056
00057 uint8_t aisConvertByte(uint8_t byte)
00058 {
00059 if (byte > 40)
00060 {
00061 byte += 56;
00062 }
00063 else
00064 {
00065 byte += 48;
00066 }
00067
00068 return byte;
00069 }
00070
00071 void aivdm2(long lat, long lon, int hdg, std::string& ais, bool isTarget = false)
00072 {
00073 if (isTarget)
00074 {
00075 ais = "!AIVDM,1,1,,A,13u?etP00000000000000000069D,0* \n";
00076 }
00077 else
00078 {
00079 ais = "!AIVDM,1,1,,A,13u?ftP00000000000000000069D,0* \n";
00080 }
00081 ROS_INFO("Encoding AIS information: lat=%d, lon=%d hdg=%d",lat,lon,hdg);
00082 unsigned char chk;
00083 int i;
00084 ais[24] = 0x0 | ((lon >> 23) & 0x1F);
00085 ais[25] = ((lon >> 17) & 0x3F);
00086 ais[26] = ((lon >> 11) & 0x3F);
00087 ais[27] = ((lon >> 5) & 0x3F);
00088 ais[28] = ((lon & 0x1F) << 1) | ((lat >> 26) & 0x01);
00089 ais[29] = ((lat >> 20) & 0x3F);
00090 ais[30] = ((lat >> 14) & 0x3F);
00091 ais[31] = ((lat >> 8) & 0x3F);
00092 ais[32] = ((lat >> 2) & 0x3F);
00093 ais[33] = ((lat & 0x3) << 4) | 0x0;
00094 ais[34] = 0;
00095 ais[35] = ((hdg >> 5) & 0xF);
00096 ais[36] = ((hdg & 0x1F) << 1);
00097 ais[37] = 0;
00098 for(int i=24; i<=37; ++i) ais[i] = aisConvertByte(uint8_t(ais[i]));
00099 chk = labust::tools::getChecksum(reinterpret_cast<const uint8_t*>(ais.data()), 44);
00100 if((chk >> 4) < 10)
00101 {
00102 ais[45] = (chk >> 4) + 48;
00103 }
00104 else
00105 {
00106 ais[45] = (chk >> 4) + 55;
00107 }
00108
00109 if((chk & 0xF) < 10)
00110 {
00111 ais[46] = (chk & 0xF) + 48;
00112 }
00113 else
00114 {
00115 ais[46] = (chk & 0xF) + 55;
00116 }
00117
00118
00119
00120
00121 }
00122
00123 void aivdm(long lat, long lon, int hdg) {
00124 unsigned char ais[47] = {'!', 'A', 'I', 'V', 'D', 'M', ',', '1', ',', '1', ',', ',', 'A', ',', '1', '3', 'u', '?', 'e', 't', 'P', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '6', '9', 'D', ',', '0', '*', ' ', ' '};
00125 unsigned char chk;
00126 int i;
00127 ais[24] = 0x0 | ((lon >> 23) & 0x1F);
00128 ais[25] = ((lon >> 17) & 0x3F);
00129 ais[26] = ((lon >> 11) & 0x3F);
00130 ais[27] = ((lon >> 5) & 0x3F);
00131 ais[28] = ((lon & 0x1F) << 1) | ((lat >> 26) & 0x01);
00132 ais[29] = ((lat >> 20) & 0x3F);
00133 ais[30] = ((lat >> 14) & 0x3F);
00134 ais[31] = ((lat >> 8) & 0x3F);
00135 ais[32] = ((lat >> 2) & 0x3F);
00136 ais[33] = ((lat & 0x3) << 4) | 0x0;
00137 ais[34] = 0;
00138 ais[35] = ((hdg >> 5) & 0xF);
00139 ais[36] = ((hdg & 0x1F) << 1);
00140 ais[37] = 0;
00141 if (ais[24] > 40)
00142 ais[24] += 56;
00143 else
00144 ais[24] += 48;
00145 if (ais[25] > 40)
00146 ais[25] += 56;
00147 else
00148 ais[25] += 48;
00149 if (ais[26] > 40)
00150 ais[26] += 56;
00151 else
00152 ais[26] += 48;
00153 if (ais[27] > 40)
00154 ais[27] += 56;
00155 else
00156 ais[27] += 48;
00157 if (ais[28] > 40)
00158 ais[28] += 56;
00159 else
00160 ais[28] += 48;
00161 if (ais[29] > 40)
00162 ais[29] += 56;
00163 else
00164 ais[29] += 48;
00165 if (ais[30] > 40)
00166 ais[30] += 56;
00167 else
00168 ais[30] += 48;
00169 if (ais[31] > 40)
00170 ais[31] += 56;
00171 else
00172 ais[31] += 48;
00173 if (ais[32] > 40)
00174 ais[32] += 56;
00175 else
00176 ais[32] += 48;
00177 if (ais[33] > 40)
00178 ais[33] += 56;
00179 else
00180 ais[33] += 48;
00181 if (ais[34] > 40)
00182 ais[34] += 56;
00183 else
00184 ais[34] += 48;
00185 if (ais[35] > 40)
00186 ais[35] += 56;
00187 else
00188 ais[35] += 48;
00189 if (ais[36] > 40)
00190 ais[36] += 56;
00191 else
00192 ais[36] += 48;
00193 if (ais[37] > 40)
00194 ais[37] += 56;
00195 else
00196 ais[37] += 48;
00197 chk = labust::tools::getChecksum(ais, 44);
00198 if((chk >> 4) < 10)
00199 ais[45] = (chk >> 4) + 48;
00200 else
00201 ais[45] = (chk >> 4) + 55;
00202 if((chk & 0xF) < 10)
00203 ais[46] = (chk & 0xF) + 48;
00204 else
00205 ais[46] = (chk & 0xF) + 55;
00206 for (i=0; i<47; i++) {
00207 printf("%c", ais[i]);
00208 }
00209 printf("\n");
00210 }
00211
00212 void onData(SharedData& shared, bool isTarget, const auv_msgs::NavSts::ConstPtr data)
00213 {
00214 std::string ais_data;
00215 int hdg = int(data->orientation.yaw*180/M_PI);
00216 if (hdg < 0) hdg +=360;
00217 aivdm2(long(data->global_position.latitude*600000),
00218 long(data->global_position.longitude*600000),
00219 hdg, ais_data, isTarget);
00220
00221 ROS_INFO("Encoded ais: %s",ais_data.c_str());
00222
00223 boost::asio::write(shared.port,boost::asio::buffer(ais_data));
00224
00225 try
00226 {
00227 tf::StampedTransform transformLocal;
00228 shared.listener.lookupTransform("worldLatLon", "local", ros::Time(0), transformLocal);
00229 std::pair<double, double> location = labust::tools::deg2meter(
00230 data->global_position.latitude - transformLocal.getOrigin().y(),
00231 data->global_position.longitude - transformLocal.getOrigin().x(),
00232 transformLocal.getOrigin().y());
00233
00234 geometry_msgs::PointStamped point;
00235 point.header.frame_id = "local";
00236 point.point.x = location.first;
00237 point.point.y = location.second;
00238 point.point.z = 0;
00239 shared.trackPoint.publish(point);
00240 }
00241 catch(tf::TransformException& ex)
00242 {
00243 ROS_ERROR("%s",ex.what());
00244 }
00245 }
00246
00247 int main(int argc, char* argv[])
00248 {
00249 ros::init(argc,argv,"repost_node");
00250 ros::NodeHandle nh,ph("~");
00251
00252 SharedData shared;
00253 ros::Subscriber cartPos = nh.subscribe<auv_msgs::NavSts>("cartPos",1,boost::bind(&onData,boost::ref(shared),false,_1));
00254 ros::Subscriber bartPos = nh.subscribe<auv_msgs::NavSts>("bartPos",1,boost::bind(&onData,boost::ref(shared),true,_1));
00255 shared.trackPoint = nh.advertise<geometry_msgs::PointStamped>("target_point",1);
00256
00257 using namespace boost::asio::ip;
00258
00259 std::string portName("/dev/ttyS0");
00260 int baud(38400);
00261 ph.param("PortName", portName,portName);
00262 ph.param("BaudRate", baud,baud);
00263 shared.port.open(portName);
00264 shared.port.set_option(boost::asio::serial_port::baud_rate(baud));
00265 if (!shared.port.is_open())
00266 {
00267 ROS_ERROR("Cannot open port.");
00268 throw std::runtime_error("Unable to open the port.");
00269 }
00270
00271 ros::spin();
00272 shared.io.stop();
00273 return 0;
00274 }