s3000_laser.cc
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  This program uses the sicks3000  component to get laser scans, and then
00004 *  publishes them as ROS messages
00005 *
00006 *  Copyright (c) 2012, Robotnik Automation, SLL
00007 *
00008 *
00009 *   This program is free software: you can redistribute it and/or modify
00010 *   it under the terms of the GNU General Public License as published by
00011 *   the Free Software Foundation, either version 3 of the License, or
00012 *   (at your option) any later version.
00013 *
00014 *   This program is distributed in the hope that it will be useful,
00015 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017 *   GNU General Public License for more details.
00018 *
00019 *   You should have received a copy of the GNU General Public License
00020 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
00021 *********************************************************************/
00022 
00023 #include <assert.h>
00024 #include <math.h>
00025 #include <iostream>
00026 #include <boost/format.hpp>
00027 
00028 #include "s3000_laser/sicks3000.h"   // s3000 driver from player (Toby Collet / Andrew Howard)
00029 #include "ros/time.h"
00030 #include "self_test/self_test.h"
00031 #include "diagnostic_msgs/DiagnosticStatus.h"
00032 #include "diagnostic_updater/diagnostic_updater.h"
00033 #include "diagnostic_updater/update_functions.h"
00034 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00035 
00036 #include "sensor_msgs/LaserScan.h"
00037 
00038 #include "tf/transform_broadcaster.h"
00039 #include "s3000_laser/enable_disable.h"
00040 
00041 using namespace std;
00042 
00043 class s3000node {
00044 
00045 private:
00046         SickS3000 *laser; 
00047         sensor_msgs::LaserScan reading;
00048 
00049         string port;
00050 
00051         self_test::TestRunner self_test_;
00052         diagnostic_updater::Updater diagnostic_;
00053 
00054         ros::NodeHandle node_handle_;
00055         ros::NodeHandle private_node_handle_;
00056         ros::Publisher laser_data_pub_;
00057 
00059         bool publish_tf_;
00060         tf::TransformBroadcaster laser_broadcaster;
00062         bool publish_scan_;     
00063 
00064         bool running;
00065 
00066         int error_count_;
00067         int slow_count_;
00068         std::string was_slow_;
00069         std::string error_status_;
00071         string frame_id_;
00072 
00073         double desired_freq_;
00074         diagnostic_updater::FrequencyStatus freq_diag_;
00076         ros::ServiceServer enable_disable_srv_;
00077 
00078 public:
00079         
00080         s3000node(ros::NodeHandle h) : self_test_(), diagnostic_(), 
00081         node_handle_(h), private_node_handle_("~"), 
00082         error_count_(0), 
00083         slow_count_(0), 
00084         desired_freq_(20), 
00085         freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05))
00086         {
00087                 ros::NodeHandle laser_node_handle(node_handle_, "s3000_laser");
00088                 private_node_handle_.param("port", port, string("/dev/ttyUSB1"));
00089 
00090                 private_node_handle_.param<bool> ("publish_tf", publish_tf_, false);
00091                 private_node_handle_.param<bool> ("publish_scan", publish_scan_, true);
00092                 laser_data_pub_ = laser_node_handle.advertise<sensor_msgs::LaserScan>("/scan", 100);
00093                 running = false;
00094                 private_node_handle_.param("frame_id", frame_id_, string("/laser"));
00095                 reading.header.frame_id = frame_id_;
00096                         
00097                 self_test_.add("Connect Test", this, &s3000node::ConnectTest);
00098 
00099                 diagnostic_.add( freq_diag_ );
00100                 diagnostic_.add( "Laser S3000 Status", this, &s3000node::deviceStatus );
00101                 ROS_INFO("s3000node: Port = %s", port.c_str());
00102                 
00103                 // Advertises new service to enable/disable the scan publication
00104                 enable_disable_srv_ = private_node_handle_.advertiseService("enable_disable",  &s3000node::EnableDisable, this);
00105                 
00106                 // Create SickS3000 in the given port
00107                 laser = new SickS3000( port );
00108         }
00109 
00110         ~s3000node()
00111         {
00112                 stop();
00113         }
00114         
00115         bool spin(){
00116 
00117                 // sensor_msgs::LaserScan data;
00118                 ros::Rate r(desired_freq_);
00119 
00120                 while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
00121                 {
00122                         if (start() == 0)
00123                         {
00124                                 while(node_handle_.ok()) {
00125                                         getData( reading );           
00126 
00127                                         self_test_.checkTest();
00128                                         diagnostic_.update();
00129                                         ros::spinOnce();
00130                                         r.sleep();          
00131                                 }
00132                         } else {
00133                                 // No need for diagnostic here since a broadcast occurs in start
00134                                 // when there is an error.
00135                                 usleep(1000000);
00136                                 self_test_.checkTest();
00137                                 ros::spinOnce();
00138                         }
00139                 }
00140 
00141                 stop();
00142 
00143                 return true;
00144         }
00145 
00146 private:
00147 
00148         int start()
00149         {
00150                 stop();
00151                 laser->Open();
00152                 diagnostic_.setHardwareID("Laser Ranger");
00153                 ROS_INFO("Laser Ranger sensor initialized.");
00154                 freq_diag_.clear();
00155                 running = true;
00156                 return(0);
00157         }
00158 
00159 
00160         int stop()
00161         {
00162                 if(running)
00163                 {
00164                         laser->Close();
00165                         running = false;
00166                 }
00167 
00168                 return(0);
00169         }
00170 
00171 
00172         void ConnectTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00173         {
00174                 laser->Open();
00175                 status.summary(0, "Connected successfully.");
00176         }
00177         
00179         void getData(sensor_msgs::LaserScan& data)
00180         {
00181                 bool bValidData = false;
00182 
00183                 laser->ReadLaser( data, bValidData );
00184 
00186                 if (bValidData) {
00187                         data.header.stamp = ros::Time::now();
00188                         data.header.frame_id = frame_id_;
00189                         if(publish_scan_)       // Publishes only if enabled
00190                                 laser_data_pub_.publish( data );
00191                         freq_diag_.tick();
00192                 }
00193 
00194                 /*
00195                 // Robotnik - to test unit
00196                 if (publish_tf_) {
00197                 // create a tf message
00198                 geometry_msgs::TransformStamped imu_trans;
00199                 imu_trans.header.stamp = data.header.stamp;
00200                 imu_trans.header.frame_id = "laser";   //odom_frame_id.c_str();
00201                 imu_trans.child_frame_id = "base_link";
00202                 imu_trans.transform.translation.x = 0.0;
00203                 imu_trans.transform.translation.y = 0.0;
00204                 imu_trans.transform.translation.z = 0.0;
00205                 imu_trans.transform.rotation = data.orientation;
00206                 //send the transform
00207                 imu_broadcaster.sendTransform(imu_trans);
00208                 }
00209                 */  
00210 
00211         }
00212 
00214         void deviceStatus(diagnostic_updater::DiagnosticStatusWrapper &status)
00215         {
00216                 if (!running)
00217                         status.summary(2, "Laser Ranger is stopped");
00218                 else if (!was_slow_.empty())
00219                 {
00220                         status.summary(1, "Excessive delay");
00221                         was_slow_.clear();
00222                 }
00223                 else{
00224                         
00225                         status.summary(0, "Laser Ranger is running");
00226                         
00227                 }
00228                 status.add("Device", port);
00229                 status.add("TF frame", frame_id_);
00230                 status.add("Error count", error_count_);
00231                 status.add("Excessive delay", slow_count_);
00232                 status.add("Scan publication", publish_scan_);
00233         }
00234 
00236         bool EnableDisable(s3000_laser::enable_disable::Request &req, s3000_laser::enable_disable::Response &res ){
00237                 publish_scan_ = req.value;
00238 
00239                 ROS_INFO("s3000_node::EnablaDisable: Setting scan publish to %d", req.value);
00240                 res.ret = true;
00241                 
00242                 return true;
00243         }
00244 };
00245 
00246 
00247 int main(int argc, char** argv)
00248 {
00249         ros::init(argc, argv, "s3000_node");
00250 
00251         ros::NodeHandle nh;
00252 
00253         s3000node s3000n(nh);
00254         s3000n.spin();
00255 
00256         return(0);
00257 }


s3000_laser
Author(s): Román Navarro
autogenerated on Mon Oct 6 2014 07:26:44