$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 00036 00037 #include <cstdio> 00038 #include <laser_joint_processor/laser_joint_processor_node.h> 00039 #include <ros/callback_queue.h> 00040 #include <ros/spinner.h> 00041 #include <boost/thread.hpp> 00042 00043 using namespace laser_joint_processor; 00044 using namespace std; 00045 00046 LaserJointProcessorNode::LaserJointProcessorNode() : 00047 snapshot_sub_(nh_, "snapshot", 1), 00048 features_sub_(nh_, "features", 1), 00049 sync_(snapshot_sub_, features_sub_, 1) 00050 { 00051 pub_ = nh_.advertise<calibration_msgs::JointStateCalibrationPattern>("joint_features", 1); 00052 00053 00054 sync_connection_ = sync_.registerCallback( boost::bind( &LaserJointProcessorNode::syncCallback, this, _1, _2)); 00055 } 00056 00057 LaserJointProcessorNode::~LaserJointProcessorNode() 00058 { 00059 sync_connection_.disconnect(); 00060 } 00061 00062 void LaserJointProcessorNode::configure(const vector<string>& joint_names, unsigned int max_cache_size) 00063 { 00064 processor_.setJointNames(joint_names); 00065 processor_.setCacheSize(max_cache_size); 00066 } 00067 00068 void LaserJointProcessorNode::syncCallback(const calibration_msgs::DenseLaserSnapshotConstPtr& snapshot, 00069 const calibration_msgs::CalibrationPatternConstPtr& features) 00070 { 00071 if (features->success == 0) 00072 { 00073 ROS_DEBUG("Didn't find checkerboard. Exiting"); 00074 return; 00075 } 00076 00077 boost::mutex::scoped_lock lock(mutex_); 00078 00079 if (queued_snapshot_) 00080 ROS_WARN("Got a snapshot when a queued snapshot has still not been processed. Throwing away older snapshot"); 00081 00082 // We got new data, so we're simply going to throw away the old stuff 00083 queued_snapshot_.reset(); 00084 queued_features_.reset(); 00085 00086 if (processor_.isSnapshotLate(*snapshot)) 00087 { 00088 ROS_WARN("Snapshot is too old to be handled by joint state cache. Discarding snapshot"); 00089 return; 00090 } 00091 else if (processor_.isSnapshotEarly(*snapshot)) 00092 { 00093 ROS_DEBUG("Don't have joint_state info for this snapshot yet. Going to queue this snapshot"); 00094 queued_snapshot_ = snapshot; 00095 queued_features_ = features; 00096 return; 00097 } 00098 ROS_DEBUG("In syncCallback. About to process pair"); 00099 processPair(snapshot, features); 00100 } 00101 00102 void LaserJointProcessorNode::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state) 00103 { 00104 boost::mutex::scoped_lock lock(mutex_); 00105 00106 //printf("J"); 00107 //fflush(stdout); 00108 00109 processor_.addJointState(joint_state); 00110 00111 // We might have to processed the queued data, since we just got a new joint_state message 00112 if (queued_snapshot_) 00113 { 00114 if (processor_.isSnapshotLate(*queued_snapshot_)) 00115 { 00116 ROS_WARN("Queued snapshot is too old to be handled by joint state cache. Discarding queued snapshot"); 00117 queued_snapshot_.reset(); 00118 queued_features_.reset(); 00119 return; 00120 } 00121 else if (processor_.isSnapshotEarly(*queued_snapshot_)) 00122 { 00123 ROS_DEBUG("Don't have joint_state info for this queued snapshot yet. Going to keep this snapshot queued"); 00124 return; 00125 } 00126 else 00127 { 00128 ROS_DEBUG("In jointStateCallback. About to process pair"); 00129 processPair(queued_snapshot_, queued_features_); 00130 queued_snapshot_.reset(); 00131 queued_features_.reset(); 00132 } 00133 } 00134 } 00135 00136 void LaserJointProcessorNode::processPair(const calibration_msgs::DenseLaserSnapshotConstPtr& snapshot, 00137 const calibration_msgs::CalibrationPatternConstPtr& features) 00138 { 00139 calibration_msgs::JointStateCalibrationPattern result; 00140 bool success; 00141 success = processor_.processLaserData(*snapshot, *features, result); 00142 if (!success) 00143 { 00144 ROS_ERROR("Error while calling LaserJointProcessor::processLaserData"); 00145 return; 00146 } 00147 00148 pub_.publish(result); 00149 00150 } 00151 00152 void getParamConfig(ros::NodeHandle& nh, vector<string>& joint_names) 00153 { 00154 joint_names.clear(); 00155 00156 bool found_joint = true; 00157 unsigned int joint_count = 0; 00158 char param_buf[1024] ; 00159 while(found_joint) 00160 { 00161 sprintf(param_buf, "joint_name_%02u", joint_count) ; 00162 std::string param_name = param_buf ; 00163 std::string cur_joint_name ; 00164 found_joint = nh.getParam(param_name, cur_joint_name) ; 00165 if (found_joint) 00166 { 00167 ROS_INFO("[%s] -> [%s]", param_name.c_str(), cur_joint_name.c_str()); 00168 joint_names.push_back(cur_joint_name); 00169 joint_count++ ; 00170 } 00171 } 00172 00173 if (joint_names.size() == 0) 00174 ROS_INFO("Found no joints on param server"); 00175 } 00176 00177 using namespace laser_joint_processor; 00178 00179 00180 00181 void spinFunc(ros::CallbackQueue* queue) 00182 { 00183 static const double timeout = 0.01; 00184 00185 while (ros::ok()) 00186 { 00187 queue->callAvailable(ros::WallDuration(timeout)); 00188 } 00189 } 00190 00191 int main(int argc, char** argv) 00192 { 00193 ros::init(argc, argv, "laser_joint_processor"); 00194 ros::NodeHandle private_nh("~"); 00195 00196 LaserJointProcessorNode processor; 00197 00198 // Put joint states callback on its own queue 00199 ros::NodeHandle n; 00200 ros::Subscriber joint_state_sub; 00201 00202 ros::CallbackQueue queue; 00203 ros::SubscribeOptions ops; 00204 ops.init<sensor_msgs::JointState>("joint_states", 2000, boost::bind(&LaserJointProcessorNode::jointStateCallback, &processor, _1)); 00205 ops.transport_hints = ros::TransportHints(); 00206 ops.callback_queue = &queue; 00207 joint_state_sub = n.subscribe(ops); 00208 00209 vector<string> joint_names; 00210 getParamConfig(private_nh, joint_names); 00211 processor.configure(joint_names, 2000); 00212 00213 boost::function<void()> func = boost::bind(&spinFunc, (ros::CallbackQueue*) &queue); 00214 00215 boost::thread jointStateSpinner(func); 00216 ros::spin(); 00217 printf("Waiting for thread to join"); 00218 jointStateSpinner.join(); 00219 printf("Done joining"); 00220 return 0; 00221 } 00222 00223 00224 00225 00226 00227 00228 00229