USBLNodelet.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: 02.04.2013.
00036 *********************************************************************/
00037 #include <labust/tritech/USBLNodelet.hpp>
00038 #include <labust/tritech/TCPDevice.hpp>
00039 #include <labust/tritech/mtMessages.hpp>
00040 #include <labust/tritech/USBLMessages.hpp>
00041 #include <pluginlib/class_list_macros.h>
00042 
00043 #include <geometry_msgs/PointStamped.h>
00044 #include <std_msgs/Float32MultiArray.h>
00045 
00046 #include <boost/archive/binary_iarchive.hpp>
00047 #include <boost/archive/binary_oarchive.hpp>
00048 
00049 PLUGINLIB_DECLARE_CLASS(usbl,USBLNodelet,labust::tritech::USBLNodelet, nodelet::Nodelet)
00050 
00051 using namespace labust::tritech;
00052 
00053 USBLNodelet::USBLNodelet():
00054         address("127.0.0.1"),
00055         port(4000),
00056         usblBusy(false),
00057         autoMode(true),
00058         ping_timeout(7){};
00059 
00060 USBLNodelet::~USBLNodelet()
00061 {
00062         stop();
00063         usbl.reset();
00064 };
00065 
00066 void USBLNodelet::onInit()
00067 {
00068         ros::NodeHandle ph = this->getPrivateNodeHandle();
00069         ph.param("ip",address,address);
00070         ph.param("port",port,port);
00071         std::string op_mode("auto");
00072         ph.param("op_mode",op_mode,op_mode);
00073         autoMode = (op_mode == "auto");
00074         //Connect to the TCPDevice
00075         usbl.reset(new TCPDevice(address,port));
00076         attitude.reset(new TCPDevice(address,port, 
00077                 labust::tritech::Nodes::AttitudeSensor,
00078                 labust::tritech::TCPRequest::atMiniAttSen,
00079                 127));
00080         //Register handlers
00081         TCPDevice::HandlerMap map;
00082         map[MTMsg::mtAlive] = boost::bind(&USBLNodelet::onTCONMsg,this,_1);
00083         map[MTMsg::mtAMNavDataV2] = boost::bind(&USBLNodelet::onNavMsg,this, _1);
00084         map[MTMsg::mtMiniAttData] = boost::bind(&USBLNodelet::onAttMsg, this, _1);
00085         //map[MTMsg::mtAMNavDataV2] = map[MTMsg::mtAMNavData];
00086         usbl->registerHandlers(map);
00087         attitude->registerHandlers(map);
00088 
00089         ros::NodeHandle nh = this->getNodeHandle();
00090         dataSub = nh.subscribe<std_msgs::String>("outgoing_data",       0, boost::bind(&USBLNodelet::onOutgoingMsg,this,_1));
00091         opMode = nh.subscribe<std_msgs::Bool>("auto_mode",      0, boost::bind(&USBLNodelet::onAutoMode,this,_1));
00092         navPub = nh.advertise<geometry_msgs::PointStamped>("usbl_nav",1);
00093         dataPub = nh.advertise<std_msgs::String>("incoming_data",1);
00094         usblTimeout = nh.advertise<std_msgs::Bool>("usbl_timeout",1);
00095         attRaw = nh.advertise<std_msgs::Float32MultiArray>("usbl_att_raw",1);
00096         attData = nh.advertise<geometry_msgs::PointStamped>("usbl_att",1);
00097 
00098         if (autoMode) worker = boost::thread(boost::bind(&USBLNodelet::run,this));
00099 }
00100 
00101 void USBLNodelet::stop()
00102 {
00103         {
00104                 boost::mutex::scoped_lock lock(pingLock);
00105                 this->usblBusy = false;
00106                 this->autoMode = false;
00107         }
00108         usblCondition.notify_all();
00109         worker.join();
00110 }
00111 
00112 void USBLNodelet::onAutoMode(const std_msgs::Bool::ConstPtr mode)
00113 {
00114         //Turn off autoMode
00115         if (this->autoMode && !mode->data)
00116         {
00117                 stop();
00118                 NODELET_INFO("Turning off USBL auto interrogation.");
00119         }
00120 
00121         //Turn on autoMode
00122         if (!this->autoMode && mode->data)
00123         {
00124                 this->autoMode = true;
00125                 worker = boost::thread(boost::bind(&USBLNodelet::run,this));
00126                 NODELET_INFO("Turning on USBL auto interrogation.");
00127         }
00128 };
00129 
00130 void USBLNodelet::onOutgoingMsg(const std_msgs::String::ConstPtr msg)
00131 {
00132         boost::mutex::scoped_lock lock(dataMux);
00133         msg_out = msg->data;
00134         if (!autoMode) sendUSBLPkg();
00135 };
00136 
00137 void USBLNodelet::onTCONMsg(labust::tritech::TCONMsgPtr tmsg)
00138 {
00139         if (tmsg->msgType == MTMsg::mtAlive)
00140         {
00141                 NODELET_DEBUG("Recevied alive message.");
00142         }
00143         else
00144         {
00145                 NODELET_DEBUG("Recevied TCONMsg %d.",tmsg->msgType);
00146         }
00147 }
00148 
00149 void USBLNodelet::onAttMsg(labust::tritech::TCONMsgPtr tmsg)
00150 {
00151         boost::archive::binary_iarchive dataSer(*tmsg->data, boost::archive::no_header);
00152         AttSenData att_data;
00153         dataSer>>att_data;
00154 
00155         geometry_msgs::PointStamped::Ptr attOut(new geometry_msgs::PointStamped());
00156         std_msgs::Float32MultiArray::Ptr rawOut(new std_msgs::Float32MultiArray());
00157 
00158         rawOut->data.push_back(att_data.cmd);
00159         rawOut->data.push_back(att_data.time);
00160         rawOut->data.push_back(att_data.pressure);
00161         rawOut->data.push_back(att_data.internalTemp);
00162         rawOut->data.push_back(att_data.externalTemp);
00163         for (int i=0; i<3; ++i) rawOut->data.push_back(att_data.acc[i]);
00164         for (int i=0; i<3; ++i) rawOut->data.push_back(att_data.mag[i]);
00165         for (int i=0; i<3; ++i) rawOut->data.push_back(att_data.gyro[i]);
00166 
00167         attRaw.publish(rawOut);
00168 }
00169 
00170 void USBLNodelet::onNavMsg(labust::tritech::TCONMsgPtr tmsg)
00171 {
00172         {
00173                 boost::mutex::scoped_lock lock(pingLock);
00174                 usblBusy = false;
00175         }
00176         usblCondition.notify_one();
00177 
00178         boost::archive::binary_iarchive dataSer(*tmsg->data, boost::archive::no_header);
00179         USBLData usbl_data;
00180         MMCMsg modem_data;
00181 
00182         if (tmsg->msgType == MTMsg::mtAMNavData)
00183         {
00184                 //Decoding V1 message
00185                 NODELET_DEBUG("Decoding V1 message type: %d\n",tmsg->msgType);
00186                 dataSer>>usbl_data;
00187         }
00188         else
00189         {
00190                 //Decoding V2 message
00191                 NODELET_DEBUG("Decoding V2 message type: %d\n",tmsg->msgType);
00192                 dataSer>>usbl_data>>modem_data;
00193         }
00194 
00195         geometry_msgs::PointStamped::Ptr usblOut(new geometry_msgs::PointStamped());
00196 
00197         if (usbl_data.reply_validity == 30)
00198         {
00199                 usblOut->header.stamp = ros::Time::now();
00200                 usblOut->header.frame_id = "usbl";
00201 
00202                 usblOut->point.x = usbl_data.attitudeCorrectedPos[1];
00203                 usblOut->point.y = usbl_data.attitudeCorrectedPos[0];
00204                 usblOut->point.z = usbl_data.attitudeCorrectedPos[2];
00205 
00206                 navPub.publish(usblOut);
00207 
00208                 if (modem_data.msgType != mmcRangeData)
00209                 {
00210                         std_msgs::String::Ptr modem(new std_msgs::String());
00211                         size_t size = modem_data.data[MMCMsg::ranged_payload_size]/8;
00212                         std::cout<<"Modem data byte size "<<size<<" data:";
00213                         for(int i=0; i<modem_data.data.size(); ++i) std::cout<<int(modem_data.data[i])<<",";
00214                         std::cout<<std::endl;
00215                         modem->data.assign(modem_data.data.begin() + MMCMsg::ranged_payload_size,
00216                                         modem_data.data.begin() + MMCMsg::ranged_payload_size + size + 1);
00217                         dataPub.publish(modem);
00218                 }
00219         }
00220         else
00221         {
00222                 NODELET_DEBUG("Invalid data reply from USBL. Validity:%d\n",usbl_data.reply_validity);
00223                 std_msgs::Bool data;
00224                 data.data = true;
00225                 usblTimeout.publish(data);
00226         }
00227 
00228         NODELET_DEBUG("Received data message.\n");
00229 }
00230 
00231 void USBLNodelet::sendUSBLPkg()
00232 {
00233         TCONMsgPtr tmsg(new TCONMsg());
00234         tmsg->txNode = 255;
00235         tmsg->rxNode = labust::tritech::Nodes::USBL;
00236         tmsg->node = 255;
00237         tmsg->msgType = MTMsg::mtMiniModemCmd;
00238 
00239         MMCMsg mmsg;
00240         mmsg.msgType = labust::tritech::mmcGetRangeSync;
00241 
00242         if (msg_out.size())
00243         {
00244                 //Switch message size
00245                 if (msg_out[0] == 48) mmsg.msgType = labust::tritech::mmcGetRangeTxRxBits48;
00246                 //mmsg.data[0] = 48;
00247                 //for (int i=0;i<msg_out.size();++i) mmsg.data[i+1] = msg_out[i];
00248                 for (int i=0;i<msg_out.size();++i) mmsg.data[i] = msg_out[i];
00249                 msg_out.clear();
00250         }
00251 
00252         boost::archive::binary_oarchive ar(*tmsg->data, boost::archive::no_header);
00253         ar<<mmsg;
00254         usbl->send(tmsg);
00255         usblBusy = true;
00256 
00257         boost::mutex::scoped_lock lock(pingLock);
00258         boost::system_time const timeout=boost::get_system_time()+boost::posix_time::seconds(ping_timeout);
00259         while (usblBusy) 
00260         {
00261                 if (!usblCondition.timed_wait(lock,timeout))
00262                 { 
00263                         NODELET_INFO("USBL went into timeout.");
00264                         std_msgs::Bool data;
00265                         data.data = true;
00266                         usblTimeout.publish(data);
00267                         break;
00268                 }
00269         }
00270 }
00271 
00272 void USBLNodelet::run()
00273 {
00274         while (ros::ok() && autoMode)
00275         {
00276                 sendUSBLPkg();
00277                 //Broadcast frame
00281                 //tf::Transform transform;
00282                 //transform.setOrigin(tf::Vector3(0.25, 0.0, 0.5) );
00283                 //transform.setRotation(tf::Quaternion(0,0,0,1));
00284                 //frameBroadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "usbl"));
00285                 NODELET_INFO("Running.");       
00286         }
00287         NODELET_INFO("Exiting run.");   
00288 }
00289 
00290 


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