From 7201726dbe3a4302e9d56b12cfce7b5e2070fe4b Mon Sep 17 00:00:00 2001 From: ipa-fmw Date: Thu, 16 Jan 2014 09:49:40 +0100 Subject: [PATCH] standalone package for unified scan publisher (issue #18) --- cob_mapping_slam/launch/2dslam.xml | 2 +- cob_mapping_slam/manifest.xml | 1 + cob_unified_scan_publisher/CMakeLists.txt | 32 +++ cob_unified_scan_publisher/Makefile | 1 + cob_unified_scan_publisher/mainpage.dox | 14 ++ cob_unified_scan_publisher/manifest.xml | 19 ++ .../ros/src/cob_unified_scan_publisher.cpp | 195 ++++++++++++++++++ 7 files changed, 263 insertions(+), 1 deletion(-) create mode 100644 cob_unified_scan_publisher/CMakeLists.txt create mode 100644 cob_unified_scan_publisher/Makefile create mode 100644 cob_unified_scan_publisher/mainpage.dox create mode 100644 cob_unified_scan_publisher/manifest.xml create mode 100644 cob_unified_scan_publisher/ros/src/cob_unified_scan_publisher.cpp diff --git a/cob_mapping_slam/launch/2dslam.xml b/cob_mapping_slam/launch/2dslam.xml index 63cbfce8..90433c8a 100644 --- a/cob_mapping_slam/launch/2dslam.xml +++ b/cob_mapping_slam/launch/2dslam.xml @@ -2,7 +2,7 @@ - + diff --git a/cob_mapping_slam/manifest.xml b/cob_mapping_slam/manifest.xml index 5c04df82..f01c897d 100644 --- a/cob_mapping_slam/manifest.xml +++ b/cob_mapping_slam/manifest.xml @@ -11,6 +11,7 @@ + diff --git a/cob_unified_scan_publisher/CMakeLists.txt b/cob_unified_scan_publisher/CMakeLists.txt new file mode 100644 index 00000000..4b09f078 --- /dev/null +++ b/cob_unified_scan_publisher/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) + +rosbuild_add_executable(cob_unified_scan_publisher ros/src/cob_unified_scan_publisher.cpp) diff --git a/cob_unified_scan_publisher/Makefile b/cob_unified_scan_publisher/Makefile new file mode 100644 index 00000000..b75b928f --- /dev/null +++ b/cob_unified_scan_publisher/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/cob_unified_scan_publisher/mainpage.dox b/cob_unified_scan_publisher/mainpage.dox new file mode 100644 index 00000000..a7f5b210 --- /dev/null +++ b/cob_unified_scan_publisher/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b cob_unified_scan_publisher + + + +--> + + +*/ diff --git a/cob_unified_scan_publisher/manifest.xml b/cob_unified_scan_publisher/manifest.xml new file mode 100644 index 00000000..fc0cb059 --- /dev/null +++ b/cob_unified_scan_publisher/manifest.xml @@ -0,0 +1,19 @@ + + + + cob_unified_scan_publisher + + + Florian Mirus + LGPL + + http://ros.org/wiki/cob_unified_scan_publisher + + + + + + + + + diff --git a/cob_unified_scan_publisher/ros/src/cob_unified_scan_publisher.cpp b/cob_unified_scan_publisher/ros/src/cob_unified_scan_publisher.cpp new file mode 100644 index 00000000..46873efd --- /dev/null +++ b/cob_unified_scan_publisher/ros/src/cob_unified_scan_publisher.cpp @@ -0,0 +1,195 @@ +/**************************************************************** + * + * Copyright (c) 2010 + * + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Project name: care-o-bot + * ROS stack name: cob_driver + * ROS package name: cob_sick_s300 + * Description: + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de + * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de + * + * Date of creation: Jan 2011 + * ToDo: + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Fraunhofer Institute for Manufacturing + * Engineering and Automation (IPA) nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +//################## +//#### includes #### + +// standard includes +//-- + +// ROS includes +#include +#include +#include +#include +#include +#include + +// ROS message includes +#include + + +//#################### +//#### node class #### +class NodeClass +{ + // + public: + + ros::NodeHandle nodeHandle; + // topics to publish + message_filters::Subscriber * topicSub_LaserFront; + message_filters::Subscriber * topicSub_LaserBack; + message_filters::TimeSynchronizer *sync; + tf::TransformListener listener_; + laser_geometry::LaserProjection projector_; + ros::Publisher topicPub_LaserUnified; + + tf::StampedTransform transform_scan_front; + tf::StampedTransform transform_scan_back; + + + NodeClass() + { + // loading config + // implementation of topics to publish + topicPub_LaserUnified = nodeHandle.advertise("scan_unified", 1); + topicSub_LaserFront = new message_filters::Subscriber(nodeHandle, "/scan_front", 1); + + topicSub_LaserBack = new message_filters::Subscriber(nodeHandle, "/scan_rear", 1); + sync = new message_filters::TimeSynchronizer(*topicSub_LaserFront, *topicSub_LaserBack, 10); + sync->registerCallback(boost::bind(&NodeClass::scanCallback, this, _1, _2)); + + //listener_.lookupTransform(scan_front->header.frame_id, "base_link", ros::Time(0), transform_scan_front); + //listener_.lookupTransform(scan_rear->header.frame_id, "base_link", ros::Time(0), transform_scan_back); + + } + + void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_front, const sensor_msgs::LaserScan::ConstPtr& scan_rear) + { + // create LaserScan message + sensor_msgs::LaserScan laserUnified; + sensor_msgs::PointCloud cloud_front; + //ROS_INFO("Converting front scan to point cloud"); + listener_.waitForTransform("/base_link", scan_front->header.frame_id, scan_front->header.stamp, ros::Duration(15.0)); + projector_.transformLaserScanToPointCloud("/base_link",*scan_front, cloud_front, listener_); + sensor_msgs::PointCloud cloud_rear; + //ROS_INFO("Converting rear scan to point cloud"); + listener_.waitForTransform("/base_link", scan_rear->header.frame_id, scan_rear->header.stamp, ros::Duration(15.0)); + projector_.transformLaserScanToPointCloud("/base_link",*scan_rear, cloud_rear, listener_); + + //ROS_INFO("Creating message header"); + laserUnified.header = scan_front->header; + laserUnified.header.frame_id = "base_link"; + laserUnified.angle_min = -M_PI+0.1; + laserUnified.angle_max = M_PI-0.1; + laserUnified.angle_increment = M_PI/180.0/2.0; + laserUnified.time_increment = 0.0; + laserUnified.scan_time = scan_front->scan_time; + laserUnified.range_min = scan_front->range_min; + laserUnified.range_max = scan_front->range_max; + laserUnified.ranges.resize((laserUnified.angle_max - laserUnified.angle_min) / laserUnified.angle_increment); + //ROS_INFO("Converting scan from point cloud"); + for (unsigned int i = 0; i < cloud_front.points.size(); i++) + { + const float &x = cloud_front.points[i].x; + const float &y = cloud_front.points[i].y; + const float &z = cloud_front.points[i].z; + //ROS_INFO("Point %f %f %f", x, y, z); + if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ) + { + ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); + continue; + } + double angle = atan2(y, x);// + M_PI/2; + if (angle < laserUnified.angle_min || angle > laserUnified.angle_max) + { + ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, laserUnified.angle_min, laserUnified.angle_max); + continue; + } + int index = (angle - laserUnified.angle_min) / laserUnified.angle_increment; + double range_sq = y*y+x*x; + //printf ("index xyz( %f %f %f) angle %f index %d range %f\n", x, y, z, angle, index, sqrt(range_sq)); + laserUnified.ranges[index] = sqrt(range_sq); + } + for (unsigned int i = 0; i < cloud_rear.points.size(); i++) + { + const float &x = cloud_rear.points[i].x; + const float &y = cloud_rear.points[i].y; + const float &z = cloud_rear.points[i].z; + //ROS_INFO("Point %f %f %f", x, y, z); + if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ) + { + ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); + continue; + } + double angle = atan2(y, x); //+ M_PI/2; //TODO: no ideas why but it works + + if (angle < laserUnified.angle_min || angle > laserUnified.angle_max) + { + ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, laserUnified.angle_min, laserUnified.angle_max); + continue; + } + int index = (angle - laserUnified.angle_min) / laserUnified.angle_increment; + double range_sq = y*y+x*x; + //printf ("index xyz( %f %f %f) angle %f index %d range %f\n", x, y, z, angle, index, sqrt(range_sq)); + laserUnified.ranges[index] = sqrt(range_sq); + } + + topicPub_LaserUnified.publish(laserUnified); + } +}; + +//####################### +//#### main programm #### +int main(int argc, char** argv) +{ + // initialize ROS, spezify name of node + ros::init(argc, argv, "scanner_unifier"); + + NodeClass nc; + + ros::spin(); + return 0; +} +