00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "laser_ortho_projector/laser_ortho_projector.h"
00031
00032 #include <pcl_conversions/pcl_conversions.h>
00033
00034 namespace scan_tools {
00035
00036 LaserOrthoProjector::LaserOrthoProjector (ros::NodeHandle nh, ros::NodeHandle nh_private):
00037 nh_(nh),
00038 nh_private_(nh_private)
00039 {
00040 ROS_INFO ("Starting LaserOrthoProjector");
00041
00042 initialized_ = false;
00043
00044
00045
00046 ortho_to_laser_.setIdentity();
00047
00048 nan_point_.x = std::numeric_limits<float>::quiet_NaN();
00049 nan_point_.y = std::numeric_limits<float>::quiet_NaN();
00050 nan_point_.z = std::numeric_limits<float>::quiet_NaN();
00051
00052
00053
00054 if (!nh_private_.getParam ("fixed_frame", world_frame_))
00055 world_frame_ = "/world";
00056 if (!nh_private_.getParam ("base_frame", base_frame_))
00057 base_frame_ = "/base_link";
00058 if (!nh_private_.getParam ("ortho_frame", ortho_frame_))
00059 ortho_frame_ = "/base_ortho";
00060 if (!nh_private_.getParam ("publish_tf", publish_tf_))
00061 publish_tf_ = false;
00062 if (!nh_private_.getParam ("use_pose", use_pose_))
00063 use_pose_ = true;
00064 if (!nh_private_.getParam ("use_imu", use_imu_))
00065 use_imu_ = false;
00066
00067 if (use_imu_ && use_pose_)
00068 ROS_FATAL("use_imu and use_pose params cannot both be true");
00069 if (!use_imu_ && !use_pose_)
00070 ROS_FATAL("use_imu and use_pose params cannot both be false");
00071
00072
00073
00074
00075 scan_subscriber_ = nh_.subscribe(
00076 "scan", 10, &LaserOrthoProjector::scanCallback, this);
00077
00078 if (use_pose_)
00079 {
00080 pose_subscriber_ = nh_.subscribe(
00081 "pose", 10, &LaserOrthoProjector::poseCallback, this);
00082 }
00083 if (use_imu_)
00084 {
00085 imu_subscriber_ = nh_.subscribe(
00086 "imu/data", 10, &LaserOrthoProjector::imuCallback, this);
00087 }
00088
00089
00090
00091 cloud_publisher_ = nh_.advertise<PointCloudT>(
00092 "cloud_ortho", 10);
00093 }
00094
00095 LaserOrthoProjector::~LaserOrthoProjector()
00096 {
00097
00098 }
00099
00100 void LaserOrthoProjector::imuCallback(const ImuMsg::ConstPtr& imu_msg)
00101 {
00102
00103 tf::Transform world_to_base;
00104 world_to_base.setIdentity();
00105
00106 tf::Quaternion q;
00107 tf::quaternionMsgToTF(imu_msg->orientation, q);
00108 world_to_base.setRotation(q);
00109
00110
00111 tf::Transform world_to_ortho;
00112 getOrthoTf(world_to_base, world_to_ortho);
00113
00114 if (publish_tf_)
00115 {
00116 tf::StampedTransform world_to_ortho_tf(
00117 world_to_ortho, imu_msg->header.stamp, world_frame_, ortho_frame_);
00118 tf_broadcaster_.sendTransform(world_to_ortho_tf);
00119 }
00120
00121
00122 ortho_to_laser_ = world_to_ortho.inverse() * world_to_base * base_to_laser_;
00123 }
00124
00125 void LaserOrthoProjector::poseCallback(const PoseMsg::ConstPtr& pose_msg)
00126 {
00127
00128 tf::Transform world_to_base;
00129 tf::poseMsgToTF(pose_msg->pose, world_to_base);
00130
00131
00132 tf::Transform world_to_ortho;
00133 getOrthoTf(world_to_base, world_to_ortho);
00134
00135 if (publish_tf_)
00136 {
00137 tf::StampedTransform world_to_ortho_tf(
00138 world_to_ortho, pose_msg->header.stamp, world_frame_, ortho_frame_);
00139 tf_broadcaster_.sendTransform(world_to_ortho_tf);
00140 }
00141
00142
00143 ortho_to_laser_ = world_to_ortho.inverse() * world_to_base * base_to_laser_;
00144 }
00145
00146 void LaserOrthoProjector::getOrthoTf(const tf::Transform& world_to_base, tf::Transform& world_to_ortho)
00147 {
00148 const tf::Vector3& w2b_o = world_to_base.getOrigin();
00149 const tf::Quaternion& w2b_q = world_to_base.getRotation();
00150
00151 tf::Vector3 wto_o(w2b_o.getX(), w2b_o.getY(), 0.0);
00152 tf::Quaternion wto_q = tf::createQuaternionFromYaw(tf::getYaw(w2b_q));
00153
00154 world_to_ortho.setOrigin(wto_o);
00155 world_to_ortho.setRotation(wto_q);
00156 }
00157
00158 void LaserOrthoProjector::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00159 {
00160 if(!initialized_)
00161 {
00162 initialized_ = getBaseToLaserTf(scan_msg);
00163
00164 if (initialized_) createCache(scan_msg);
00165 else return;
00166 }
00167
00168 if(!use_pose_)
00169 {
00170
00171 tf::StampedTransform world_to_base_tf;
00172 try
00173 {
00174 tf_listener_.waitForTransform (
00175 world_frame_, base_frame_, scan_msg->header.stamp, ros::Duration(0.1));
00176 tf_listener_.lookupTransform (
00177 world_frame_, base_frame_, scan_msg->header.stamp, world_to_base_tf);
00178 }
00179 catch (tf::TransformException ex)
00180 {
00181
00182 ROS_WARN("Skipping scan %s", ex.what());
00183 return;
00184 }
00185
00186
00187 tf::Transform world_to_ortho;
00188 getOrthoTf(world_to_base_tf, world_to_ortho);
00189 }
00190
00191
00192
00193 PointCloudT::Ptr cloud =
00194 boost::shared_ptr<PointCloudT>(new PointCloudT());
00195
00196 pcl_conversions::toPCL(scan_msg->header, cloud->header);
00197 cloud->header.frame_id = ortho_frame_;
00198
00199 for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
00200 {
00201 double r = scan_msg->ranges[i];
00202
00203 if (r > scan_msg->range_min)
00204 {
00205 tf::Vector3 p(r * a_cos_[i], r * a_sin_[i], 0.0);
00206 p = ortho_to_laser_ * p;
00207
00208 PointT point;
00209 point.x = p.getX();
00210 point.y = p.getY();
00211 point.z = 0.0;
00212 cloud->points.push_back(point);
00213 }
00214 }
00215
00216 cloud->width = cloud->points.size();
00217 cloud->height = 1;
00218 cloud->is_dense = true;
00219
00220 cloud_publisher_.publish (cloud);
00221 }
00222
00223 bool LaserOrthoProjector::getBaseToLaserTf (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00224 {
00225 tf::StampedTransform base_to_laser_tf;
00226 try
00227 {
00228 tf_listener_.waitForTransform(
00229 base_frame_, scan_msg->header.frame_id, scan_msg->header.stamp, ros::Duration(1.0));
00230 tf_listener_.lookupTransform (
00231 base_frame_, scan_msg->header.frame_id, scan_msg->header.stamp, base_to_laser_tf);
00232 }
00233 catch (tf::TransformException ex)
00234 {
00235 ROS_WARN("LaserOrthoProjector: Could not get initial laser transform(%s)", ex.what());
00236 return false;
00237 }
00238 base_to_laser_ = base_to_laser_tf;
00239
00240 return true;
00241 }
00242
00243 void LaserOrthoProjector::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00244 {
00245 a_cos_.clear();
00246 a_sin_.clear();
00247
00248 for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
00249 {
00250 double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
00251 a_cos_.push_back(cos(angle));
00252 a_sin_.push_back(sin(angle));
00253 }
00254 }
00255
00256 }