$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #include <cstdio> 00036 #include <laser_joint_processor/joint_imager.h> 00037 #include <ros/console.h> 00038 00039 using namespace laser_joint_processor; 00040 using namespace std; 00041 00042 JointImager::JointImager() 00043 { 00044 00045 00046 00047 } 00048 00049 JointImager::~JointImager() 00050 { 00051 // Deallocate everything 00052 for (unsigned int i=0; i<images.size(); i++) 00053 cvReleaseImage(&images[i]); 00054 } 00055 00056 bool JointImager::update(const calibration_msgs::DenseLaserSnapshot& snapshot, 00057 const settlerlib::SortedDeque<joint_states_settler::DeflatedJointStates>& cache, 00058 const ros::Duration& max_interp) 00059 { 00060 if (cache.size() == 0) 00061 return false; 00062 00063 unsigned int num_channels = cache.back().channels_.size(); 00064 00065 // Size the openCV datatypes accordingly 00066 allocateImages(snapshot.num_scans, snapshot.readings_per_scan, num_channels); 00067 00068 vector<double> cur_positions; 00069 00070 //bool success; 00071 00072 unsigned int after_index = 0; 00073 for (unsigned int i=0; i<snapshot.num_scans; i++) 00074 { 00075 for (unsigned int j=0; j<snapshot.readings_per_scan; j++) 00076 { 00077 ros::Time pixel_time = snapshot.scan_start[i] + ros::Duration(snapshot.time_increment * j); 00078 while(after_index < cache.size() && cache[after_index].header.stamp < pixel_time) 00079 after_index++; 00080 assert(after_index < cache.size()); // Should never encounter this, if we already did our bounds checking on the snapshot 00081 assert(after_index > 0); 00082 00083 // Sanity check: Determine how far apart are the elems we're interpolating between 00084 ros::Duration interp_diff = cache[after_index].header.stamp - cache[after_index-1].header.stamp; 00085 if (interp_diff > max_interp) 00086 { 00087 ROS_WARN("Interpolating between elems that are [%.2fs] apart. Probably dopped some messages. Going to ignore this snapshot", interp_diff.toSec()); 00088 return false; 00089 } 00090 00091 settlerlib::Deflated::interp(cache[after_index-1], cache[after_index], pixel_time, cur_positions); 00092 00093 // Populate position field [Channel 0] in each joint image 00094 for (unsigned int k=0;k<num_channels;k++) 00095 *(((float*)(images[k]->imageData + images[k]->widthStep*i))+2*j + 0) = cur_positions[k]; 00096 } 00097 ros::Time scan_start_time = snapshot.scan_start[i]; 00098 ros::Time scan_end_time = snapshot.scan_start[i] + ros::Duration(snapshot.time_increment * (snapshot.readings_per_scan-1)); 00099 00100 for (unsigned int k=0; k<num_channels; k++) 00101 { 00102 float start_pos = *(((float*)(images[k]->imageData + images[k]->widthStep*i))+2*0 + 0); 00103 float end_pos = *(((float*)(images[k]->imageData + images[k]->widthStep*i))+2*(snapshot.readings_per_scan-1) + 0); 00104 float vel = (end_pos - start_pos) / (scan_end_time-scan_start_time).toSec(); 00105 // Walk along image row, populating the velocity channel. (Every other float is a velocity). 00106 float* vel_ptr = (float*)(images[k]->imageData + images[k]->widthStep*i) + 2*0 + 1; 00107 for (unsigned int j=0; j<snapshot.readings_per_scan; j++) 00108 { 00109 *vel_ptr = vel; 00110 vel_ptr += 2; 00111 } 00112 } 00113 } 00114 00115 /* (Inefficient implementation) 00116 // Iterate over scan 00117 for (unsigned int i=0; i<snapshot.num_scans; i++) 00118 { 00119 ros::Time scan_start_time = snapshot.scan_start[i]; 00120 ros::Time scan_end_time = snapshot.scan_start[i] + ros::Duration(snapshot.time_increment * (snapshot.readings_per_scan-1)); 00121 00122 success = computeVelocity(scan_start_time, scan_end_time, cache, cur_velocities); 00123 if (!success) 00124 { 00125 ROS_ERROR("Error computing velocity"); 00126 return false; 00127 } 00128 00129 // Iterate over each ray in a scan 00130 for (unsigned int j=0; j<snapshot.readings_per_scan; j++) 00131 { 00132 ros::Time pixel_time = snapshot.scan_start[i] + ros::Duration(snapshot.time_increment * j); 00133 success = interpPosition(pixel_time, cache, cur_positions); 00134 if (!success) 00135 { 00136 ROS_ERROR("Error interpolating for pixel position"); 00137 return false; 00138 } 00139 for (unsigned int k=0;k<num_channels;k++) 00140 { 00141 *(((float*)(images[k]->imageData + images[k]->widthStep*i))+2*j + 0) = cur_positions[k]; 00142 *(((float*)(images[k]->imageData + images[k]->widthStep*i))+2*j + 1) = cur_velocities[k]; 00143 } 00144 } 00145 } 00146 */ 00147 return true; 00148 } 00149 00150 void JointImager::displayImage(unsigned int i) 00151 { 00152 IplImage* image = getJointImage(i); 00153 00154 for (int i=0; i<image->height; i++) 00155 { 00156 for (int j=0; j<image->width; j++) 00157 { 00158 printf("%5.2f ", *((float*)(image->imageData + i*image->widthStep) + j*image->nChannels)); 00159 } 00160 printf("\n"); 00161 } 00162 } 00163 00164 void JointImager::writeImage(unsigned int i, const string& filename) 00165 { 00166 FILE* file = fopen(filename.c_str(), "w"); 00167 00168 if (file) 00169 printf("About to write to file %s\n", filename.c_str()); 00170 00171 IplImage* image = getJointImage(i); 00172 00173 for (int i=0; i<image->height; i++) 00174 { 00175 for (int j=0; j<image->width; j++) 00176 { 00177 fprintf(file, "% 3.2f ", *((float*)(image->imageData + i*image->widthStep) + j*image->nChannels)); 00178 } 00179 fprintf(file, "\n"); 00180 } 00181 00182 fclose(file); 00183 } 00184 00185 00186 bool JointImager::interpPosition(const ros::Time& target, 00187 const settlerlib::SortedDeque<joint_states_settler::DeflatedJointStates>& cache, 00188 vector<double>& result) 00189 { 00190 bool success; 00191 joint_states_settler::DeflatedJointStates before; 00192 joint_states_settler::DeflatedJointStates after; 00193 00194 success = cache.getElemBeforeTime(target, before); 00195 if (!success) 00196 { 00197 ROS_ERROR("Couldn't find elem before time"); 00198 return false; 00199 } 00200 success = cache.getElemAfterTime(target, after); 00201 if (!success) 00202 { 00203 ROS_ERROR("Couldn't find elem after time"); 00204 return false; 00205 } 00206 if (before.channels_.size() != after.channels_.size()) 00207 { 00208 ROS_ERROR("# of joints has changed in the middle of a run"); 00209 return false; 00210 } 00211 00212 success = settlerlib::Deflated::interp(before, after, target, result); 00213 if (!success) 00214 { 00215 ROS_ERROR("Error performing interpolation"); 00216 return false; 00217 } 00218 00219 return true; 00220 } 00221 00222 bool JointImager::computeVelocity(const ros::Time& start, const ros::Time& end, 00223 const settlerlib::SortedDeque<joint_states_settler::DeflatedJointStates>& cache, 00224 vector<double>& result) 00225 { 00226 vector<double> start_positions; 00227 vector<double> end_positions; 00228 00229 bool success; 00230 success = interpPosition(start, cache, start_positions); 00231 if (!success) 00232 { 00233 ROS_ERROR("Error extracting start position"); 00234 return false; 00235 } 00236 00237 success = interpPosition(end, cache, end_positions); 00238 if (!success) 00239 { 00240 ROS_ERROR("Error extracting start position"); 00241 return false; 00242 } 00243 00244 if (start_positions.size() != end_positions.size()) 00245 { 00246 ROS_ERROR("# of joints has changed during run. Can't compute velocity between samples"); 00247 return false; 00248 } 00249 00250 const unsigned int N = start_positions.size(); 00251 00252 result.resize(N); 00253 for (unsigned int i=0; i<N; i++) 00254 result[i] = (end_positions[i] - start_positions[i]) / (end-start).toSec(); 00255 00256 return true; 00257 } 00258 00259 void JointImager::allocateImages(unsigned int height, unsigned int width, unsigned int channels) 00260 { 00261 // Deallocate everything 00262 for (unsigned int i=0; i<images.size(); i++) 00263 cvReleaseImage(&images[i]); 00264 00265 images.resize(channels); 00266 00267 CvSize image_size = cvSize(width, height); 00268 00269 for (unsigned int i=0; i<channels; i++) 00270 images[i] = cvCreateImage(image_size, IPL_DEPTH_32F, 2); 00271 } 00272 00273 IplImage* JointImager::getJointImage(unsigned int index) const 00274 { 00275 return images[index]; 00276 }