00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
00107
00108
00109 processor_.addJointState(joint_state);
00110
00111
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 ros::SingleThreadedSpinner spinner;
00184 spinner.spin(queue);
00185 }
00186
00187 int main(int argc, char** argv)
00188 {
00189 ros::init(argc, argv, "laser_joint_processor");
00190 ros::NodeHandle private_nh("~");
00191
00192 LaserJointProcessorNode processor;
00193
00194
00195 ros::NodeHandle n;
00196 ros::Subscriber joint_state_sub;
00197
00198 ros::CallbackQueue queue;
00199 ros::SubscribeOptions ops;
00200 ops.init<sensor_msgs::JointState>("joint_states", 2000, boost::bind(&LaserJointProcessorNode::jointStateCallback, &processor, _1));
00201 ops.transport_hints = ros::TransportHints();
00202 ops.callback_queue = &queue;
00203 joint_state_sub = n.subscribe(ops);
00204
00205 vector<string> joint_names;
00206 getParamConfig(private_nh, joint_names);
00207 processor.configure(joint_names, 2000);
00208
00209 boost::function<void()> func = boost::bind(&spinFunc, (ros::CallbackQueue*) &queue);
00210
00211 boost::thread jointStateSpinner(func);
00212 ros::spin();
00213 printf("Waiting for thread to join");
00214 jointStateSpinner.join();
00215 printf("Done joining");
00216 return 0;
00217 }
00218
00219
00220
00221
00222
00223
00224
00225