int WINAPI subscribe(::WQEvent* pEvent, LPVOID lpUserParam) {
SockPair::SOCKET_ptr* socks = static_cast<SockPair::SOCKET_ptr*>(lpUserParam);
assert(socks != NULL);
assert(pEvent != NULL);
switch (pEvent->EventType) {
case eWQPartialResponse:
return publishResult(static_cast<Event&>(*pEvent), socks);
case eWQErrorReport:
std::cerr << "<WQ> error report: " << util::error2Text(pEvent->ErrCode) << std::endl;
return false;
case eWQOthers:
// Detect unsubscribe and free up socket pair
if (pEvent->ErrCode == WQERR_USER_CANCEL) {
sd0(*socks[CLIENT]);
REGISTRY.erase(pEvent->RequestID);
delete[] socks;
return true;
}
else {
// fall through
}
default:
std::cerr << "<WQ> unsupported subscription response: " << *pEvent << std::endl;
return false;
}
}
void ThriftAsyncResult::readAndPublishResult( ) {
while( future && asyncResultSink ) {
try {
{
std::unique_lock<std::mutex> lock( mutex );
while( outstanding == 0 && !abort ) {
cond.wait( lock );
}
if( abort ) {
break;
}
}
Hypertable::ThriftGen::ResultSerialized result;
HT4C_TRY {
try {
ThriftClientLock sync( client.get() );
client->future_get_result_serialized( result, future, queryFutureResultTimeoutMs );
}
catch( Hypertable::ThriftGen::ClientException& e ) {
if( e.code == Hypertable::Error::REQUEST_TIMEOUT
|| e.code == Hypertable::Error::NOT_IMPLEMENTED ) { // FIXME, ThriftBroker does not implement get_future_result_serialized for async mutators
continue;
}
throw;
}
// ignore cancelled scanners
if( result.id && result.is_scan && !result.is_error && !result.is_empty ) {
std::lock_guard<std::mutex> lock( mutex );
if( asyncTableScanners.find(result.id) == asyncTableScanners.end() ) {
continue;
}
}
}
HT4C_THRIFT_RETHROW
if( !publishResult(result, this, asyncResultSink, false) || result.is_empty ) {
std::lock_guard<std::mutex> lock( mutex );
outstanding = std::max( 0, --outstanding );
cond.notify_all();
}
}
catch( Common::HypertableException& e ) {
cancel();
asyncResultSink->failure( e );
}
catch( ... ) {
cancel();
std::stringstream ss;
ss << "Caught unknown exception\n\tat " << __FUNCTION__ << " (" << __FILE__ << ':' << __LINE__ << ')';
Common::HypertableException e( Hypertable::Error::EXTERNAL, ss.str(), __LINE__, __FUNCTION__, __FILE__ );
asyncResultSink->failure( e );
}
}
}
void LaserMapping::process()
{
if (!hasNewData())// waiting for new data to arrive...
return;
reset();// reset flags, etc.
if (!BasicLaserMapping::process(fromROSTime(_timeLaserOdometry)))
return;
publishResult();
}
//OpenNI Grabber's cloud Callback function
void
ParticleFilterTracking::cloud_cb (const sensor_msgs::PointCloud2 &pc)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
frame_id_ = pc.header.frame_id;
std::vector<int> indices;
pcl::fromROSMsg(pc, *cloud);
cloud->is_dense = false;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
//when counter_ > 10, try to track.
if(counter_ > 10){
cloud_pass_downsampled_.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::copyPointCloud(*cloud, *cloud_pass_downsampled_);
if (!cloud_pass_downsampled_->points.empty()){
boost::mutex::scoped_lock lock(mtx_);
tracker_->setInputCloud (cloud_pass_downsampled_);
tracker_->compute ();
publishParticles();
publishResult();
ROS_INFO("Tracking.. %d", (int)cloud->points.size());
}else{
if(counter_%20 == 0){
ROS_INFO("Not Tracking... Select Point/Area (Press 'h' to show Help)");
}
}
new_cloud_ = true;
}
//when counter_ == 10, generate Target Model.
else if (counter_ == 10){
ROS_INFO("Target Model Segment Start");
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr segmented_cloud_(new pcl::PointCloud<pcl::PointXYZRGBA>);
initTargetModel(cloud, segmented_cloud_);
resetTrackingTargetModel(segmented_cloud_);
}
counter_++;
}
