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 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
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
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
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
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
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
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
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
00126 tf::Transform world_to_base;
00127 tf::poseMsgToTF(pose_msg->pose, world_to_base);
00128
00129
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
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
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
00180 ROS_WARN("Skipping scan %s", ex.what());
00181 return;
00182 }
00183
00184
00185 tf::Transform world_to_ortho;
00186 getOrthoTf(world_to_base_tf, world_to_ortho);
00187 }
00188
00189
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;
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 }