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 <labust/control/CNRRemoteRadio.hpp>
00038 #include <labust/math/NumberManipulation.hpp>
00039 #include <labust/tools/GeoUtilities.hpp>
00040 #include <labust/tools/MatrixLoader.hpp>
00041 #include <labust/tools/conversions.hpp>
00042 #include <navcon_msgs/SetHLMode.h>
00043 #include <navcon_msgs/HLMessage.h>
00044 #include <labust/control/crc16.h>
00045
00046 #include <boost/archive/binary_oarchive.hpp>
00047 #include <boost/archive/binary_iarchive.hpp>
00048 #include <boost/bind.hpp>
00049 #include <boost/regex.hpp>
00050 #include <boost/crc.hpp>
00051
00052 #include <std_msgs/Bool.h>
00053
00054 #include <stdexcept>
00055 #include <bitset>
00056
00057 using labust::control::CNRRemoteRadio;
00058
00059 CNRRemoteRadio::CNRRemoteRadio():
00060 ph("~"),
00061 lastModemMsg(ros::Time::now()),
00062 timeout(10),
00063 currYaw(0),
00064 desiredHeading(0),
00065 buoyDistance(10),
00066 currLat(0),
00067 currLon(0),
00068 yawInc(0.5),
00069 port(io),
00070 buffer(sync_length,0),
00071 id(bart),
00072 doDummyRequest(false),
00073 doLaunch(false),
00074 wasLaunched(false),
00075 lastmode(1)
00076 {this->onInit();}
00077
00078 CNRRemoteRadio::~CNRRemoteRadio()
00079 {
00080 io.stop();
00081 iorunner.join();
00082 if (client) client.shutdown();
00083 }
00084
00085 void CNRRemoteRadio::onInit()
00086 {
00087 ros::NodeHandle nh,ph("~");
00088
00089 std::string portName("/dev/ttyUSB0");
00090 int baud(9600);
00091 ph.param("PortName",portName,portName);
00092 ph.param("BaudRate",baud,baud);
00093 ph.param("Timeout",timeout,timeout);
00094 ph.param("ID",id,id);
00095 ph.param("YawInc",yawInc,yawInc);
00096 ph.param("DummyRequester",doDummyRequest,doDummyRequest);
00097 ph.param("BuoyDistance",doDummyRequest,doDummyRequest);
00098
00099 port.open(portName);
00100 port.set_option(boost::asio::serial_port::baud_rate(baud));
00101
00102 if (!port.is_open())
00103 {
00104 ROS_ERROR("Cannot open port.");
00105 throw std::runtime_error("Unable to open the port.");
00106 }
00107
00108 joyOut = nh.advertise<sensor_msgs::Joy>("joy_out",1);
00109 posOut = nh.advertise<auv_msgs::NavSts>("bart_position",1);
00110 posCOut = nh.advertise<auv_msgs::NavSts>("cart_position",1);
00111 launched = nh.advertise<std_msgs::Bool>("launched",1);
00112 hlMsg = nh.advertise<navcon_msgs::HLMessage>("hl_message",1);
00113 client = nh.serviceClient<navcon_msgs::SetHLMode>("SetHLMode", true);
00114 stateHat = nh.subscribe<auv_msgs::NavSts>("stateHat",1,&CNRRemoteRadio::onStateHat,this);
00115
00116 curMode = nh.subscribe<std_msgs::Int32>("current_mode",1,&CNRRemoteRadio::onCurrentMode,this);
00117 launchFlag = nh.subscribe<std_msgs::Bool>("set_launch",1,&CNRRemoteRadio::onLaunch,this);
00118
00119 this->start_receive();
00120 iorunner = boost::thread(boost::bind(&boost::asio::io_service::run,&io));
00121 }
00122
00123 void CNRRemoteRadio::onCurrentMode(const std_msgs::Int32::ConstPtr& mode)
00124 {}
00125
00126 void CNRRemoteRadio::onLaunch(const std_msgs::Bool::ConstPtr& launch)
00127 {
00128 doLaunch = launch->data;
00129 }
00130
00131 void CNRRemoteRadio::onStateHat(const auv_msgs::NavSts::ConstPtr& estimate)
00132 {
00133 boost::mutex::scoped_lock l(cdataMux);
00134 currYaw = estimate->orientation.yaw;
00135 currLat = estimate->global_position.latitude;
00136 currLon = estimate->global_position.longitude;
00137 }
00138
00139 void CNRRemoteRadio::start_receive()
00140 {
00141 boost::asio::async_read(port, boost::asio::buffer(&buffer[0],sync_length),
00142 boost::bind(&CNRRemoteRadio::onSync,this,_1,_2));
00143 }
00144
00145 void CNRRemoteRadio::onSync(const boost::system::error_code& error, const size_t& transferred)
00146 {
00147 if (!error)
00148 {
00149 if (transferred == 1)
00150 {
00151
00152 ringBuffer[0] = ringBuffer[1];
00153 ringBuffer[1] = ringBuffer[2];
00154 ringBuffer[2] = buffer[0];
00155 }
00156 else
00157 {
00158 ringBuffer[0] = buffer[0];
00159 ringBuffer[1] = buffer[1];
00160 ringBuffer[2] = buffer[2];
00161 }
00162
00163 if ((ringBuffer[0] == 'C') && (ringBuffer[1]=='P'))
00164 {
00165 ROS_INFO("Synced on CP.");
00166 buffer.resize(sync_length + ringBuffer[2] + chksum_size);
00167 buffer[0] = ringBuffer[0];
00168 buffer[1] = ringBuffer[1];
00169 buffer[2] = ringBuffer[2];
00170 ROS_INFO("Reading %d bytes.", ringBuffer[2] + chksum_size);
00171 boost::asio::async_read(port,boost::asio::buffer(&buffer[3], ringBuffer[2] + chksum_size),boost::bind(&CNRRemoteRadio::onIncomingData,this,_1,_2));
00172 }
00173 else
00174 {
00175 ROS_INFO("No sync: %d %d %d",ringBuffer[0],ringBuffer[1],ringBuffer[2]);
00176 boost::asio::async_read(port, boost::asio::buffer(&buffer[0],1),
00177 boost::bind(&CNRRemoteRadio::onSync,this,_1,_2));
00178 }
00179 }
00180 }
00181
00182 void CNRRemoteRadio::onIncomingData(const boost::system::error_code& error, const size_t& transferred)
00183 {
00184 int dataLen = sync_length + transferred - chksum_size;
00185 uint16_t chksum = compute_crc16(reinterpret_cast<const char*>(&buffer[0]),dataLen);
00186 uint16_t rcvchksum = 256*buffer[dataLen] + buffer[dataLen+1];
00187
00188 if (chksum == rcvchksum)
00189 {
00190 int32_t recv = (buffer[id_field] & 0xF0) >> 4;
00191 int32_t sender = buffer[id_field] & 0x0F;
00192
00193 ROS_INFO("Message from %d to %d.",sender,recv);
00194
00195
00196 if ((this->id == station) && (sender == bart))
00197 {
00198 int32_t data1=static_cast<int32_t>(
00199 htonl(*reinterpret_cast<uint32_t*>(&buffer[data1_field])));
00200 int32_t data2=static_cast<int32_t>(
00201 htonl(*reinterpret_cast<uint32_t*>(&buffer[data2_field])));
00202
00203 lastModemMsg = ros::Time::now();
00204 auv_msgs::NavSts currPose;
00205 currPose.global_position.latitude = data1/double(latlonmux);
00206 currPose.global_position.longitude = data2/double(latlonmux);
00207 posOut.publish(currPose);
00208 }
00209
00210 if ((this->id == station) && (sender == cart))
00211 {
00212 int32_t data1=static_cast<int32_t>(
00213 htonl(*reinterpret_cast<uint32_t*>(&buffer[data1_field+1])));
00214 int32_t data2=static_cast<int32_t>(
00215 htonl(*reinterpret_cast<uint32_t*>(&buffer[data2_field+1])));
00216 int16_t hdg=static_cast<int16_t>(
00217 htons(*reinterpret_cast<int16_t*>(&buffer[data2_field+5])));
00218
00219 ROS_INFO("Received from CART: %d %d %d", data1,data2,hdg);
00220
00221 lastModemMsg = ros::Time::now();
00222 auv_msgs::NavSts currPose;
00223 currPose.global_position.latitude = data1/double(latlonmux);
00224 currPose.global_position.longitude = data2/double(latlonmux);
00225 currPose.orientation.yaw = labust::math::wrapRad(hdg/100./180.*M_PI);
00226 posCOut.publish(currPose);
00227 }
00228
00229 if ((this->id == bart) && (recv == bart))
00230 {
00231 if (buffer[launch_field] == 1)
00232 {
00233 ROS_INFO("The vehicle will be launched.");
00234 if (!wasLaunched)
00235 {
00236 std_msgs::Bool launcher;
00237 launcher.data = true;
00238 launched.publish(launcher);
00239 doLaunch = true;
00240 }
00241
00242 if (wasLaunched)
00243 {
00244
00245 navcon_msgs::SetHLMode mode;
00246 while (!client)
00247 {
00248 ROS_ERROR("HLManager client not connected. Trying reset.");
00249 client = nh.serviceClient<navcon_msgs::SetHLMode>("SetHLMode", true);
00250 }
00251
00252 ROS_INFO("Was launched - Request stop launch.");
00253 client.call(mode);
00254 doLaunch = false;
00255 wasLaunched = false;
00256 }
00257 }
00258 else
00259 {
00260 if (doLaunch)
00261 {
00262 wasLaunched = true;
00263 }
00264
00265 if (!doLaunch)
00266 {
00267 ROS_INFO("Request stop launch.");
00268
00269 navcon_msgs::SetHLMode mode;
00270 while (!client)
00271 {
00272 ROS_ERROR("HLManager client not connected. Trying reset.");
00273 client = nh.serviceClient<navcon_msgs::SetHLMode>("SetHLMode", true);
00274 }
00275
00276 client.call(mode);
00277 }
00278
00279 if (wasLaunched)
00280 {
00281 ROS_INFO("Was launched.");
00282 }
00283 }
00284 boost::mutex::scoped_lock l2(clientMux);
00285 lastModemMsg = ros::Time::now();
00286 this->replyBuoy();
00287 }
00288
00289 if ((this->id == cart) && (recv == cart))
00290 {
00291 lastmode = buffer[mode_field];
00292 std::bitset<8> modes(buffer[mode_field]);
00293 navcon_msgs::HLMessagePtr msg(new navcon_msgs::HLMessage());
00294 navcon_msgs::SetHLMode mode;
00295
00296 if (modes[stopbit])
00297 {
00298 ROS_INFO("Switch to stop mode.");
00299 mode.request.mode = mode.request.Stop;
00300 }
00301 else
00302 {
00303 int32_t data1=static_cast<int32_t>(
00304 htonl(*reinterpret_cast<uint32_t*>(&buffer[data1_field])));
00305 int32_t data2=static_cast<int32_t>(
00306 htonl(*reinterpret_cast<uint32_t*>(&buffer[data2_field])));
00307
00308 if (modes[manualbit])
00309 {
00310 mode.request.mode = mode.request.Manual;
00311 sensor_msgs::Joy joy;
00312 joy.axes.assign(6,0);
00313 joy.axes[1] = data2/10000.;
00314 joy.axes[2] = -data1/10000.;
00315
00316 if (fabs(joy.axes[1]) > 1)
00317 {
00318 ROS_ERROR("Remote joystick force is above 1: %f", joy.axes[1]);
00319 joy.axes[1] = 0;
00320 }
00321 if (fabs(joy.axes[2]) > 1)
00322 {
00323 ROS_ERROR("Remote joystick force is above 1: %f",joy.axes[2]);
00324 joy.axes[2] = 0;
00325 }
00326 joyOut.publish(joy);
00327
00328 ROS_INFO("Switch to manual mode: %f %f",
00329 joy.axes[1],
00330 joy.axes[2]);
00331 }
00332
00333 if (modes[automaticbit])
00334 {
00335 mode.request.mode = mode.request.HeadingControl;
00336 sensor_msgs::Joy joy;
00337 joy.axes.assign(6,0);
00338 joy.axes[1] = data2/10000.;
00339 if (fabs(joy.axes[1]) > 1)
00340 {
00341 ROS_ERROR("Remote joystick force is above 1:", joy.axes[1]);
00342 joy.axes[1] = 0;
00343 }
00344 joyOut.publish(joy);
00345
00346 boost::mutex::scoped_lock l(cdataMux);
00347 desiredHeading += data1/10000.*yawInc;
00348 mode.request.yaw = labust::math::wrapRad(desiredHeading);
00349
00350 ROS_INFO("Automatic mode state: %d %d",
00351 data2,data1);
00352
00353 ROS_INFO("Switch to automatic mode: %f %f",
00354 joy.axes[1],
00355 mode.request.yaw);
00356 }
00357 else
00358 {
00359 desiredHeading = currYaw;
00360 }
00361
00362 if (modes[remotebit])
00363 {
00364 mode.request.mode = mode.request.CirclePoint;
00365 mode.request.surge = 0.5;
00366
00367 mode.request.ref_point.header.frame_id = "worldLatLon";
00368 if ((data1 == 0) && (data2 == 0))
00369 {
00370
00371 mode.request.ref_point.point.x = desiredLat;
00372 mode.request.ref_point.point.y = desiredLon;
00373 }
00374 else
00375 {
00376 mode.request.ref_point.point.x = data1/double(latlonmux);
00377 mode.request.ref_point.point.y = data2/double(latlonmux);
00378 }
00379
00380 ROS_INFO("Switch to remote mode: %f %f.",
00381 mode.request.ref_point.point.x,
00382 mode.request.ref_point.point.y);
00383 }
00384 else
00385 {
00386 std::pair<double, double> location = labust::tools::meter2deg(buoyDistance*cos(currYaw),
00387 buoyDistance*sin(currYaw),
00388 currLat);
00389
00390 desiredLat= currLat + location.first;
00391 desiredLon = currLon + location.second;
00392 }
00393 }
00394
00395 boost::mutex::scoped_lock l2(clientMux);
00396 lastModemMsg = ros::Time::now();
00397 if (!client)
00398 {
00399 ROS_ERROR("HLManager client not connected. Trying reset.");
00400 client = nh.serviceClient<navcon_msgs::SetHLMode>("SetHLMode", true);
00401 }
00402 else
00403 {
00404 client.call(mode);
00405 }
00406
00407 this->reply();
00408 }
00409 }
00410 else
00411 {
00412 ROS_ERROR("Wrong CRC.");
00413 }
00414
00415 start_receive();
00416 }
00417
00418 void CNRRemoteRadio::reply()
00419 {
00420 char ret[17];
00421 ret[0] = 'C';
00422 ret[1] = 'P';
00423 ret[2] = 12;
00424 ret[3] = (station<<4) + cart;
00425 ret[4] = lastmode;
00426 boost::mutex::scoped_lock l(cdataMux);
00427 uint32_t lat = htonl(currLat*latlonmux);
00428 uint32_t lon = htonl(currLon*latlonmux);
00429 uint16_t hdg = htons(currYaw*100/M_PI*180);
00430 l.unlock();
00431 ROS_INFO("The cart reply: %d, %d %d",htonl(lat), htonl(lon), int16_t(htons(hdg)));
00432 memcpy(&ret[5],&lat,sizeof(uint32_t));
00433 memcpy(&ret[9],&lon,sizeof(uint32_t));
00434 memcpy(&ret[13],&hdg,sizeof(uint16_t));
00435 int crc = compute_crc16(&ret[0], 15);
00436 ret[15] = crc/256;
00437 ret[16] = crc%256;
00438 boost::asio::write(port, boost::asio::buffer(ret,sizeof(ret)));
00439 }
00440
00441 void CNRRemoteRadio::replyBuoy()
00442 {
00443 char ret[15];
00444 ret[0] = 'C';
00445 ret[1] = 'P';
00446 ret[2] = 10;
00447 ret[3] = (station<<4) + bart;
00448 boost::mutex::scoped_lock l(cdataMux);
00449 uint32_t lat = htonl(currLat*latlonmux);
00450 uint32_t lon = htonl(currLon*latlonmux);
00451 l.unlock();
00452 ROS_INFO("The buoy reply: %d, %d",htonl(lat), htonl(lon));
00453 memcpy(&ret[4],&lat,sizeof(uint32_t));
00454 memcpy(&ret[8],&lon,sizeof(uint32_t));
00455 ret[12] = doLaunch;
00456 int crc = compute_crc16(&ret[0], 13);
00457 ret[13] = crc/256;
00458 ret[14] = crc%256;
00459 boost::asio::write(port, boost::asio::buffer(ret,sizeof(ret)));
00460 }
00461
00462 void CNRRemoteRadio::dummyRequest()
00463 {
00464 char ret[7];
00465 ret[0] = 'C';
00466 ret[1] = 'P';
00467 ret[2] = 2;
00468 ret[3] = (bart<<4) + station;
00469 ret[4] = doLaunch;
00470
00471 int crc = compute_crc16(&ret[0], 5);
00472 ret[5] = crc/256;
00473 ret[6] = crc%256;
00474 boost::asio::write(port, boost::asio::buffer(ret,sizeof(ret)));
00475 }
00476
00477 void CNRRemoteRadio::onTimeout()
00478 {
00479 if (!client)
00480 {
00481 ROS_ERROR("HLManager client not connected. Trying reset.");
00482 client = nh.serviceClient<navcon_msgs::SetHLMode>("SetHLMode", true);
00483 }
00484 else
00485 {
00486 navcon_msgs::SetHLMode mode;
00487 mode.request.mode = 0;
00488 client.call(mode);
00489 }
00490
00491 ROS_ERROR("Lost connection with CNRRemote!");
00492
00493 io.stop();
00494 iorunner.join();
00495 port.close();
00496
00497 io.reset();
00498 lastModemMsg = ros::Time::now();
00499 ros::Rate rate(1);
00500 for(int i=0;i<3;++i) rate.sleep();
00501
00502 std::string portName;
00503 int baud;
00504 ph.param("PortName",portName,portName);
00505 ph.param("BaudRate",baud,baud);
00506
00507 port.open(portName);
00508 port.set_option(boost::asio::serial_port::baud_rate(baud));
00509
00510 if (!port.is_open())
00511 {
00512 ROS_ERROR("Cannot open port.");
00513 throw std::runtime_error("Unable to open the port.");
00514 }
00515
00516 this->start_receive();
00517 iorunner = boost::thread(boost::bind(&boost::asio::io_service::run,&io));
00518 }
00519
00520 void CNRRemoteRadio::start()
00521 {
00522 ros::Rate rate(20);
00523
00524 ros::Time lastDummyReq(ros::Time::now());
00525 while (ros::ok())
00526 {
00527 rate.sleep();
00528 ros::spinOnce();
00529 boost::mutex::scoped_lock l(clientMux);
00530 if ((ros::Time::now()-lastModemMsg).toSec() > timeout )
00531 {
00532 this->onTimeout();
00533 }
00534
00535 if (doDummyRequest && ((ros::Time::now() - lastDummyReq).toSec() > 0.5))
00536 {
00537 lastDummyReq = ros::Time::now();
00538
00539
00540
00541
00542 }
00543 }
00544 }
00545