laser_joint_processor: laser_joint_processor.cpp Source 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 
00036 
00037 #include <laser_joint_processor/laser_joint_processor.h>
00038 
00039 using namespace laser_joint_processor;
00040 using namespace std;
00041 
00042 LaserJointProcessor::LaserJointProcessor()
00043 {
00044 
00045 }
00046 
00047 void LaserJointProcessor::setCacheSize(unsigned int max_size)
00048 {
00049   joint_state_cache_.setMaxSize(max_size);
00050 }
00051 
00052 void LaserJointProcessor::setJointNames(const std::vector<std::string>& joint_names)
00053 {
00054   joint_names_ = joint_names;
00055   deflater_.setDeflationJointNames(joint_names);
00056 }
00057 
00058 bool LaserJointProcessor::addJointState(const sensor_msgs::JointStateConstPtr& joint_state)
00059 {
00060   joint_states_settler::DeflatedJointState deflated;
00061   deflater_.deflate(joint_state, deflated);
00062   //printf("Deflated: %f\n", deflated.channels_[0]);
00063   joint_state_cache_.add(deflated);
00064 
00065   return true;
00066 }
00067 
00068 bool LaserJointProcessor::isSnapshotEarly(const calibration_msgs::DenseLaserSnapshot& snapshot) const
00069 {
00070   ROS_DEBUG("Checking if snapshot is early:");
00071   if (joint_state_cache_.size() == 0)
00072   {
00073     ROS_DEBUG("  cache is empty, so snapshot is definitely early");
00074     return true;
00075   }
00076   if (snapshot.scan_start.size() == 0)
00077     ROS_FATAL("Received malformed DenseLaserSnapshot. (scan_start.size() == 0)");
00078   ros::Time snapshot_end = snapshot.scan_start.back() + ros::Duration(snapshot.time_increment*(snapshot.num_scans-1));
00079 
00080   ros::Time cache_front = joint_state_cache_.front().header.stamp;
00081   ros::Time cache_back = joint_state_cache_.back().header.stamp;
00082   ROS_DEBUG("  Newest elem in cache (back):  %u.%u", cache_back.sec, cache_back.nsec);
00083   ROS_DEBUG("  Oldest elem in cache (front): %u.%u", cache_front.sec, cache_front.nsec);
00084   ROS_DEBUG("  End of snapshot:      %u.%u", snapshot_end.sec, snapshot_end.nsec);
00085   ros::Duration diff = snapshot_end - cache_back;
00086   ROS_DEBUG("  cache_back - snapshot_end: %.2f", diff.toSec());
00087 
00088   return  diff >= ros::Duration(0,0);
00089 }
00090 
00091 bool LaserJointProcessor::isSnapshotLate(const calibration_msgs::DenseLaserSnapshot& snapshot) const
00092 {
00093   ROS_DEBUG("Checking if snapshot is late:");
00094   if (joint_state_cache_.size() == 0)
00095   {
00096     ROS_DEBUG("  cache is empty, so snapshot isn't late yet");
00097     return false;
00098   }
00099 
00100   if (snapshot.scan_start.size() == 0)
00101     ROS_FATAL("Received malformed DenseLaserSnapshot. (scan_start.size() == 0)");
00102   ros::Time snapshot_start = snapshot.scan_start.front();
00103   ros::Time cache_front = joint_state_cache_.front().header.stamp;
00104   ros::Time cache_back = joint_state_cache_.back().header.stamp;
00105 
00106   ros::Duration diff = snapshot_start - cache_front;
00107 
00108   ROS_DEBUG("  Newest elem in cache (back):  %u.%u", cache_back.sec, cache_back.nsec);
00109   ROS_DEBUG("  Oldest elem in cache (front): %u.%u", cache_front.sec, cache_front.nsec);
00110   ROS_DEBUG("  Start of snapshot:            %u.%u", snapshot_start.sec, snapshot_start.nsec);
00111   ROS_DEBUG("  cache_front - snapshot_start: %.2f", diff.toSec());
00112 
00113   return diff <= ros::Duration(0,0);
00114 }
00115 
00116 bool LaserJointProcessor::processLaserData( const calibration_msgs::DenseLaserSnapshot& snapshot,
00117                                             const calibration_msgs::CalibrationPattern& features,
00118                                             calibration_msgs::JointStateCalibrationPattern& result,
00119                                             const ros::Duration& max_interp)
00120 {
00121   // Store # of joints
00122   const unsigned int num_joints = joint_names_.size();
00123   // store # of feature points
00124   const unsigned int P = features.image_points.size();
00125 
00126   result.object_points = features.object_points;
00127   result.joint_points.clear();
00128   result.joint_points.resize(P);
00129 
00130   bool success;
00131   success = imager_.update(snapshot, joint_state_cache_, max_interp);
00132 
00133   //imager_.displayImage(0);
00134 /*  imager_.writeImage(0, "/wg/stor2/vpradeep/ros/pkgs/wg-ros-pkg/calibration_experimental/pr2_calibration_launch/data/image.txt");
00135 
00136 
00137   FILE* file;
00138   file = fopen("/wg/stor2/vpradeep/ros/pkgs/wg-ros-pkg/calibration_experimental/pr2_calibration_launch/data/scan_start.txt", "w");
00139   for (unsigned int i=0; i<snapshot.scan_start.size(); i++)
00140   {
00141     fprintf(file, "%u.%u\n", snapshot.scan_start[i].sec, snapshot.scan_start[i].nsec);
00142   }
00143   fclose(file);
00144 
00145   file = fopen("/wg/stor2/vpradeep/ros/pkgs/wg-ros-pkg/calibration_experimental/pr2_calibration_launch/data/joint_cache.txt", "w");
00146   for (unsigned int i=0; i<joint_state_cache_.size(); i++)
00147   {
00148     fprintf(file, "%u.%u  ", joint_state_cache_[i].header.stamp.sec, joint_state_cache_[i].header.stamp.nsec);
00149     for (unsigned int j=0; j<joint_state_cache_[i].channels_.size(); j++)
00150     {
00151       fprintf(file, "%4.3f  ", joint_state_cache_[i].channels_[j]);
00152     }
00153     fprintf(file, "\n");
00154   }
00155   fclose(file);
00156 
00157   file = fopen("/wg/stor2/vpradeep/ros/pkgs/wg-ros-pkg/calibration_experimental/pr2_calibration_launch/data/features.txt", "w");
00158   for (unsigned int i=0; i<features.image_points.size(); i++)
00159   {
00160     fprintf(file, "%4.3f  %4.3f\n", features.image_points[i].x, features.image_points[i].y);
00161   }
00162   fclose(file);
00163 */
00164   //printf("\n");
00165   //imager_.displayImage(1);
00166 
00167   if (!success)
00168   {
00169     ROS_ERROR("imager_.update failed");
00170     return false;
00171   }
00172 
00173   for (unsigned int j=0; j<num_joints; j++)
00174   {
00175 
00176     vector<float> positions;
00177     vector<float> velocities;
00178     interp_.interp(features.image_points, imager_.getJointImage(j),
00179                    positions, velocities);
00180 
00181     for (unsigned int i=0; i<P; i++)
00182     {
00183       result.joint_points[i].name.push_back(joint_names_[j]);
00184       result.joint_points[i].position.push_back(positions[i]);
00185       result.joint_points[i].velocity.push_back(velocities[i]);
00186       result.joint_points[i].effort.push_back(1234);
00187     }
00188   }
00189 
00190   vector<float> laser_angles;
00191   vector<float> laser_ranges;
00192 
00193   success = interpSnapshot(features.image_points, snapshot, laser_angles, laser_ranges);
00194   if (!success)
00195   {
00196     ROS_ERROR("interpSnapshot failed");
00197     return false;
00198   }
00199 
00200   for (unsigned int i=0; i<P; i++)
00201   {
00202     result.joint_points[i].name.push_back("laser_angle_joint");
00203     result.joint_points[i].position.push_back(laser_angles[i]);
00204     result.joint_points[i].velocity.push_back(0);
00205     result.joint_points[i].effort.push_back(1234);
00206 
00207     result.joint_points[i].name.push_back("laser_range_joint");
00208     result.joint_points[i].position.push_back(laser_ranges[i]);
00209     result.joint_points[i].velocity.push_back(0);
00210     result.joint_points[i].effort.push_back(1234);
00211   }
00212 
00213   result.header.stamp = snapshot.header.stamp;
00214 
00215   return true;
00216 }
00217 
00218 
00219 
00220