usbl_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, LABUST, UNIZG-FER
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the LABUST nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *
00034 *  Author: Dula Nad
00035 *  Created: 14.02.2013.
00036 *********************************************************************/
00037 #include <labust/tritech/TCPDevice.hpp>
00038 #include <labust/tritech/mtMessages.hpp>
00039 #include <labust/tritech/USBLMessages.hpp>
00040 #include <underwater_sensor_msgs/USBL.h>
00041 
00042 #include <std_msgs/String.h>
00043 #include <ros/ros.h>
00044 
00045 #include <boost/bind.hpp>
00046 #include <boost/archive/binary_iarchive.hpp>
00047 #include <boost/archive/binary_oarchive.hpp>
00048 
00049 boost::condition_variable usblCondition;
00050 boost::mutex usblMux;
00051 bool usblBusy(false);
00052 
00053 void onMsg(labust::tritech::TCPDevice* usbl, const std_msgs::String::ConstPtr msg)
00054 {
00055         boost::mutex::scoped_lock lock(usblMux);
00056         while (usblBusy)        usblCondition.wait(lock);
00057 
00058         using namespace labust::tritech;
00059 
00060         TCONMsgPtr tmsg(new TCONMsg());
00061         tmsg->txNode = 255;
00062         tmsg->rxNode = labust::tritech::Nodes::USBL;
00063         tmsg->node = labust::tritech::Nodes::USBL;;
00064         tmsg->msgType = MTMsg::mtMiniModemCmd;
00065 
00066         std::istringstream s;
00067         //int id = msg->data.size();
00068 
00069         MMCMsg mmsg;
00070         mmsg.msgType = labust::tritech::mmcGetRangeTxRxBits24;
00071         mmsg.data[0] = 24;
00072         for (int i=0;i<3;++i)   mmsg.data[i+1] = msg->data[i];
00073 
00074         /*switch (msg->data.size())
00075         {
00076          case 0: mmsg.msgType = labust::tritech::mmcGetRangeSync; break;
00077          case 1: mmsg.msgType = labust::tritech::mmcGetRangeTxRxByte; break;
00078          case 2: case 3: mmsg.msgType = labust::tritech::mmcGetRangeTxRxBits24; break;
00079          case 4: mmsg.msgType = labust::tritech::mmcGetRangeTxRxBits32; break;
00080          case 5: mmsg.msgType = labust::tritech::mmcGetRangeTxRxBits40; break;
00081          case 6: mmsg.msgType = labust::tritech::mmcGetRangeTxRxBits48; break;
00082    default:break;
00083         }
00084 
00085         if (msg->data.size()) >
00086         mmsg.data[0] = msg->data.size();
00087         mmsg.data[1] = 50;
00088         mmsg.data[2] = 49;
00089         mmsg.data[3] = 48;
00090         mmsg.data[4] = 55;
00091         mmsg.data[5] = 56;
00092         mmsg.data[6] = 57;
00093 */
00094         std::ostream out();
00095         boost::archive::binary_oarchive ar(*tmsg->data, boost::archive::no_header);
00096         ar<<mmsg;
00097 
00098         usbl->send(tmsg);
00099         usblBusy = true;
00100 };
00101 
00102 void onNavMsg(ros::Publisher* usblNav, ros::Publisher* usblMsg, labust::tritech::TCONMsgPtr tmsg)
00103 {
00104         using namespace labust::tritech;
00105 
00106         {
00107                 boost::mutex::scoped_lock lock(usblMux);
00108                 usblBusy = false;
00109         }
00110         usblCondition.notify_one();
00111 
00112         boost::archive::binary_iarchive dataSer(*tmsg->data, boost::archive::no_header);
00113         USBLDataV2 usbl_data;
00114         dataSer>>usbl_data;
00115 
00116         if (usbl_data.nav.reply_validity != 30)
00117         {
00118                 std::cerr<<"Invalid reply form USBL."<<std::endl;
00119                 //return;
00120         }
00121 
00122         underwater_sensor_msgs::USBL usblOut;
00123 
00124         usblOut.slant_range = usbl_data.nav.range;
00125         usblOut.position.x = usbl_data.nav.attitudeCorrectedPos[0];
00126         usblOut.position.y = usbl_data.nav.attitudeCorrectedPos[1];
00127         usblOut.position.z = usbl_data.nav.attitudeCorrectedPos[2];
00128         usblOut.bearing = atan2(usblOut.position.y, usblOut.position.x);
00129         usblOut.id = usbl_data.nav.unitID;
00130         usblOut.header.stamp = ros::Time::now();
00131 
00132         usblNav->publish(usblOut);
00133 
00134         std_msgs::String modem;
00135         modem.data.assign(usbl_data.modem.data.begin(), usbl_data.modem.data.end());
00136         usblMsg->publish(modem);
00137 
00138         std::cout<<"Received data message."<<std::endl;
00139 }
00140 
00141 int main(int argc, char* argv[])
00142 {
00143         using namespace labust::tritech;
00144 
00145         ros::init(argc,argv,"usbl_node");
00146 
00147         ros::NodeHandle nh,ph("~");
00148 
00149         std::string address("127.0.0.1");
00150         int port(4000);
00151 
00152         ph.param("ip",address,address);
00153         ph.param("port",port,port);
00154 
00155         TCPDevice usbl(address,port);
00156 
00157         ros::Subscriber inMsg = nh.subscribe<std_msgs::String>(
00158                         "modem_data",   1,boost::bind(&onMsg,&usbl,_1));
00159         ros::Publisher pub = nh.advertise<underwater_sensor_msgs::USBL>("usbl_nav",1);
00160         ros::Publisher pub2 = nh.advertise<std_msgs::String>("usbl_data",1);
00161 
00162         TCPDevice::HandlerMap map;
00163         map[MTMsg::mtAMNavDataV2] = boost::bind(&onNavMsg,&pub,&pub2, _1);
00164         usbl.registerHandlers(map);
00165 
00166         ros::spin();
00167 
00168         return 0;
00169 }
00170 
00171 
00172 


usbl
Author(s): Gyula Nagy
autogenerated on Thu Jul 10 2014 10:34:21