$search
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: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de 00018 * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de 00019 * 00020 * Date of creation: June 2011 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 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h> 00066 00067 00068 //#################### 00069 //#### node class #### 00070 class NodeClass 00071 { 00072 // 00073 public: 00074 int start_left_scan, stop_left_scan, start_right_scan, stop_right_scan; 00075 int start_tray_filter, stop_tray_filter; 00076 double tray_filter_min_angle, tray_filter_max_angle; 00077 bool bFilterTray_; 00078 00079 ros::NodeHandle nodeHandle; 00080 // topics to publish 00081 ros::Subscriber topicSub_LaserScan_raw; 00082 ros::Subscriber topicSub_Tray; 00083 ros::Publisher topicPub_LaserScan; 00084 ros::Publisher topicPub_LaserScan_self; 00085 00086 NodeClass() 00087 { 00088 // loading config 00089 nodeHandle.param<int>("start_left_scan", start_left_scan, 0); 00090 nodeHandle.param<int>("stop_left_scan", stop_left_scan, 248); 00091 nodeHandle.param<int>("start_right_scan", start_right_scan, 442); 00092 nodeHandle.param<int>("stop_right_scan", stop_right_scan, 681); 00093 nodeHandle.param<int>("start_tray_filter", start_tray_filter, 442); 00094 nodeHandle.param<int>("stop_tray_filter", stop_tray_filter, 520); 00095 nodeHandle.param<double>("tray_filter_min_angle", tray_filter_min_angle, -2.941); 00096 nodeHandle.param<double>("tray_filter_max_angle", tray_filter_max_angle, -1.1431); 00097 // implementation of topics to publish 00098 topicPub_LaserScan = nodeHandle.advertise<sensor_msgs::LaserScan>("scan_top_filtered", 1); 00099 topicPub_LaserScan_self = nodeHandle.advertise<sensor_msgs::LaserScan>("scan_top_self_filtered", 1); 00100 topicSub_LaserScan_raw = nodeHandle.subscribe("scan_top", 1, &NodeClass::scanCallback, this); 00101 topicSub_Tray = nodeHandle.subscribe("/tray_controller/state", 1, &NodeClass::trayCallback, this); 00102 bFilterTray_ = false; 00103 } 00104 00105 00106 void trayCallback(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg) 00107 { 00108 if(msg->actual.positions[0] > tray_filter_min_angle and msg->actual.positions[0] < tray_filter_max_angle) 00109 bFilterTray_ = true; 00110 else 00111 bFilterTray_ = false; 00112 } 00113 00114 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) 00115 { 00116 // create LaserScan message 00117 sensor_msgs::LaserScan laserScan; 00118 laserScan.header.stamp = msg->header.stamp; 00119 00120 // fill message 00121 laserScan.header.frame_id = msg->header.frame_id; 00122 laserScan.angle_increment = msg->angle_increment; 00123 laserScan.range_min = msg->range_min; //TODO read from ini-file/parameter-file 00124 laserScan.range_max = msg->range_max; //TODO read from ini-file/parameter-file 00125 laserScan.time_increment = msg->time_increment; //TODO read from ini-file/parameter-file 00126 00127 // rescale scan 00128 int num_readings = (stop_right_scan - start_left_scan); 00129 laserScan.angle_min = msg->angle_min; // first ScanAngle 00130 laserScan.angle_max = msg->angle_max; // last ScanAngle 00131 laserScan.ranges.resize(num_readings); 00132 laserScan.intensities.resize(0); 00133 00134 for(int i = 0; i < (stop_left_scan - start_left_scan); i++) 00135 { 00136 laserScan.ranges[i] = msg->ranges[start_left_scan + i]; 00137 } 00138 for(int i = stop_left_scan+1; i < start_right_scan; i++) 00139 { 00140 laserScan.ranges[i] = 0.0; 00141 } 00142 for(int i = start_right_scan; i < stop_right_scan; i++) 00143 { 00144 laserScan.ranges[i] = msg->ranges[i]; 00145 } 00146 00147 sensor_msgs::LaserScan laserScan_self; 00148 laserScan_self = laserScan; 00149 if(bFilterTray_) 00150 { 00151 for(int i = start_tray_filter; i < stop_tray_filter; i++) 00152 { 00153 laserScan_self.ranges[i] = 0.0; 00154 } 00155 } 00156 // publish message 00157 topicPub_LaserScan.publish(laserScan); 00158 topicPub_LaserScan_self.publish(laserScan_self); 00159 00160 } 00161 }; 00162 00163 //####################### 00164 //#### main programm #### 00165 int main(int argc, char** argv) 00166 { 00167 // initialize ROS, spezify name of node 00168 ros::init(argc, argv, "hokuyo_filter"); 00169 00170 NodeClass nc; 00171 00172 ros::spin(); 00173 return 0; 00174 } 00175