$search
00001 /* 00002 * Copyright (c) 2010, 2011, Ivan Dryanovski, William Morris 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the CCNY Robotics Lab nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include "laser_ortho_projector/laser_ortho_projector.h" 00031 00032 namespace scan_tools { 00033 00034 LaserOrthoProjector::LaserOrthoProjector (ros::NodeHandle nh, ros::NodeHandle nh_private): 00035 nh_(nh), 00036 nh_private_(nh_private) 00037 { 00038 ROS_INFO ("Starting LaserOrthoProjector"); 00039 00040 initialized_ = false; 00041 00042 // set initial orientation to 0 00043 00044 ortho_to_laser_.setIdentity(); 00045 00046 nan_point_.x = std::numeric_limits<float>::quiet_NaN(); 00047 nan_point_.y = std::numeric_limits<float>::quiet_NaN(); 00048 nan_point_.z = std::numeric_limits<float>::quiet_NaN(); 00049 00050 // **** parameters 00051 00052 if (!nh_private_.getParam ("fixed_frame", world_frame_)) 00053 world_frame_ = "/world"; 00054 if (!nh_private_.getParam ("base_frame", base_frame_)) 00055 base_frame_ = "/base_link"; 00056 if (!nh_private_.getParam ("ortho_frame", ortho_frame_)) 00057 ortho_frame_ = "/base_ortho"; 00058 if (!nh_private_.getParam ("publish_tf", publish_tf_)) 00059 publish_tf_ = false; 00060 if (!nh_private_.getParam ("use_pose", use_pose_)) 00061 use_pose_ = true; 00062 if (!nh_private_.getParam ("use_imu", use_imu_)) 00063 use_imu_ = false; 00064 00065 if (use_imu_ && use_pose_) 00066 ROS_FATAL("use_imu and use_pose params cannot both be true"); 00067 if (!use_imu_ && !use_pose_) 00068 ROS_FATAL("use_imu and use_pose params cannot both be false"); 00069 00070 00071 // **** subscribe to laser scan messages 00072 00073 scan_subscriber_ = nh_.subscribe( 00074 "scan", 10, &LaserOrthoProjector::scanCallback, this); 00075 00076 if (use_pose_) 00077 { 00078 pose_subscriber_ = nh_.subscribe( 00079 "pose", 10, &LaserOrthoProjector::poseCallback, this); 00080 } 00081 if (use_imu_) 00082 { 00083 imu_subscriber_ = nh_.subscribe( 00084 "imu/data", 10, &LaserOrthoProjector::imuCallback, this); 00085 } 00086 00087 // **** advertise orthogonal scan 00088 00089 cloud_publisher_ = nh_.advertise<PointCloudT>( 00090 "cloud_ortho", 10); 00091 } 00092 00093 LaserOrthoProjector::~LaserOrthoProjector() 00094 { 00095 00096 } 00097 00098 void LaserOrthoProjector::imuCallback(const ImuMsg::ConstPtr& imu_msg) 00099 { 00100 // obtain world to base frame transform from the pose message 00101 tf::Transform world_to_base; 00102 world_to_base.setIdentity(); 00103 00104 tf::Quaternion q; 00105 tf::quaternionMsgToTF(imu_msg->orientation, q); 00106 world_to_base.setRotation(q); 00107 00108 // calculate world to ortho frame transform 00109 tf::Transform world_to_ortho; 00110 getOrthoTf(world_to_base, world_to_ortho); 00111 00112 if (publish_tf_) 00113 { 00114 tf::StampedTransform world_to_ortho_tf( 00115 world_to_ortho, imu_msg->header.stamp, world_frame_, ortho_frame_); 00116 tf_broadcaster_.sendTransform(world_to_ortho_tf); 00117 } 00118 00119 // calculate ortho to laser tf, and save it for when scans arrive 00120 ortho_to_laser_ = world_to_ortho.inverse() * world_to_base * base_to_laser_; 00121 } 00122 00123 void LaserOrthoProjector::poseCallback(const PoseMsg::ConstPtr& pose_msg) 00124 { 00125 // obtain world to base frame transform from the pose message 00126 tf::Transform world_to_base; 00127 tf::poseMsgToTF(pose_msg->pose, world_to_base); 00128 00129 // calculate world to ortho frame transform 00130 tf::Transform world_to_ortho; 00131 getOrthoTf(world_to_base, world_to_ortho); 00132 00133 if (publish_tf_) 00134 { 00135 tf::StampedTransform world_to_ortho_tf( 00136 world_to_ortho, pose_msg->header.stamp, world_frame_, ortho_frame_); 00137 tf_broadcaster_.sendTransform(world_to_ortho_tf); 00138 } 00139 00140 // calculate ortho to laser tf, and save it for when scans arrive 00141 ortho_to_laser_ = world_to_ortho.inverse() * world_to_base * base_to_laser_; 00142 } 00143 00144 void LaserOrthoProjector::getOrthoTf(const tf::Transform& world_to_base, tf::Transform& world_to_ortho) 00145 { 00146 const tf::Vector3& w2b_o = world_to_base.getOrigin(); 00147 const tf::Quaternion& w2b_q = world_to_base.getRotation(); 00148 00149 tf::Vector3 wto_o(w2b_o.getX(), w2b_o.getY(), 0.0); 00150 tf::Quaternion wto_q = tf::createQuaternionFromYaw(tf::getYaw(w2b_q)); 00151 00152 world_to_ortho.setOrigin(wto_o); 00153 world_to_ortho.setRotation(wto_q); 00154 } 00155 00156 void LaserOrthoProjector::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg) 00157 { 00158 if(!initialized_) 00159 { 00160 initialized_ = getBaseToLaserTf(scan_msg); 00161 00162 if (initialized_) createCache(scan_msg); 00163 else return; 00164 } 00165 00166 if(!use_pose_) 00167 { 00168 // obtain transform between fixed and base frame 00169 tf::StampedTransform world_to_base_tf; 00170 try 00171 { 00172 tf_listener_.waitForTransform ( 00173 world_frame_, base_frame_, scan_msg->header.stamp, ros::Duration(0.1)); 00174 tf_listener_.lookupTransform ( 00175 world_frame_, base_frame_, scan_msg->header.stamp, world_to_base_tf); 00176 } 00177 catch (tf::TransformException ex) 00178 { 00179 // transform unavailable - skip scan 00180 ROS_WARN("Skipping scan %s", ex.what()); 00181 return; 00182 } 00183 00184 // calculate world to ortho frame transform 00185 tf::Transform world_to_ortho; 00186 getOrthoTf(world_to_base_tf, world_to_ortho); 00187 } 00188 00189 // **** build and publish projected cloud 00190 00191 PointCloudT::Ptr cloud = 00192 boost::shared_ptr<PointCloudT>(new PointCloudT()); 00193 00194 cloud->header = scan_msg->header; 00195 cloud->header.frame_id = ortho_frame_; 00196 00197 for (unsigned int i = 0; i < scan_msg->ranges.size(); i++) 00198 { 00199 double r = scan_msg->ranges[i]; 00200 00201 if (r > scan_msg->range_min) 00202 { 00203 tf::Vector3 p(r * a_cos_[i], r * a_sin_[i], 0.0); 00204 p = ortho_to_laser_ * p; 00205 00206 PointT point; 00207 point.x = p.getX(); 00208 point.y = p.getY(); 00209 point.z = 0.0; 00210 cloud->points.push_back(point); 00211 } 00212 } 00213 00214 cloud->width = cloud->points.size(); 00215 cloud->height = 1; 00216 cloud->is_dense = true; // no nan's present 00217 00218 cloud_publisher_.publish (cloud); 00219 } 00220 00221 bool LaserOrthoProjector::getBaseToLaserTf (const sensor_msgs::LaserScan::ConstPtr& scan_msg) 00222 { 00223 tf::StampedTransform base_to_laser_tf; 00224 try 00225 { 00226 tf_listener_.waitForTransform( 00227 base_frame_, scan_msg->header.frame_id, scan_msg->header.stamp, ros::Duration(1.0)); 00228 tf_listener_.lookupTransform ( 00229 base_frame_, scan_msg->header.frame_id, scan_msg->header.stamp, base_to_laser_tf); 00230 } 00231 catch (tf::TransformException ex) 00232 { 00233 ROS_WARN("LaserOrthoProjector: Could not get initial laser transform(%s)", ex.what()); 00234 return false; 00235 } 00236 base_to_laser_ = base_to_laser_tf; 00237 00238 return true; 00239 } 00240 00241 void LaserOrthoProjector::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg) 00242 { 00243 a_cos_.clear(); 00244 a_sin_.clear(); 00245 00246 for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i) 00247 { 00248 double angle = scan_msg->angle_min + i * scan_msg->angle_increment; 00249 a_cos_.push_back(cos(angle)); 00250 a_sin_.push_back(sin(angle)); 00251 } 00252 } 00253 00254 } //namespace scan_tools