node.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #include <boost/shared_ptr.hpp>
00033 #include "comm.h"
00034 #include "hl_interface.h"
00035 #include "ssdk_interface.h"
00036 #include "ekf_interface.h"
00037 
00038 void printTopicInfo()
00039 {
00040   //  print published/subscribed topics
00041   std::string nodeName = ros::this_node::getName();
00042   ros::V_string topics;
00043   ros::this_node::getSubscribedTopics(topics);
00044   std::string topicsStr = nodeName + ":\n\tsubscribed to topics:\n";
00045   for (unsigned int i = 0; i < topics.size(); i++)
00046     topicsStr += ("\t\t" + topics.at(i) + "\n");
00047 
00048   topicsStr += "\tadvertised topics:\n";
00049   ros::this_node::getAdvertisedTopics(topics);
00050   for (unsigned int i = 0; i < topics.size(); i++)
00051     topicsStr += ("\t\t" + topics.at(i) + "\n");
00052 
00053   ROS_INFO_STREAM(""<< topicsStr);
00054 }
00055 
00056 class Watchdog{
00057 private:
00058   CommPtr & comm_;
00059   const double PERIOD;
00060   ros::Timer timer;
00061   uint32_t last_rx_count_;
00062   uint32_t timeout_count_;
00063   void check(const ros::TimerEvent & e)
00064   {
00065     if(last_rx_count_ == comm_->getRxPacketsGood()){
00066       timeout_count_++;
00067       ROS_WARN("No new valid packets within the last %f s",PERIOD);
00068     }
00069 
00070     if(timeout_count_ > 4){
00071       // a bit harsh, but: roslaunch will restart if "respawn" is set true, and no idea what would happen with all the static local variables :(( --> TODO: remove those!
00072       ROS_FATAL("No valid packets within the last %f s, aborting !", 5*PERIOD);
00073       ros::Duration(0.1).sleep(); //sleep a bit such that the above message will still get transmitted
00074       ros::shutdown();
00075     }
00076 
00077     last_rx_count_ = comm_->getRxPacketsGood();
00078   }
00079 
00080 public:
00081   Watchdog(CommPtr & comm):comm_(comm), PERIOD(1), last_rx_count_(0), timeout_count_(0)
00082   {
00083     ROS_INFO("creating watchdog monitoring successful rx packets");
00084     ros::NodeHandle nh;
00085     timer = nh.createTimer(ros::Duration(PERIOD), &Watchdog::check, this);
00086   }
00087 };
00088 
00089 int main(int argc, char ** argv)
00090 {
00091   ros::init(argc, argv, "fcu");
00092   ros::NodeHandle nh("fcu");
00093 
00094 
00095   std::string port, portRX, portTX;
00096   int baudrate;
00097   ros::NodeHandle pnh("~/fcu");
00098   CommPtr comm(new Comm);
00099 
00100   bool connected = false;
00101 
00102   pnh.param("baudrate", baudrate, HLI_DEFAULT_BAUDRATE);
00103 
00104   if (pnh.getParam("serial_port_rx", portRX) && pnh.getParam("serial_port_tx", portTX))
00105   {
00106     connected = comm->connect(portRX, portTX, baudrate);
00107   }
00108   else
00109   {
00110     pnh.param("serial_port", port, std::string("/dev/ttyUSB0"));
00111     connected = comm->connect(port, port, baudrate);
00112   }
00113 
00114   if (!connected)
00115   {
00116     ROS_ERROR("unable to connect");
00117     ros::Duration(2).sleep();
00118     return -1;
00119   }
00120 
00121   HLInterface asctecInterface(nh, comm);
00122   SSDKInterface ssdk_if(nh, comm);
00123   EKFInterface ekf_if(nh, comm);
00124 
00125   printTopicInfo();
00126 
00127   Watchdog wd(comm);
00128 
00129   ros::spin();
00130 
00131 
00132   return 0;
00133 }


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Thu Aug 27 2015 12:26:52