r2000_node.cpp
Go to the documentation of this file.
00001 // Copyright (c) 2014, Pepperl+Fuchs GmbH, Mannheim
00002 // Copyright (c) 2014, Denis Dillenberger
00003 // All rights reserved.
00004 //
00005 // Redistribution and use in source and binary forms, with or without modification,
00006 // are permitted provided that the following conditions are met:
00007 //
00008 // * Redistributions of source code must retain the above copyright notice, this
00009 //   list of conditions and the following disclaimer.
00010 //
00011 // * Redistributions in binary form must reproduce the above copyright notice, this
00012 //   list of conditions and the following disclaimer in the documentation and/or
00013 //   other materials provided with the distribution.
00014 //
00015 // * Neither the name of Pepperl+Fuchs nor the names of its
00016 //   contributors may be used to endorse or promote products derived from
00017 //   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 THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
00023 // ANY 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 ON
00026 // 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 #include "r2000_node.h"
00031 #include <sensor_msgs/LaserScan.h>
00032 #include <pepperl_fuchs_r2000/r2000_driver.h>
00033 
00034 namespace pepperl_fuchs {
00035 
00036 //-----------------------------------------------------------------------------
00037 R2000Node::R2000Node():nh_("~")
00038 {
00039     driver_ = 0;
00040     // Reading and checking parameters
00041     //-------------------------------------------------------------------------
00042     nh_.param("frame_id", frame_id_, std::string("/scan"));
00043     nh_.param("scanner_ip",scanner_ip_,std::string(""));
00044     nh_.param("scan_frequency",scan_frequency_,35);
00045     nh_.param("samples_per_scan",samples_per_scan_,3600);
00046 
00047     if( scanner_ip_ == "" )
00048     {
00049         std::cerr << "IP of laser range finder not set!" << std::endl;
00050         return;
00051     }
00052 
00053     if( !connect() )
00054         return;
00055 
00056     // Declare publisher and create timer
00057     //-------------------------------------------------------------------------
00058     scan_publisher_ = nh_.advertise<sensor_msgs::LaserScan>("scan",100);
00059     cmd_subscriber_ = nh_.subscribe("control_command",100,&R2000Node::cmdMsgCallback,this);
00060     get_scan_data_timer_ = nh_.createTimer(ros::Duration(1/(2*std::atof(driver_->getParametersCached().at("scan_frequency").c_str()))), &R2000Node::getScanData, this);
00061 }
00062 
00063 //-----------------------------------------------------------------------------
00064 bool R2000Node::connect()
00065 {
00066     delete driver_;
00067 
00068     // Connecting to laser range finder
00069     //-------------------------------------------------------------------------
00070     driver_ = new R2000Driver();
00071     std::cout << "Connecting to scanner at " << scanner_ip_ << " ... ";
00072     if( driver_->connect(scanner_ip_,80) )
00073         std::cout << "OK" << std::endl;
00074     else
00075     {
00076         std::cout << "FAILED!" << std::endl;
00077         std::cerr << "Connection to scanner at " << scanner_ip_ << " failed!" << std::endl;
00078         return false;
00079     }
00080 
00081     // Setting, reading and displaying parameters
00082     //-------------------------------------------------------------------------
00083     driver_->setScanFrequency(scan_frequency_);
00084     driver_->setSamplesPerScan(samples_per_scan_);
00085     auto params = driver_->getParameters();
00086     std::cout << "Current scanner settings:" << std::endl;
00087     std::cout << "============================================================" << std::endl;
00088     for( const auto& p : params )
00089         std::cout << p.first << " : " << p.second << std::endl;
00090     std::cout << "============================================================" << std::endl;
00091 
00092     // Start capturing scanner data
00093     //-------------------------------------------------------------------------
00094     std::cout << "Starting capturing: ";
00095     if( driver_->startCapturingTCP() )
00096         std::cout << "OK" << std::endl;
00097     else
00098     {
00099         std::cout << "FAILED!" << std::endl;
00100         return false;
00101     }
00102     return true;
00103 }
00104 
00105 //-----------------------------------------------------------------------------
00106 void R2000Node::getScanData(const ros::TimerEvent &e)
00107 {
00108     if( !driver_->isCapturing() )
00109     {
00110         std::cout << "ERROR: Laser range finder disconnected. Trying to reconnect..." << std::endl;
00111         while( !connect() )
00112         {
00113             std::cout << "ERROR: Reconnect failed. Trying again in 2 seconds..." << std::endl;
00114             usleep((2*1000000));
00115         }
00116     }
00117     auto scandata = driver_->getFullScan();
00118     if( scandata.amplitude_data.empty() || scandata.distance_data.empty() )
00119         return;
00120 
00121     sensor_msgs::LaserScan scanmsg;
00122     scanmsg.header.frame_id = frame_id_;
00123     scanmsg.header.stamp = ros::Time::now();
00124 
00125     scanmsg.angle_min = -M_PI;
00126     scanmsg.angle_max = +M_PI;
00127     scanmsg.angle_increment = 2*M_PI/float(scandata.distance_data.size());
00128     scanmsg.time_increment = 1/35.0f/float(scandata.distance_data.size());
00129 
00130     scanmsg.scan_time = 1/std::atof(driver_->getParametersCached().at("scan_frequency").c_str());
00131     scanmsg.range_min = std::atof(driver_->getParametersCached().at("radial_range_min").c_str());
00132     scanmsg.range_max = std::atof(driver_->getParametersCached().at("radial_range_max").c_str());
00133 
00134     scanmsg.ranges.resize(scandata.distance_data.size());
00135     scanmsg.intensities.resize(scandata.amplitude_data.size());
00136     for( std::size_t i=0; i<scandata.distance_data.size(); i++ )
00137     {
00138         scanmsg.ranges[i] = float(scandata.distance_data[i])/1000.0f;
00139         scanmsg.intensities[i] = scandata.amplitude_data[i];
00140     }
00141     scan_publisher_.publish(scanmsg);
00142 }
00143 
00144 //-----------------------------------------------------------------------------
00145 void R2000Node::cmdMsgCallback(const std_msgs::StringConstPtr &msg)
00146 {
00147     const std::string& cmd = msg->data;
00148     static const std::string set_scan_frequency_cmd("set scan_frequency=");
00149     static const std::string set_samples_per_scan_cmd("set samples_per_scan=");
00150 
00151     // Setting of scan_frequency
00152     //-------------------------------------------------------------------------
00153     if( cmd.substr(0,set_scan_frequency_cmd.size()) == set_scan_frequency_cmd )
00154     {
00155         std::string value = cmd.substr(set_scan_frequency_cmd.size());
00156         int frequency = std::atoi(value.c_str());
00157         if(frequency>=10 && frequency<=50)
00158         {
00159             scan_frequency_ = frequency;
00160             driver_->setScanFrequency(frequency);
00161         }
00162     }
00163 
00164     // Setting of samples_per_scan
00165     //-------------------------------------------------------------------------
00166     if( cmd.substr(0,set_samples_per_scan_cmd.size()) == set_samples_per_scan_cmd )
00167     {
00168         std::string value = cmd.substr(set_samples_per_scan_cmd.size());
00169         int samples = std::atoi(value.c_str());
00170         if(samples>=72 && samples<=25200)
00171         {
00172             samples_per_scan_ = samples;
00173             driver_->setSamplesPerScan(samples);
00174         }
00175     }
00176 }
00177 
00178 //-----------------------------------------------------------------------------
00179 } // NS
00180 
00181 int main(int argc, char **argv)
00182 {
00183     ros::init(argc, argv, "r2000_node", ros::init_options::AnonymousName);
00184     new pepperl_fuchs::R2000Node();
00185     ros::spin();
00186     return 0;
00187 }


pepperl_fuchs_r2000
Author(s): Denis Dillenberger
autogenerated on Wed Aug 26 2015 15:22:22