NavQuestSocketNode.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: 23.01.2013.
00036  *********************************************************************/
00037 #include <labust/navigation/NavQuestSocketNode.hpp>
00038 #include <labust/navigation/DVLdataClass.h>
00039 #include <labust/navigation/NavQuestMessages.hpp>
00040 #include <labust/archive/delimited_iarchive.hpp>
00041 #include <labust/archive/delimited_oarchive.hpp>
00042 #include <labust/preprocessor/clean_serializator.hpp>
00043 #include <labust/tools/conversions.hpp>
00044 #include <labust/math/NumberManipulation.hpp>
00045 #include <labust/tools/StringUtilities.hpp>
00046 
00047 #include <geometry_msgs/TwistStamped.h>
00048 #include <std_msgs/Bool.h>
00049 #include <std_msgs/Float32.h>
00050 #include <auv_msgs/RPY.h>
00051 
00052 #include <boost/bind.hpp>
00053 #include <boost/regex.hpp>
00054 #include <boost/serialization/string.hpp>
00055 
00056 #include <iosfwd>
00057 
00058 PP_LABUST_CLEAN_ARRAY_OSERIALIZATOR_IMPL(labust::archive::delimited_oarchive)
00059 PP_LABUST_CLEAN_ARRAY_ISERIALIZATOR_IMPL(labust::archive::delimited_iarchive)
00060 
00061 using namespace labust::navigation;
00062 
00063 int labust::navigation::error_code(const NQRes& data)
00064 {
00065         std::stringstream ss(data.error_code);
00066         int error;
00067         ss>>std::hex>>error;
00068         return error;
00069 }
00070 
00071 NavQuestSocketNode::NavQuestSocketNode():
00072                                                         io(),
00073                                                         socket(io),
00074                                                         useFixed(true),
00075                                                         base_orientation(0)
00076 {
00077         this->onInit();
00078 }
00079 
00080 NavQuestSocketNode::~NavQuestSocketNode()
00081 {
00082         io.stop();
00083         runner.join();
00084 }
00085 
00086 void NavQuestSocketNode::onInit()
00087 {
00088         ros::NodeHandle nh,ph("~");
00089         ros::Rate r(1);
00090         bool setupOk(false);
00091         while (!(setupOk = this->setup_socket()) && ros::ok())
00092         {
00093                 ROS_ERROR("NavQuestSocketNode::Failed to open socket.");
00094                 r.sleep();
00095         }
00096 
00097         if (setupOk)
00098         {
00099                 //Advertise beam data
00100                 beam_pub["velo_rad"].reset(new NavQuestBP(nh, "velo_rad"));
00101                 beam_pub["wvelo_rad"].reset(new NavQuestBP(nh, "wvelo_rad"));
00102                 speed_pub["velo_instrument"].reset(
00103                                 new TwistPublisher(nh, "velo_instrument", "dvl_frame"));
00104                 speed_pub["velo_earth"].reset(
00105                                 new TwistPublisher(nh, "velo_earth", "local"));
00106                 speed_pub["water_velo_instrument"].reset(
00107                                 new TwistPublisher(nh, "water_velo_instrument", "dvl_frame"));
00108                 speed_pub["water_velo_earth"].reset(
00109                                 new TwistPublisher(nh, "water_velo_earth", "local"));
00110                 lock = nh.advertise<std_msgs::Bool>("dvl_bottom",1);
00111                 altitude = nh.advertise<std_msgs::Float32>("altitude",1);
00112                 imuPub = nh.advertise<auv_msgs::RPY>("dvl_rpy",1);
00113 
00114                 useFixed = ph.getParam("fixed_orientation", base_orientation);
00115                 nh.param("magnetic_declination",magnetic_declination, 0.0);
00116 
00117                 //Start the receive cycle
00118                 this->start_receive();
00119                 runner = boost::thread(boost::bind(&boost::asio::io_service::run,&io));
00120                 setup_messaging();
00121 
00122         }
00123 }
00124 
00125 void NavQuestSocketNode::setup_messaging()
00126 {
00127         //Setup map for more than one message
00128 }
00129 
00130 void NavQuestSocketNode::setup_publishers()
00131 {
00132         //Setup maps for publishing
00133 }
00134 
00135 void NavQuestSocketNode::start_receive()
00136 {
00137         using namespace boost::asio;
00138         async_read_until(socket, buffer,
00139                         boost::regex("\r\n"),
00140                         boost::bind(&NavQuestSocketNode::onDvlData, this, _1,_2));
00141 }
00142 
00143 bool NavQuestSocketNode::setup_socket()
00144 {
00145         ros::NodeHandle ph("~");
00146         std::string IPAddress("192.168.255.10");
00147         int tcpPort(10004);
00148 
00149         ph.param("IPAdress",IPAddress,IPAddress);
00150         ph.param("tcpPort",tcpPort,tcpPort);
00151 
00152         using namespace boost::asio;
00153         ip::tcp::endpoint endpoint(ip::address::from_string(IPAddress.c_str()), tcpPort);
00154         socket.connect(endpoint);
00155 
00156         return socket.is_open();
00157 }
00158 
00159 bool NavQuestSocketNode::test_header(const std::string& match, const std::string& stream)
00160 {
00161         return stream.substr(0,match.size()) == match;
00162 }
00163 
00164 void NavQuestSocketNode::publishDvlData(const NQRes& data)
00165 {
00166         geometry_msgs::TwistStamped::Ptr twist(new geometry_msgs::TwistStamped());
00167 
00168         bool testDVL = data.velo_instrument[0] == data.velo_instrument[1];
00169         testDVL = testDVL && (data.velo_instrument[1] == data.velo_instrument[2]);
00170         testDVL = testDVL && (data.velo_instrument[2] == 0);
00171 
00172         //Data validity
00173         bool beamValidity = true;
00174         for (int i=0; i<4; ++i)
00175         {
00176                 beamValidity = beamValidity && (data.beam_status[i] == 1);
00177                 //ROS_INFO("Beam validity %d: %d", i, data.beam_status[i]);
00178                 //ROS_INFO("Water vel credit %d: %f", i, data.wvelo_credit[i]);
00179         }
00180 
00181         if (testDVL)
00182         {
00183                 ROS_INFO("All zero dvl. Ignore measurement.");
00184                 return;
00185         }
00186 
00187         if (!beamValidity)
00188         {
00189                 //ROS_INFO("One or more beams are invalid. Ignore measurement: %f %f", data.velo_instrument[0]/1000, data.velo_instrument[1]/1000);
00190                 return;
00191         }
00192         else
00193         {
00194                 ROS_INFO("Beams are valid. Accept measurement: %f %f", data.velo_instrument[0]/1000, data.velo_instrument[1]/1000);
00195         }
00196 
00197         (*beam_pub["velo_rad"])(data.velo_rad);
00198         (*beam_pub["wvelo_rad"])(data.wvelo_rad);
00199         (*speed_pub["velo_instrument"])(data.velo_instrument);
00200         (*speed_pub["velo_earth"])(data.velo_earth);
00201         (*speed_pub["water_velo_instrument"])(data.water_velo_instrument);
00202         (*speed_pub["water_velo_earth"])(data.water_velo_earth);
00203 
00204         enum{valid_flag=3};
00205         bool water_lock= (data.velo_instrument[valid_flag]==2) || (data.velo_earth[valid_flag]==2);
00206         bool valid=data.velo_instrument[valid_flag] && data.velo_earth[valid_flag];
00207         std_msgs::Bool bottom_lock;
00208         bottom_lock.data = !water_lock && valid;
00209         lock.publish(bottom_lock);
00210 
00211         //Altitude
00212         if (data.altitude_estimate > 0)
00213         {
00214                 std_msgs::Float32Ptr alt(new std_msgs::Float32());
00215                 alt->data = data.altitude_estimate;
00216                 altitude.publish(alt);
00217         }
00218 
00219         //TF frame
00220         //Either use a fixed rotation here or the DVL measurements
00221         enum {roll=0, pitch, yaw};
00222         tf::Transform transform;
00223         //Moved 1.4m from the rotation origin.
00224         transform.setOrigin(tf::Vector3(1.4,0,0));
00225         //transform.setOrigin(tf::Vector3(0,0,0));
00226         if (useFixed)
00227         {
00228                 transform.setRotation(tf::createQuaternionFromRPY(0,0, base_orientation));
00229                 broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "dvl_frame"));
00230         }
00231         else
00232         {
00233                 Eigen::Quaternion<float> quat;
00234                 labust::tools::quaternionFromEulerZYX(labust::math::wrapRad(data.rph[roll]/180*M_PI),
00235                                 labust::math::wrapRad(data.rph[pitch]/180*M_PI),
00236                                 labust::math::wrapRad(data.rph[yaw]/180*M_PI + magnetic_declination), quat);
00237                 transform.setRotation(tf::Quaternion(quat.x(), quat.y(), quat.z(), quat.w()));
00238                 broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "dvl_frame"));
00239         }
00240 
00241         //RPY
00242         auv_msgs::RPY::Ptr rpy(new auv_msgs::RPY());
00243         rpy->roll = labust::math::wrapRad(data.rph[roll]/180*M_PI);
00244         rpy->pitch = labust::math::wrapRad(data.rph[pitch]/180*M_PI);
00245         rpy->yaw = labust::math::wrapRad(data.rph[yaw]/180*M_PI);
00246         imuPub.publish(rpy);
00247 }
00248 
00249 void NavQuestSocketNode::conditionDvlData(const NQRes& data)
00250 {}
00251 
00252 void NavQuestSocketNode::onDvlData(const boost::system::error_code& e,
00253                 std::size_t size)
00254 {
00255         if (!e)
00256         {
00257                 std::istream is(&buffer);
00258                 std::string data(size,'\0');
00259                 is.read(&data[0],size);
00260                 //ROS_ERROR("dvl: %s", &data[0]);
00261 
00262                 if (test_header("$#NQ.RES", data))
00263                 {
00264                         NQRes dvl_data;
00265                         std::istringstream is;
00266                         is.rdbuf()->pubsetbuf(&data[0],size);
00267                         labust::archive::delimited_iarchive ia(is);
00268                         ia>>dvl_data;
00269 
00270                         if (error_code(dvl_data) == 0) publishDvlData(dvl_data);
00271 
00272                         ROS_INFO("DVL decoded: header:%s, error:%s.",
00273                                         dvl_data.header.c_str(),
00274                                         dvl_data.error_code.c_str());
00275                 }
00276         }
00277         else
00278         {
00279                 ROS_ERROR("NavQuestSocketNode: %s",e.message().c_str());
00280         }
00281         this->start_receive();
00282 }
00283 
00284 
00285 int main(int argc, char* argv[])
00286 {
00287         ros::init(argc,argv,"navquest_socket_node");
00288         NavQuestSocketNode node;
00289         ros::spin();
00290 
00291         return 0;
00292 }
00293 


navquest_dvl
Author(s): Gyula Nagy
autogenerated on Fri Feb 7 2014 11:37:15