NavQuestNode.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/NavQuestNode.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 NavQuestNode::NavQuestNode():
00072                                         io(),
00073                                         port(io),
00074                                         useFixed(true),
00075                                         base_orientation(0)
00076 {
00077         this->onInit();
00078 }
00079 
00080 NavQuestNode::~NavQuestNode()
00081 {
00082         io.stop();
00083         runner.join();
00084 }
00085 
00086 void NavQuestNode::onInit()
00087 {
00088         ros::NodeHandle nh, ph("~");
00089         ros::Rate r(1);
00090         bool setupOk(false);
00091         while (!(setupOk = this->setup_port()) && ros::ok())
00092         {
00093                 ROS_ERROR("NavQuestNode::Failed to open port.");
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 void NavQuestNode::setup_messaging()
00125 {
00126         //Setup map for more than one message
00127 }
00128 
00129 void NavQuestNode::setup_publishers()
00130 {
00131         //Setup maps for publishing
00132 }
00133 
00134 void NavQuestNode::start_receive()
00135 {
00136         using namespace boost::asio;
00137         async_read_until(port, buffer,
00138                         boost::regex("\r\n"),
00139                         boost::bind(&NavQuestNode::onDvlData, this, _1,_2));
00140 }
00141 
00142 bool NavQuestNode::setup_port()
00143 {
00144         ros::NodeHandle ph("~");
00145         std::string portName("/dev/ttyUSB0");
00146         int baud(115200);
00147 
00148         ph.param("PortName",portName,portName);
00149         ph.param("Baud",baud,baud);
00150 
00151         using namespace boost::asio;
00152         port.open(portName);
00153         port.set_option(serial_port::baud_rate(baud));
00154         port.set_option(serial_port::flow_control(
00155                         serial_port::flow_control::none));
00156 
00157         return port.is_open();
00158 }
00159 
00160 bool NavQuestNode::test_header(const std::string& match, const std::string& stream)
00161 {
00162         return stream.substr(0,match.size()) == match;
00163 }
00164 
00165 void NavQuestNode::publishDvlData(const NQRes& data)
00166 {
00167         geometry_msgs::TwistStamped::Ptr twist(new geometry_msgs::TwistStamped());
00168         
00169         bool testDVL = data.velo_instrument[0] == data.velo_instrument[1];
00170         testDVL = testDVL && (data.velo_instrument[1] == data.velo_instrument[2]);
00171         testDVL = testDVL && (data.velo_instrument[2] == 0);
00172 
00173         //Data validity
00174         bool beamValidity = true;
00175         for (int i=0; i<4; ++i)
00176         {
00177           beamValidity = beamValidity && (data.beam_status[i] == 1);
00178           //ROS_INFO("Beam validity %d: %d", i, data.beam_status[i]);
00179           //ROS_INFO("Water vel credit %d: %f", i, data.wvelo_credit[i]);
00180         }
00181 
00182         if (testDVL)
00183         {
00184           ROS_INFO("All zero dvl. Ignore measurement.");
00185           return;
00186         }
00187         
00188         if (!beamValidity)
00189         {
00190           //ROS_INFO("One or more beams are invalid. Ignore measurement: %f %f", data.velo_instrument[0]/1000, data.velo_instrument[1]/1000);
00191           return;
00192         }
00193         else
00194         {
00195           ROS_INFO("Beams are valid. Accept measurement: %f %f", data.velo_instrument[0]/1000, data.velo_instrument[1]/1000);
00196         }
00197 
00198         (*beam_pub["velo_rad"])(data.velo_rad);
00199         (*beam_pub["wvelo_rad"])(data.wvelo_rad);
00200         (*speed_pub["velo_instrument"])(data.velo_instrument);
00201         (*speed_pub["velo_earth"])(data.velo_earth);
00202         (*speed_pub["water_velo_instrument"])(data.water_velo_instrument);
00203         (*speed_pub["water_velo_earth"])(data.water_velo_earth);
00204 
00205         //Bottom lock flag
00206         enum{valid_flag=3};
00207         bool water_lock= (data.velo_instrument[valid_flag]==2) || (data.velo_earth[valid_flag]==2);
00208         bool valid=data.velo_instrument[valid_flag] && data.velo_earth[valid_flag];
00209         std_msgs::Bool bottom_lock;
00210         bottom_lock.data = !water_lock && valid;
00211         lock.publish(bottom_lock);
00212         //ROS_INFO("Has bottom lock %d", bottom_lock.data);
00213 
00214         //Altitude
00215         if (data.altitude_estimate > 0)
00216         {
00217                 std_msgs::Float32Ptr alt(new std_msgs::Float32());
00218                 alt->data = data.altitude_estimate;
00219                 altitude.publish(alt);
00220         }
00221 
00222         //TF frame
00223         //Either use a fixed rotation here or the DVL measurements
00224         enum {roll=0, pitch, yaw};
00225         tf::Transform transform;
00226         transform.setOrigin(tf::Vector3(0,0,0));
00227         if (useFixed)
00228         {
00229                 transform.setRotation(tf::createQuaternionFromRPY(0,0, base_orientation));
00230                 broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "dvl_frame"));
00231         }
00232         else
00233         {
00234                 Eigen::Quaternion<float> quat;
00235                 labust::tools::quaternionFromEulerZYX(labust::math::wrapRad(data.rph[roll]/180*M_PI),
00236                                 labust::math::wrapRad(data.rph[pitch]/180*M_PI),
00237                                 labust::math::wrapRad(data.rph[yaw]/180*M_PI + magnetic_declination), quat);
00238                 transform.setRotation(tf::Quaternion(quat.x(), quat.y(), quat.z(), quat.w()));
00239                 broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "dvl_frame"));
00240         }
00241 
00242         //RPY
00243         auv_msgs::RPY::Ptr rpy(new auv_msgs::RPY());
00244         rpy->roll = labust::math::wrapRad(data.rph[roll]/180*M_PI);
00245         rpy->pitch = labust::math::wrapRad(data.rph[pitch]/180*M_PI);
00246         rpy->yaw = labust::math::wrapRad(data.rph[yaw]/180*M_PI);
00247         imuPub.publish(rpy);
00248 }
00249 
00250 void NavQuestNode::conditionDvlData(const NQRes& data)
00251 {}
00252 
00253 void NavQuestNode::onDvlData(const boost::system::error_code& e,
00254                 std::size_t size)
00255 {
00256         if (!e)
00257         {
00258                 std::istream is(&buffer);
00259                 std::string data(size,'\0');
00260                 is.read(&data[0],size);
00261 
00262                 if (test_header("$#NQ.RES", data))
00263                 {
00264                         NQRes dvl_data;
00265                         int chk = labust::tools::getChecksum(reinterpret_cast<unsigned char*>(&data[15]), data.size()-3-15);
00266                         std::istringstream is;
00267                         is.rdbuf()->pubsetbuf(&data[0],size);
00268                         labust::archive::delimited_iarchive ia(is);
00269                         ia>>dvl_data;
00270 
00271                         if (error_code(dvl_data) == 0) publishDvlData(dvl_data);
00272                         ROS_INFO("Calculated checksum:calc=%d, recvd=%d", chk, dvl_data.checksum);
00273 
00274                         //ROS_INFO("DVL decoded: header:%s, error:%s.",
00275                         //              dvl_data.header.c_str(),
00276                         //              dvl_data.error_code.c_str());
00277                 }
00278         }
00279         else
00280         {
00281                 ROS_ERROR("NavQuestNode: %s",e.message().c_str());
00282         }
00283         this->start_receive();
00284 }
00285 
00286 
00287 int main(int argc, char* argv[])
00288 {
00289         ros::init(argc,argv,"navquest_node");
00290         NavQuestNode node;
00291         ros::spin();
00292 
00293         return 0;
00294 }
00295 
00296 


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