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 nan_point_.x = std::numeric_limits<float>::quiet_NaN();
00043 nan_point_.y = std::numeric_limits<float>::quiet_NaN();
00044 nan_point_.z = std::numeric_limits<float>::quiet_NaN();
00045
00046
00047
00048 if (!nh_private_.getParam ("fixed_frame", world_frame_))
00049 world_frame_ = "/world";
00050 if (!nh_private_.getParam ("base_frame", base_frame_))
00051 base_frame_ = "/base_link";
00052 if (!nh_private_.getParam ("ortho_frame", ortho_frame_))
00053 ortho_frame_ = "/base_ortho";
00054 if (!nh_private_.getParam ("publish_tf", publish_tf_))
00055 publish_tf_ = false;
00056 if (!nh_private_.getParam ("use_imu", use_imu_))
00057 use_imu_ = true;
00058
00059
00060
00061 scan_subscriber_ = nh_.subscribe(
00062 scan_topic_, 10, &LaserOrthoProjector::scanCallback, this);
00063
00064 if (use_imu_)
00065 {
00066 imu_subscriber_ = nh_.subscribe(
00067 imu_topic_, 10, &LaserOrthoProjector::imuCallback, this);
00068 }
00069
00070
00071
00072 cloud_publisher_ = nh_.advertise<PointCloudT>(
00073 cloud_topic_, 10);
00074 }
00075
00076 LaserOrthoProjector::~LaserOrthoProjector ()
00077 {
00078
00079 }
00080
00081 void LaserOrthoProjector::imuCallback (const sensor_msgs::Imu::ConstPtr& imu_msg)
00082 {
00083 latest_imu_msg_ = *imu_msg;
00084 }
00085
00086 void LaserOrthoProjector::scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00087 {
00088 if(!initialized_)
00089 {
00090 initialized_ = getBaseToLaserTf(scan_msg);
00091
00092 if (initialized_) createCache(scan_msg);
00093 else return;
00094 }
00095
00096
00097
00098 btTransform world_to_base;
00099
00100 if(use_imu_)
00101 {
00102 world_to_base.setIdentity();
00103 btQuaternion q;
00104 tf::quaternionMsgToTF(latest_imu_msg_.orientation, q);
00105 world_to_base.setRotation(q);
00106 }
00107 else
00108 {
00109 tf::StampedTransform world_to_base_tf;
00110
00111 try
00112 {
00113 tf_listener_.waitForTransform (
00114 world_frame_, base_frame_, scan_msg->header.stamp, ros::Duration(0.5));
00115 tf_listener_.lookupTransform (
00116 world_frame_, base_frame_, scan_msg->header.stamp, world_to_base_tf);
00117 }
00118 catch (tf::TransformException ex)
00119 {
00120
00121 ROS_WARN ("Skipping scan (%s)", ex.what ());
00122 return;
00123 }
00124 world_to_base = world_to_base_tf;
00125 }
00126
00127 double roll, pitch, yaw;
00128 btMatrix3x3 m (world_to_base.getRotation ());
00129 m.getRPY (roll, pitch, yaw);
00130
00131
00132
00133 btTransform world_to_ortho;
00134
00135 btQuaternion rotation;
00136 rotation.setRPY (0.0, 0.0, yaw);
00137 world_to_ortho.setRotation (rotation);
00138
00139 btVector3 origin;
00140 origin.setValue (world_to_base.getOrigin().getX(),
00141 world_to_base.getOrigin().getY(),
00142 0.0);
00143 world_to_ortho.setOrigin (origin);
00144
00145 if (publish_tf_)
00146 {
00147 tf::StampedTransform world_to_ortho_tf(world_to_ortho, scan_msg->header.stamp, world_frame_, ortho_frame_);
00148 tf_broadcaster_.sendTransform (world_to_ortho_tf);
00149 }
00150
00151
00152
00153 PointCloudT::Ptr cloud = boost::make_shared<PointCloudT>();
00154
00155 cloud->header = scan_msg->header;
00156 cloud->header.frame_id = ortho_frame_;
00157
00158 for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
00159 {
00160 double r = scan_msg->ranges[i];
00161
00162 if (r > scan_msg->range_min)
00163 {
00164 btVector3 p(r * a_cos_[i], r * a_sin_[i], 0.0);
00165 p = world_to_ortho.inverse() * world_to_base * base_to_laser_ * p;
00166 p.setZ(0.0);
00167
00168 PointT point;
00169 point.x = p.getX();
00170 point.y = p.getY();
00171 point.z = p.getZ();
00172 cloud->points.push_back(point);
00173 }
00174 }
00175
00176 cloud->width = cloud->points.size();
00177 cloud->height = 1;
00178 cloud->is_dense = true;
00179
00180 cloud_publisher_.publish (cloud);
00181 }
00182
00183 bool LaserOrthoProjector::getBaseToLaserTf (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00184 {
00185 tf::StampedTransform base_to_laser_tf;
00186 try
00187 {
00188 tf_listener_.waitForTransform(
00189 base_frame_, scan_msg->header.frame_id, scan_msg->header.stamp, ros::Duration(1.0));
00190 tf_listener_.lookupTransform (
00191 base_frame_, scan_msg->header.frame_id, scan_msg->header.stamp, base_to_laser_tf);
00192 }
00193 catch (tf::TransformException ex)
00194 {
00195 ROS_WARN("LaserOrthoProjector: Could not get initial laser transform(%s)", ex.what());
00196 return false;
00197 }
00198 base_to_laser_ = base_to_laser_tf;
00199
00200 return true;
00201 }
00202
00203 void LaserOrthoProjector::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00204 {
00205 a_cos_.clear();
00206 a_sin_.clear();
00207
00208 for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
00209 {
00210 double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
00211 a_cos_.push_back(cos(angle));
00212 a_sin_.push_back(sin(angle));
00213 }
00214 }
00215
00216 }