laser_joint_processor: laser_joint_processor: laser_joint

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