joint_imager.cpp
Go to the documentation of this file.
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 }
00052 
00053 bool JointImager::update(const calibration_msgs::DenseLaserSnapshot& snapshot,
00054                          const settlerlib::SortedDeque<joint_states_settler::DeflatedJointStates>& cache,
00055                          const ros::Duration& max_interp)
00056 {
00057   if (cache.size() == 0)
00058     return false;
00059 
00060   unsigned int num_channels = cache.back().channels_.size();
00061 
00062   // Size the openCV datatypes accordingly
00063   allocateImages(snapshot.num_scans, snapshot.readings_per_scan, num_channels);
00064 
00065   vector<double> cur_positions;
00066 
00067   //bool success;
00068 
00069   unsigned int after_index = 0;
00070   for (unsigned int i=0; i<snapshot.num_scans; i++)
00071   {
00072     for (unsigned int j=0; j<snapshot.readings_per_scan; j++)
00073     {
00074       ros::Time pixel_time = snapshot.scan_start[i] + ros::Duration(snapshot.time_increment * j);
00075       while(after_index < cache.size() && cache[after_index].header.stamp < pixel_time)
00076         after_index++;
00077       assert(after_index < cache.size());    // Should never encounter this, if we already did our bounds checking on the snapshot
00078       assert(after_index > 0);
00079 
00080       // Sanity check: Determine how far apart are the elems we're interpolating between
00081       ros::Duration interp_diff = cache[after_index].header.stamp - cache[after_index-1].header.stamp;
00082       if (interp_diff > max_interp)
00083       {
00084         ROS_WARN("Interpolating between elems that are [%.2fs] apart. Probably dopped some messages. Going to ignore this snapshot", interp_diff.toSec());
00085         return false;
00086       }
00087 
00088       settlerlib::Deflated::interp(cache[after_index-1], cache[after_index], pixel_time, cur_positions);
00089 
00090       // Populate position field [Channel 0] in each joint image
00091       for (unsigned int k=0;k<num_channels;k++)
00092         images[k](i,j)[0] = cur_positions[k];
00093     }
00094     ros::Time scan_start_time = snapshot.scan_start[i];
00095     ros::Time scan_end_time   = snapshot.scan_start[i] + ros::Duration(snapshot.time_increment * (snapshot.readings_per_scan-1));
00096 
00097     for (unsigned int k=0; k<num_channels; k++)
00098     {
00099       float start_pos = images[k](i,0)[0];
00100       float end_pos   = images[k](i,snapshot.readings_per_scan-1)[0];
00101       float vel = (end_pos - start_pos) / (scan_end_time-scan_start_time).toSec();
00102       // Walk along image row, populating the velocity channel. (Every other float is a velocity).
00103       float* vel_ptr = &images[k](i,0)[1];
00104       for (unsigned int j=0; j<snapshot.readings_per_scan; j++)
00105       {
00106         *vel_ptr = vel;
00107         vel_ptr += 2;
00108       }
00109     }
00110   }
00111 
00112   /* (Inefficient implementation)
00113   // Iterate over scan
00114   for (unsigned int i=0; i<snapshot.num_scans; i++)
00115   {
00116     ros::Time scan_start_time = snapshot.scan_start[i];
00117     ros::Time scan_end_time   = snapshot.scan_start[i] + ros::Duration(snapshot.time_increment * (snapshot.readings_per_scan-1));
00118 
00119     success = computeVelocity(scan_start_time, scan_end_time, cache, cur_velocities);
00120     if (!success)
00121     {
00122       ROS_ERROR("Error computing velocity");
00123       return false;
00124     }
00125 
00126     // Iterate over each ray in a scan
00127     for (unsigned int j=0; j<snapshot.readings_per_scan; j++)
00128     {
00129       ros::Time pixel_time = snapshot.scan_start[i] + ros::Duration(snapshot.time_increment * j);
00130       success = interpPosition(pixel_time, cache, cur_positions);
00131       if (!success)
00132       {
00133         ROS_ERROR("Error interpolating for pixel position");
00134         return false;
00135       }
00136       for (unsigned int k=0;k<num_channels;k++)
00137       {
00138         *(((float*)(images[k]->imageData + images[k]->widthStep*i))+2*j + 0) = cur_positions[k];
00139         *(((float*)(images[k]->imageData + images[k]->widthStep*i))+2*j + 1) = cur_velocities[k];
00140       }
00141     }
00142   }
00143   */
00144   return true;
00145 }
00146 
00147 void JointImager::displayImage(unsigned int i)
00148 {
00149   cv::Mat_<cv::Vec2f> image = getJointImage(i);
00150 
00151   for (int i=0; i<image.rows; i++)
00152   {
00153     for (int j=0; j<image.cols; j++)
00154     {
00155       printf("%5.2f  ", image(i,j)[0]);
00156     }
00157     printf("\n");
00158   }
00159 }
00160 
00161 void JointImager::writeImage(unsigned int i, const string& filename)
00162 {
00163   FILE* file = fopen(filename.c_str(), "w");
00164 
00165   if (file)
00166     printf("About to write to file %s\n", filename.c_str());
00167 
00168   cv::Mat_<cv::Vec2f> image = getJointImage(i);
00169 
00170   for (int i=0; i<image.rows; i++)
00171   {
00172     for (int j=0; j<image.cols; j++)
00173     {
00174       fprintf(file, "% 3.2f  ", image(i,j)[0]);
00175     }
00176     fprintf(file, "\n");
00177   }
00178 
00179   fclose(file);
00180 }
00181 
00182 
00183 bool JointImager::interpPosition(const ros::Time& target,
00184                                  const settlerlib::SortedDeque<joint_states_settler::DeflatedJointStates>& cache,
00185                                  vector<double>& result)
00186 {
00187   bool success;
00188   joint_states_settler::DeflatedJointStates before;
00189   joint_states_settler::DeflatedJointStates after;
00190 
00191   success = cache.getElemBeforeTime(target, before);
00192   if (!success)
00193   {
00194     ROS_ERROR("Couldn't find elem before time");
00195     return false;
00196   }
00197   success = cache.getElemAfterTime(target, after);
00198   if (!success)
00199   {
00200     ROS_ERROR("Couldn't find elem after time");
00201     return false;
00202   }
00203   if (before.channels_.size() != after.channels_.size())
00204   {
00205     ROS_ERROR("# of joints has changed in the middle of a run");
00206     return false;
00207   }
00208 
00209   success = settlerlib::Deflated::interp(before, after, target, result);
00210   if (!success)
00211   {
00212     ROS_ERROR("Error performing interpolation");
00213     return false;
00214   }
00215 
00216   return true;
00217 }
00218 
00219 bool JointImager::computeVelocity(const ros::Time& start, const ros::Time& end,
00220                                   const settlerlib::SortedDeque<joint_states_settler::DeflatedJointStates>& cache,
00221                                   vector<double>& result)
00222 {
00223   vector<double> start_positions;
00224   vector<double> end_positions;
00225 
00226   bool success;
00227   success = interpPosition(start, cache, start_positions);
00228   if (!success)
00229   {
00230     ROS_ERROR("Error extracting start position");
00231     return false;
00232   }
00233 
00234   success = interpPosition(end,   cache, end_positions);
00235   if (!success)
00236   {
00237     ROS_ERROR("Error extracting start position");
00238     return false;
00239   }
00240 
00241   if (start_positions.size() != end_positions.size())
00242   {
00243     ROS_ERROR("# of joints has changed during run.  Can't compute velocity between samples");
00244     return false;
00245   }
00246 
00247   const unsigned int N = start_positions.size();
00248 
00249   result.resize(N);
00250   for (unsigned int i=0; i<N; i++)
00251     result[i] = (end_positions[i] - start_positions[i]) / (end-start).toSec();
00252 
00253   return true;
00254 }
00255 
00256 void JointImager::allocateImages(unsigned int height, unsigned int width, unsigned int channels)
00257 {
00258   images.resize(channels);
00259 
00260   cv::Size image_size = cv::Size(width, height);
00261 
00262   for (unsigned int i=0; i<channels; i++)
00263     images[i] = cv::Mat_<cv::Vec2f>(image_size);
00264 }
00265 
00266 cv::Mat_<cv::Vec2f> JointImager::getJointImage(unsigned int index) const
00267 {
00268   return images[index];
00269 }


laser_joint_processor
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:32