00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2010 00004 * 00005 * Fraunhofer Institute for Manufacturing Engineering 00006 * and Automation (IPA) 00007 * 00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 * 00010 * Project name: care-o-bot 00011 * ROS stack name: cob_driver 00012 * ROS package name: cob_sick_s300 00013 * Description: 00014 * 00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 * 00017 * Author: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de 00018 * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de 00019 * 00020 * Date of creation: Jan 2010 00021 * ToDo: 00022 * 00023 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00024 * 00025 * Redistribution and use in source and binary forms, with or without 00026 * modification, are permitted provided that the following conditions are met: 00027 * 00028 * * Redistributions of source code must retain the above copyright 00029 * notice, this list of conditions and the following disclaimer. 00030 * * Redistributions in binary form must reproduce the above copyright 00031 * notice, this list of conditions and the following disclaimer in the 00032 * documentation and/or other materials provided with the distribution. 00033 * * Neither the name of the Fraunhofer Institute for Manufacturing 00034 * Engineering and Automation (IPA) nor the names of its 00035 * contributors may be used to endorse or promote products derived from 00036 * this software without specific prior written permission. 00037 * 00038 * This program is free software: you can redistribute it and/or modify 00039 * it under the terms of the GNU Lesser General Public License LGPL as 00040 * published by the Free Software Foundation, either version 3 of the 00041 * License, or (at your option) any later version. 00042 * 00043 * This program is distributed in the hope that it will be useful, 00044 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00045 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00046 * GNU Lesser General Public License LGPL for more details. 00047 * 00048 * You should have received a copy of the GNU Lesser General Public 00049 * License LGPL along with this program. 00050 * If not, see <http://www.gnu.org/licenses/>. 00051 * 00052 ****************************************************************/ 00053 00054 //################## 00055 //#### includes #### 00056 00057 // standard includes 00058 //-- 00059 00060 // ROS includes 00061 #include <ros/ros.h> 00062 00063 // ROS message includes 00064 #include <sensor_msgs/LaserScan.h> 00065 00066 00067 //#################### 00068 //#### node class #### 00069 class NodeClass 00070 { 00071 // 00072 public: 00073 int start_scan, stop_scan; 00074 00075 ros::NodeHandle nodeHandle; 00076 // topics to publish 00077 ros::Subscriber topicSub_LaserScan_raw; 00078 ros::Publisher topicPub_LaserScan; 00079 00080 NodeClass() 00081 { 00082 // loading config 00083 nodeHandle.param<int>("start_scan", start_scan, 115); 00084 nodeHandle.param<int>("stop_scan", stop_scan, 426); 00085 // implementation of topics to publish 00086 topicPub_LaserScan = nodeHandle.advertise<sensor_msgs::LaserScan>("scan_filtered", 1); 00087 topicSub_LaserScan_raw = nodeHandle.subscribe("scan", 1, &NodeClass::scanCallback, this); 00088 } 00089 00090 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) 00091 { 00092 // create LaserScan message 00093 sensor_msgs::LaserScan laserScan; 00094 laserScan.header.stamp = msg->header.stamp; 00095 00096 // fill message 00097 laserScan.header.frame_id = msg->header.frame_id; 00098 laserScan.angle_increment = msg->angle_increment; 00099 laserScan.range_min = 0.0; //TODO read from ini-file/parameter-file 00100 laserScan.range_max = 100.0; //TODO read from ini-file/parameter-file 00101 laserScan.time_increment = msg->time_increment; //TODO read from ini-file/parameter-file 00102 00103 // rescale scan 00104 int num_readings = stop_scan - start_scan; 00105 laserScan.angle_min = (-135.0/180.0*3.14) + laserScan.angle_increment * start_scan; // first ScanAngle 00106 laserScan.angle_max = (-135.0/180.0*3.14) + laserScan.angle_increment * stop_scan; // l ast ScanAngle 00107 laserScan.set_ranges_size(num_readings); 00108 laserScan.set_intensities_size(num_readings); 00109 00110 for(int i = 0; i < (stop_scan - start_scan); i++) 00111 { 00112 laserScan.ranges[i] = msg->ranges[start_scan + i]; 00113 laserScan.intensities[i] = msg->intensities[start_scan + i]; 00114 } 00115 00116 // publish message 00117 topicPub_LaserScan.publish(laserScan); 00118 00119 } 00120 }; 00121 00122 //####################### 00123 //#### main programm #### 00124 int main(int argc, char** argv) 00125 { 00126 // initialize ROS, spezify name of node 00127 ros::init(argc, argv, "scanner_filter"); 00128 00129 NodeClass nc; 00130 00131 ros::spin(); 00132 return 0; 00133 } 00134