// spin until all counters reach at least max
void MyInfo::waitThreadFunc(
robot_interaction::LockedRobotState* locked_state,
int** counters,
int max)
{
bool go = true;
while(go)
{
go = false;
cnt_lock_.lock();
for (int i = 0 ; counters[i] ; ++i)
{
if (counters[i][0] < max)
go = true;
}
cnt_lock_.unlock();
checkState(*locked_state);
checkState(*locked_state);
}
cnt_lock_.lock();
quit_ = true;
cnt_lock_.unlock();
}
void TimerHandler(
const boost::system::error_code & error,
boost::shared_ptr< boost::asio::deadline_timer > timer,
boost::shared_ptr< boost::asio::io_service::strand > strand
)
{
if( error )
{
global_stream_lock.lock();
std::cout << "[" << boost::this_thread::get_id()
<< "] Error: " << error << std::endl;
global_stream_lock.unlock();
}
else
{
std::cout << "[" << boost::this_thread::get_id()
<< "] TimerHandler " << std::endl;
timer->expires_from_now( boost::posix_time::seconds( 1 ) );
timer->async_wait(
strand->wrap( boost::bind( &TimerHandler, _1, timer, strand ) )
);
}
}
void cmd_vel_received(const geometry_msgs::Twist::ConstPtr& cmd_vel)
{
//roomba->drive(cmd_vel->linear.x, cmd_vel->angular.z);
double linear_speed = cmd_vel->linear.x; // m/s
double angular_speed = cmd_vel->angular.z; // rad/s
ROS_INFO("Velocity received: %.2f %.2f", linear_speed, angular_speed);
//int left_speed_mm_s = (int)((linear_speed-ROBOT_AXLE_LENGTH*angular_speed/2)*1e3); // Left wheel velocity in mm/s
//int right_speed_mm_s = (int)((linear_speed+ROBOT_AXLE_LENGTH*angular_speed/2)*1e3); // Right wheel velocity in mm/s
orcp2::ORCP2 orcp(drive_serial);
drive_serial_write.lock();
if( linear_speed < std::numeric_limits<double>::epsilon() &&
linear_speed > -std::numeric_limits<double>::epsilon() ) {
// zero linear speed - turn in place
uint16_t speed = speed_koef * angular_speed * wheel_track * gear_reduction / 2.0;
orcp.drive_4wd(speed, -speed, speed, -speed);
}
else if( angular_speed < std::numeric_limits<double>::epsilon() &&
angular_speed > -std::numeric_limits<double>::epsilon() ) {
// zero angular speed - pure forward/backward motion
orcp.motorsWrite(speed_koef * linear_speed * wheel_track * gear_reduction / 2.0);
}
else {
// Rotation about a point in space
//$TODO
uint16_t left = speed_koef * linear_speed - angular_speed * wheel_track * gear_reduction / 2.0;
uint16_t right = speed_koef * linear_speed + angular_speed * wheel_track * gear_reduction / 2.0;
orcp.drive_4wd(left, right, left, right);
}
drive_serial_write.unlock();
}
void ServiceImpl::startSimControlTask()
{
assert(serviceThread == NULL);
//get module handles
initModuleHandles();
//get cmd arguments
s_vpi_vlog_info vlog_info;
vpi_get_vlog_info(&vlog_info);
parseOptions(vlog_info.argc, vlog_info.argv);
simMutex.lock();
//start server thread
serviceThread = new boost::thread(boost::bind(&ServiceImpl::serverThreadProc, this));
}
void setFloor(pcl::visualization::PCLVisualizer *viewer, string svm_filename, float voxel_size, Eigen::Matrix3f rgb_intrinsics_matrix, float min_height, float max_height)
{
callback_args cb_args;
PointCloudT::Ptr clicked_points_3d(new PointCloudT);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
viewer->registerPointPickingCallback(pp_callback, (void*)&cb_args);
cout << "Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT";
viewer->addText("Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT", 250, 300, 20, 1, 1, 1);
viewer->addText("Nastepnie nacisnij klawisz Q", 250, 250, 20, 1, 1, 1);
// Spin until 'Q' is pressed:
viewer->spin();
std::cout << "Gotowe." << std::endl;
cloud_mutex.unlock();
// Ground plane estimation:
ground_coeffs.resize(4);
std::vector<int> clicked_points_indices;
for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
clicked_points_indices.push_back(i);
pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
model_plane.computeModelCoefficients(clicked_points_indices, ground_coeffs);
std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;
// Create classifier for people detection:
pcl::people::PersonClassifier<pcl::RGB> person_classifier;
person_classifier.loadSVMFromFile(svm_filename); // load trained SVM
// People detection app initialization:
// people detection object
people_detector.setVoxelSize(voxel_size); // set the voxel size
people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters
people_detector.setClassifier(person_classifier); // set person classifier
// people_detector.setHeightLimits(min_height, max_height); // set person height limits
people_detector.setPersonClusterLimits(min_height, max_height, 0.1, 8.0); // set person height limits
floor_tagged = true;
}
void LaserScan_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
/* Using input from the front laser scanner check for two things:
1. Whether there are not obstacles in front of the robot (close range)
2. To which direction should the robot face to drive without hitting an obstacle (mid-range)
*/
/*
Number of range measurements = 512
Angle of each sensor = about 0.35 degrees (0.00613592332229 radians)
Field of view = about 180 degrees (3.141592741 radians)
*/
// look for a clear corridor of 30 degrees and 1.5 meters ahead
int numPixels = 512;
float pixelAngle = 0.35; // in degrees
if(numPixels!=msg->ranges.size())
cout << "Warning: number of Hokuyo laser sensors different than usual (512)" << endl;
// check that we don't run into any thing
float safeDistance = 0.40; // 40cm
float safetyAngle = 120; // 120 degrees
int numClearPixelsPerSide = (safetyAngle/2)/pixelAngle;
if(!canAdvance(msg->ranges,numClearPixelsPerSide,safeDistance)) {
cout << "Stop, cannot advance" << endl;
dir_value_mutex.lock();
can_move = false;
dir_value_mutex.unlock();
} else {
dir_value_mutex.lock();
can_move = true;
dir_value_mutex.unlock();
}
// find corridor direction
int numCorridorPixels = 120; // 120*0.00613592332229*180/pi=42 degrees
float clearDistance = 1.5; // 1.5 meters ahead
float degreesToTurn = findClearCorridor(msg->ranges,numCorridorPixels,clearDistance,pixelAngle);
// negative means to turn right
dir_value_mutex.lock();
direction = degreesToTurn;
dir_value_mutex.unlock();
cout << "Degrees to turn = " << degreesToTurn << endl;
}
void WorkerThread( boost::shared_ptr< boost::asio::io_service > io_service )
{
global_stream_lock.lock();
std::cout << "[" << boost::this_thread::get_id()
<< "] Thread Start" << std::endl;
global_stream_lock.unlock();
while( true )
{
try
{
boost::system::error_code ec;
io_service->run( ec );
if( ec )
{
global_stream_lock.lock();
std::cout << "[" << boost::this_thread::get_id()
<< "] Error: " << ec << std::endl;
global_stream_lock.unlock();
}
break;
}
catch( std::exception & ex )
{
global_stream_lock.lock();
std::cout << "[" << boost::this_thread::get_id()
<< "] Exception: " << ex.what() << std::endl;
global_stream_lock.unlock();
}
}
global_stream_lock.lock();
std::cout << "[" << boost::this_thread::get_id()
<< "] Thread Finish" << std::endl;
global_stream_lock.unlock();
}
void WorkerThread(boost::shared_ptr<boost::asio::io_service> iosvc, int counter)
{
global_stream_lock.lock();
std::cout << "Thread " << counter << " Start.\n";
global_stream_lock.unlock();
try
{
iosvc->run();
global_stream_lock.lock();
std::cout << "Thread " << counter << " End.\n";
global_stream_lock.unlock();
}
catch(std::exception & ex)
{
global_stream_lock.lock();
std::cout << "Message: " << ex.what() << ".\n";
global_stream_lock.unlock();
}
}
// Check the state. It should always be valid.
void MyInfo::checkState(robot_interaction::LockedRobotState &locked_state)
{
robot_state::RobotStateConstPtr s = locked_state.getState();
robot_state::RobotState cp1(*s);
// take some time
cnt_lock_.lock();
cnt_lock_.unlock();
cnt_lock_.lock();
cnt_lock_.unlock();
cnt_lock_.lock();
cnt_lock_.unlock();
// check mim_f == joint_f
EXPECT_EQ(s->getVariablePositions()[MIM_F],
s->getVariablePositions()[JOINT_F] * 1.5 + 0.1);
robot_state::RobotState cp2(*s);
EXPECT_NE(cp1.getVariablePositions(), cp2.getVariablePositions());
EXPECT_NE(cp1.getVariablePositions(), s->getVariablePositions());
int cnt = cp1.getVariableCount();
for (int i = 0 ; i < cnt ; ++i)
{
EXPECT_EQ(cp1.getVariablePositions()[i],
cp2.getVariablePositions()[i]);
EXPECT_EQ(cp1.getVariablePositions()[i],
s->getVariablePositions()[i]);
}
// check mim_f == joint_f
EXPECT_EQ(s->getVariablePositions()[MIM_F],
s->getVariablePositions()[JOINT_F] * 1.5 + 0.1);
}
modbus_t *getContext() {
update_mutex.lock();
return ctx;
}
void AutoTune::start()
{
// vector<double> inputRates={ 0.630478,0.996342,0.729125,0.523573,0.276309,0.053071,0.00143688,0.431644};
// double res;
// EvaluateResult(inputRates, res);
#if 1
this->workerSequence.resize(this->workerNum,NULL);
vector<double> currResult(this->workerNum,-1.0);
//construct parameter array.
vector<Point3d> paramArray;
for (double pCrit = pCritMin; pCrit <= pCritMax; pCrit+=critStep) {
for (double nCrit = nCritMin; nCrit <= nCritMax; nCrit+=critStep) {
for (double epsilon = epsilonMin; epsilon <= epsilonMax; epsilon+=epsilonStep) {
Point3d newParam;
newParam.x = pCrit;
newParam.y = nCrit;
newParam.z = epsilon;
paramArray.push_back(newParam);
}
}
}
LOG(INFO)<<"paramArray size = "<<paramArray.size();
while (!paramArray.empty()) {
for (int i=0; i<workerNum; i++) {
if(NULL == workerSequence[i])
{
if(paramArray.empty()) break;
Point3d param = paramArray.back();
paramArray.pop_back();
this->workingParams[i] = param;
#if( defined(WIN32) && _MSC_VER<=1700)//before VS2012, the variadic template is not fully supported.
this->workerSequence[i] = new boost::thread(&AutoTune::WorkWithParameters,this,i,param.x,param.y,param.z);
#else
this->workerSequence[i] = new std::thread(&AutoTune::WorkWithParameters,this,i,param.x,param.y,param.z);
#endif
}else{
workerSequence[i]->join();
delete workerSequence[i];
//write result
myLock.lock();
if(accuracies[i]>0.8)
{
fstream fs("result.txt",ios::out|ios::app);
fs<<"Result = "<<this->accuracies[i]<<" parameters = "<<workingParams[i]<<" weights = ";
for (int j=0; j<dim; j++) {
fs<<this->weightResults[i][j]<<" ";
}
fs<<endl;
fs.close();
}
myLock.unlock();
//create new worker.
if(paramArray.empty()) break;
Point3d param = paramArray.back();
paramArray.pop_back();
this->workingParams[i] = param;
#if( defined(WIN32) && _MSC_VER<=1700)//before VS2012, the variadic template is not fully supported.
this->workerSequence[i] = new boost::thread(&AutoTune::WorkWithParameters,this,i,param.x,param.y,param.z);
#else
this->workerSequence[i] = new std::thread(&AutoTune::WorkWithParameters,this,i,param.x,param.y,param.z);
#endif
}
}
}
#endif
}
void callbackClusteredClouds(const clustered_clouds_msgs::ClusteredCloudsConstPtr& msg)
{
#if PUBLISH_TRANSFORM
static tf::TransformBroadcaster tf_br;
#endif
g_marker_array.markers.clear();
g_marker_id = 0;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<tf::Vector3> hand_positions, arm_directions;
mutex_config.lock();
for(size_t i = 0; i < msg->clouds.size(); i++)
{
bool cloud_with_rgb_data = false;
std::string field_list = pcl::getFieldsList (msg->clouds[i]);
if(field_list.rfind("rgb") != std::string::npos)
{
cloud_with_rgb_data = true;
}
if(cloud_with_rgb_data)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr hand_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr finger_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
tf::Vector3 hand_position, arm_position, arm_direction;
bool found = checkCloud<pcl::PointXYZRGB>(msg->clouds[i],
hand_cloud,
//finger_cloud,
msg->header.frame_id,
hand_position,
arm_position,
arm_direction);
if(found)
{
hand_positions.push_back(hand_position);
arm_directions.push_back(arm_direction);
*cloud_out += *hand_cloud;
}
}
//else
//{
//checkCloud<pcl::PointXYZ>(msg->clouds[i], msg->header.frame_id);
//}
}
mutex_config.unlock();
if(hand_positions.size() > g_hand_trackers.size())
{
std::vector<tf::Vector3> hand_positions_tmp = hand_positions;
cv::Mat result;
for (size_t i = 0; i < g_hand_trackers.size(); i++)
{
result = g_hand_trackers[i].first.lastResult();
tf::Vector3 point1(result.at<float>(0), result.at<float>(1), result.at<float>(2));
int index = closestPoint(point1, hand_positions_tmp);
//remove
hand_positions_tmp.erase(hand_positions_tmp.begin() + index);
}
//add new tracker
for(size_t i = 0; i < hand_positions_tmp.size(); i++)
{
KalmanFilter3d tracker;
cv::Point3f point(hand_positions_tmp[i].x(),
hand_positions_tmp[i].y(),
hand_positions_tmp[i].z());
tracker.initialize(1.0 / 30.0, point, 1e-2, 1e-1, 1e-1);
TrackerWithHistory t;
t.first = tracker;
g_hand_trackers.push_back(t);
}
}
cv::Mat result;
std::vector<tf::Vector3> results;
interaction_msgs::ArmsPtr arms_msg(new interaction_msgs::Arms);
arms_msg->arms.resize(g_hand_trackers.size());
for (size_t i = 0; i < g_hand_trackers.size(); i++)
{
g_hand_trackers[i].first.predict(result);
results.push_back(tf::Vector3(result.at<float>(0), result.at<float>(1), result.at<float>(2)));
arms_msg->arms[i].hand.rotation.w = 1;
}
int index;
cv::Point3f measurement;
tf::Quaternion q_hand;
Eigen::Quaternionf q;
tf::Quaternion q_rotate;
q_rotate.setEuler(0, 0, M_PI);
int last_index = -1;
for(size_t i = 0; i < hand_positions.size(); i++)
{
index = closestPoint(hand_positions[i], results);
if(last_index == index)
continue;
last_index = index;
measurement.x = hand_positions[i].x();
measurement.y = hand_positions[i].y();
measurement.z = hand_positions[i].z();
g_hand_trackers[index].first.update(measurement, result);
results[index] = tf::Vector3(result.at<float>(0), result.at<float>(1), result.at<float>(2));
q.setFromTwoVectors(Eigen::Vector3f(1, 0, 0),
Eigen::Vector3f(arm_directions[i].x(), arm_directions[i].y(), arm_directions[i].z()));
tf::Quaternion q_tf(q.x(), q.y(), q.z(), q.w());
q_hand = q_tf * q_rotate;
arms_msg->arms[index].hand.rotation.x = q_hand.x();
arms_msg->arms[index].hand.rotation.y = q_hand.y();
arms_msg->arms[index].hand.rotation.z = q_hand.z();
arms_msg->arms[index].hand.rotation.w = q_hand.w();
}
for(size_t i = 0; i < results.size(); i++)
{
g_hand_trackers[i].first.updateState();
arms_msg->arms[i].arm_id = g_hand_trackers[i].first.id();
arms_msg->arms[i].hand.translation.x = results[i].x();
arms_msg->arms[i].hand.translation.y = results[i].y();
arms_msg->arms[i].hand.translation.z = results[i].z();
//prepare markers if needed
if(g_marker_array_pub.getNumSubscribers() != 0)
{
geometry_msgs::Point marker_point;
marker_point.x = results[i].x();
marker_point.y = results[i].y();
marker_point.z = results[i].z();
g_hand_trackers[i].second.push_back(marker_point);
if(g_hand_trackers[i].second.size() > HAND_HISTORY_SIZE)
g_hand_trackers[i].second.pop_front();
}
#if PUBLISH_TRANSFORM
tf::Transform hand_tf;
hand_tf.setOrigin(results[i]);
if(index != -1)
{
hand_tf.setRotation(tf::Quaternion(arms_msg->arms[i].hand.rotation.x,
arms_msg->arms[i].hand.rotation.y,
arms_msg->arms[i].hand.rotation.z,
arms_msg->arms[i].hand.rotation.w));
}
else
{
hand_tf.setRotation(tf::Quaternion(0, 0, 0, 1));
}
std::stringstream ss;
ss << "hand" << g_hand_trackers[i].first.id();
tf_br.sendTransform(tf::StampedTransform(hand_tf, ros::Time::now(), msg->header.frame_id, ss.str()));
#endif
}
//remove dead tracker
std::vector<TrackerWithHistory>::iterator it = g_hand_trackers.begin();
while(it != g_hand_trackers.end())
{
if(it->first.getState() == KalmanFilter3d::DIE)
{
ROS_DEBUG("remove tracker %d", it->first.id());
it = g_hand_trackers.erase(it);
}
else
{
it++;
}
}
//prepare markers if needed
if(g_marker_array_pub.getNumSubscribers() != 0)
{
Marker marker;
marker.type = Marker::LINE_STRIP;
marker.lifetime = ros::Duration(0.1);
marker.header.frame_id = msg->header.frame_id;
marker.scale.x = 0.005;
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0;
marker.pose.orientation.y = 0;
marker.pose.orientation.z = 0;
marker.pose.orientation.w = 1;
marker.ns = "point_history";
for (size_t i = 0; i < g_hand_trackers.size(); i++)
{
marker.points.clear();
if (g_hand_trackers[i].second.size() != 0)
{
marker.id = g_marker_id++;
std::list<geometry_msgs::Point>::iterator it;
for (it = g_hand_trackers[i].second.begin(); it != g_hand_trackers[i].second.end(); it++)
{
marker.points.push_back(*it);
}
marker.color.r = 1;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 1.0;
g_marker_array.markers.push_back(marker);
}
}
}
if(g_arms_pub.getNumSubscribers() != 0)
{
arms_msg->header.stamp = ros::Time::now();
arms_msg->header.frame_id = msg->header.frame_id;
g_arms_pub.publish(arms_msg);
}
if(g_cloud_pub.getNumSubscribers() != 0)
{
sensor_msgs::PointCloud2 cloud_out_msg;
pcl::toROSMsg(*cloud_out, cloud_out_msg);
cloud_out_msg.header.stamp = ros::Time::now();
cloud_out_msg.header.frame_id = msg->header.frame_id;
g_cloud_pub.publish(cloud_out_msg);
}
if((g_marker_array_pub.getNumSubscribers() != 0) && (!g_marker_array.markers.empty()))
{
g_marker_array_pub.publish(g_marker_array);
}
}
// Called when a thread wants to indicate that it has started.
void threadStarted()
{
runningLock.lock();
runningThreads++;
runningLock.unlock();
}
void imageCallback(const sensor_msgs::ImageConstPtr & msg){
#ifdef PRINT_ROS_INFO
ROS_INFO("Got image message.");
#endif
// get the compressed image, and convert it to Opencv format.
cv::Mat img;
try{
img = cv_bridge::toCvShare(msg, "bgr8")->image;
}
catch(cv_bridge::Exception & e){
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
#ifdef PRINT_ROS_INFO
ROS_INFO("Converting image done.");
#endif
//std::cout << "image size = ( " << img.rows << " X " << img.cols << " )." << std::endl;
//printf("image data address 0x%x\n", img.data);
if( startTracking ){
trackerMutex.lock();
#ifdef PRINT_ROS_INFO
ROS_INFO("Tracker: Reading Frame ... ");
#endif
// update the tracking status, and draw the result.
tracker->readFrame(img);
#ifdef PRINT_ROS_INFO
ROS_INFO("Tracker: Updating status ... ");
#endif
tracker->updateTrackerStatus();
#ifdef PRINT_ROS_INFO
ROS_INFO("Tracker: status updated ... ");
ROS_INFO("Tracker: drawing ... ");
#endif
cv::Mat temp;
img.copyTo(temp);
tracker->drawTrackers(temp);
#ifdef PRINT_ROS_INFO
ROS_INFO("Tracker: Publishing ... ");
#endif
// republish this image.
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", temp).toImageMsg();
pub.publish(msg);
// publish to topic -- object_location
cv::Point2i location = tracker->getWeightedAverageLocation();
std::stringstream locationStrStream;
int currentNum = tracker->getSampleNum();
int numWithHigConfidence = tracker->getNumOfSamplesHasProbLargerThan(PROB_THRESHOD);
float highConfidenceSampleRatio;
if( currentNum <= 0){
highConfidenceSampleRatio = 0;
}else{
highConfidenceSampleRatio = numWithHigConfidence * 1.0f / currentNum;
}
std::cout << "High confidence sample ratio = " << highConfidenceSampleRatio << std::endl;
if( location.x < 0 || location.y < 0 || highConfidenceSampleRatio <= HIGH_CONFID_NUM_RATIO_THRESHOLD ){
//locationStrStream << "object_x " << "0" << " , " << "object_y " << "0" << " , ";
locationStrStream << "object_x " << img.cols /2 << ", " << "object_y " << img.rows / 2 << ", ";
// make offsets to the samples:
ROS_INFO("Tracker offset!");
if( lastMovementDirection == TRACKER_UP)
tracker->offsetTracker(TRACKER_DOWN);
else if( lastMovementDirection == TRACKER_DOWN)
tracker->offsetTracker(TRACKER_UP);
else if( lastMovementDirection == TRACKER_LEFT)
tracker->offsetTracker(TRACKER_RIGHT);
else if( lastMovementDirection == TRACKER_RIGHT)
tracker->offsetTracker(TRACKER_LEFT);
}else{
// "x 10, y 10, width 360, height 640"
locationStrStream << "object_x " << location.x << ", " << "object_y " << location.y << ", ";
lastMovementDirection = -1;
}
locationStrStream << "width " << img.cols << ", " << "height " << img.rows << ", ";
locationStrStream << "direction follow" ;
std_msgs::String locationMsg;
locationMsg.data = locationStrStream.str();
location_pub.publish(locationMsg);
// release the lock
trackerMutex.unlock();
}
else if (! objectSpecified ) {
// show the image and let the user specify the inital result.
//std::cout << img.rows << "," << img.cols << std::endl;
cv::Mat tempImage(img.rows, img.cols, img.type());
img.copyTo(tempImage);
if( trackerMaxSize < 0){
trackerMaxSize = MIN(img.rows, img.cols) - 1;
}
ROS_INFO("Drawing tracker ... ");
cv::rectangle(tempImage, cv::Rect(tempImage.cols / 2 - trackerSize / 2, tempImage.rows / 2 - trackerSize / 2, trackerSize, trackerSize), cv::Scalar(0,0,255));
// republish this image.
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", tempImage).toImageMsg();
pub.publish(msg);
}
else{
trackerMutex.lock();
// haven't started tracking, but the initial object is specified.
// create a tracker;
if( tracker != NULL) delete tracker;
tracker = new Tracker2D();
tracker->setLimit(5 , img.cols - 5, 5 , img.rows - 5);
tracker->initialize(cv::Point2f(img.cols / 2, img.rows / 2), trackerSize);
// set object feature.
cv::Mat objImage = img(cv::Rect(img.cols / 2 - trackerSize /2, img.rows / 2 - trackerSize /2, trackerSize, trackerSize));
tracker->setObjectFeature(objImage);
ROS_INFO("Starting Tracking ... " );
startTracking = true;
trackerMutex.unlock();
}
}
void threadHandler(char driveLtr)
{
/* Lock the mutex so we don't try and access the database at the same time */
gMutex.lock();
bool authorized = false;
/* Lock the USB drive */
UsbOps ops;
ops.lockUSB(driveLtr);
/* Query authorized devices */
Sql sql;
if (!sql.dbConnect((char*)Paths::getDatabasePath().c_str(), false))
{
ErrorLog::logErrorToFile("*CRITICAL*", "Unable to open authorized drives database!");
ops.ejectUSB();
gMutex.unlock();
return;
}
std::vector<authedDrive> drvs;
sql.queryAuthedDrives(&drvs);
/* Get the serial key of the device */
UsbKey usbKey;
UsbKeyhdr hdr;
ops.unlockUSB();
usbKey.getUsbKeyHdr(&hdr, driveLtr);
ops.lockUSB(driveLtr);
/* Check if the serial exists in the database */
for (std::vector<authedDrive>::iterator it = drvs.begin(); it != drvs.end(); it++)
{
std::cout << it->serial.c_str() << " " << hdr.serialkey.c_str() << std::endl;
if (it->serial.compare(hdr.serialkey) == 0)
{
authorized = true;
break;
}
}
ops.unlockUSB();
/* Log media insertion event */
AccessLog *log = new AccessLog();
log->createLogStruct(&log->logUSBStruct, driveLtr, (char*)hdr.serialkey.c_str());
log->logUSBStruct.accepted = authorized;
/* Get config settings, check if remote SQL is enabled */
ConfigParser configParser((char*)Paths::getConfigPath().c_str());
if (configParser.getValue("SQLenabled") == "1")
{
/* SQL enabled = true */
log->logUsbDrive(log->logUSBStruct, true);
}
else
log->logUsbDrive(log->logUSBStruct, false);
/* If not authorized, eject it! */
if (!authorized)
{
ops.lockUSB(driveLtr);
ops.ejectUSB();
}
/* Check for viruses here... */
gMutex.unlock();
delete log;
}
/** Callback: On recalc local map & publish it */
void onDoPublish(const ros::TimerEvent& )
{
mrpt::utils::CTimeLoggerEntry tle(m_profiler,"onDoPublish");
// Purge old observations & latch a local copy:
TListObservations obs;
{
mrpt::utils::CTimeLoggerEntry tle(m_profiler,"onDoPublish.removingOld");
m_hist_obs_mtx.lock();
// Purge old obs:
if (!m_hist_obs.empty())
{
const double last_time = m_hist_obs.rbegin()->first;
TListObservations::iterator it_first_valid = m_hist_obs.lower_bound( last_time-m_time_window );
const size_t nToRemove = std::distance( m_hist_obs.begin(), it_first_valid );
ROS_DEBUG("[onDoPublish] Removing %u old entries, last_time=%lf",static_cast<unsigned int>(nToRemove), last_time);
m_hist_obs.erase(m_hist_obs.begin(), it_first_valid);
}
// Local copy in this thread:
obs = m_hist_obs;
m_hist_obs_mtx.unlock();
}
ROS_DEBUG("Building local map with %u observations.",static_cast<unsigned int>(obs.size()));
if (obs.empty())
return;
// Build local map(s):
// -----------------------------------------------
m_localmap_pts.clear();
mrpt::poses::CPose3D curRobotPose;
{
mrpt::utils::CTimeLoggerEntry tle2(m_profiler,"onDoPublish.buildLocalMap");
// Get the latest robot pose in the reference frame (typ: /odom -> /base_link)
// so we can build the local map RELATIVE to it:
try {
tf::StampedTransform tx;
m_tf_listener.lookupTransform(m_frameid_reference,m_frameid_robot, ros::Time(0), tx);
mrpt_bridge::convert(tx,curRobotPose);
ROS_DEBUG("[onDoPublish] Building local map relative to latest robot pose: %s", curRobotPose.asString().c_str() );
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
return;
}
// For each observation: compute relative robot pose & insert obs into map:
for (TListObservations::const_iterator it=obs.begin();it!=obs.end();++it)
{
const TInfoPerTimeStep & ipt = it->second;
// Relative pose in the past:
mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE);
relPose.inverseComposeFrom( ipt.robot_pose, curRobotPose );
// Insert obs:
m_localmap_pts.insertObservationPtr(ipt.observation,&relPose);
} // end for
}
// Publish them:
if (m_pub_local_map_pointcloud.getNumSubscribers()>0)
{
sensor_msgs::PointCloudPtr msg_pts = sensor_msgs::PointCloudPtr( new sensor_msgs::PointCloud);
msg_pts->header.frame_id = m_frameid_robot;
msg_pts->header.stamp = ros::Time( obs.rbegin()->first );
mrpt_bridge::point_cloud::mrpt2ros(m_localmap_pts,msg_pts->header, *msg_pts);
m_pub_local_map_pointcloud.publish(msg_pts);
}
// Show gui:
if (m_show_gui)
{
if (!m_gui_win)
{
m_gui_win = mrpt::gui::CDisplayWindow3D::Create("LocalObstaclesNode",800,600);
mrpt::opengl::COpenGLScenePtr &scene = m_gui_win->get3DSceneAndLock();
scene->insert( mrpt::opengl::CGridPlaneXY::Create() );
scene->insert( mrpt::opengl::stock_objects::CornerXYZSimple(1.0,4.0) );
mrpt::opengl::CSetOfObjectsPtr gl_obs = mrpt::opengl::CSetOfObjects::Create();
gl_obs->setName("obstacles");
scene->insert( gl_obs );
mrpt::opengl::CPointCloudPtr gl_pts = mrpt::opengl::CPointCloud::Create();
gl_pts->setName("points");
gl_pts->setPointSize(2.0);
gl_pts->setColor_u8( mrpt::utils::TColor(0x0000ff) );
scene->insert( gl_pts );
m_gui_win->unlockAccess3DScene();
}
mrpt::opengl::COpenGLScenePtr &scene = m_gui_win->get3DSceneAndLock();
mrpt::opengl::CSetOfObjectsPtr gl_obs = mrpt::opengl::CSetOfObjectsPtr( scene->getByName("obstacles") );
ROS_ASSERT(gl_obs.present());
gl_obs->clear();
mrpt::opengl::CPointCloudPtr gl_pts = mrpt::opengl::CPointCloudPtr( scene->getByName("points") );
for (TListObservations::const_iterator it=obs.begin();it!=obs.end();++it)
{
const TInfoPerTimeStep & ipt = it->second;
// Relative pose in the past:
mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE);
relPose.inverseComposeFrom( ipt.robot_pose, curRobotPose );
mrpt::opengl::CSetOfObjectsPtr gl_axis = mrpt::opengl::stock_objects::CornerXYZSimple(0.9,2.0);
gl_axis->setPose(relPose);
gl_obs->insert(gl_axis);
} // end for
gl_pts->loadFromPointsMap(&m_localmap_pts);
m_gui_win->unlockAccess3DScene();
m_gui_win->repaint();
}
} // onDoPublish
int main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::parse (argc, argv, "-d", device_id) >= 0)
cout << "Using device id \""<<device_id<<"\".\n";
if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution = pcl::deg2rad (angular_resolution);
if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
cout << "Setting support size to "<<support_size<<"m.\n";
if (pcl::console::parse (argc, argv, "-i", min_interest_value) >= 0)
cout << "Setting minimum interest value to "<<min_interest_value<<".\n";
if (pcl::console::parse (argc, argv, "-t", max_no_of_threads) >= 0)
cout << "Setting maximum number of threads to "<<max_no_of_threads<<".\n";
pcl::visualization::RangeImageVisualizer range_image_widget ("Range Image");
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.addCoordinateSystem (1.0f);
viewer.setBackgroundColor (1, 1, 1);
viewer.initCameraParameters ();
viewer.camera_.pos[0] = 0.0;
viewer.camera_.pos[1] = -0.3;
viewer.camera_.pos[2] = -2.0;
viewer.camera_.focal[0] = 0.0;
viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
viewer.camera_.focal[2] = 1.0;
viewer.camera_.view[0] = 0.0;
viewer.camera_.view[1] = -1.0;
viewer.camera_.view[2] = 0.0;
viewer.updateCamera ();
openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
if (driver.getNumberDevices () > 0)
{
for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
{
cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx)
<< ", product: " << driver.getProductName (deviceIdx) << ", connected: "
<< (int) driver.getBus (deviceIdx) << " @ " << (int) driver.getAddress (deviceIdx)
<< ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'\n";
}
}
else
{
cout << "\nNo devices connected.\n\n";
return 1;
}
pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
EventHelper event_helper;
boost::function<void (const boost::shared_ptr<openni_wrapper::DepthImage>&) > f_depth_image =
boost::bind (&EventHelper::depth_image_cb, &event_helper, _1);
boost::signals2::connection c_depth_image = interface->registerCallback (f_depth_image);
cout << "Starting grabber\n";
interface->start ();
cout << "Done\n";
boost::shared_ptr<pcl::RangeImagePlanar> range_image_planar_ptr (new pcl::RangeImagePlanar);
pcl::RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
pcl::RangeImageBorderExtractor range_image_border_extractor;
pcl::NarfKeypoint narf_keypoint_detector;
narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);
narf_keypoint_detector.getParameters ().support_size = support_size;
narf_keypoint_detector.getParameters ().max_no_of_threads = max_no_of_threads;
narf_keypoint_detector.getParameters ().min_interest_value = min_interest_value;
//narf_keypoint_detector.getParameters ().calculate_sparse_interest_image = false;
//narf_keypoint_detector.getParameloadters ().add_points_on_straight_edges = true;
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>& keypoints_cloud = *keypoints_cloud_ptr;
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
range_image_widget.spinOnce (); // process GUI events
pcl_sleep (0.01);
bool got_new_range_image = false;
if (received_new_depth_data && depth_image_mutex.try_lock ())
{
received_new_depth_data = false;
//unsigned long time_stamp = depth_image_ptr->getTimeStamp ();
//int frame_id = depth_image_ptr->getFrameID ();
const unsigned short* depth_map = depth_image_ptr->getDepthMetaData ().Data ();
int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
float center_x = width/2, center_y = height/2;
float focal_length_x = depth_image_ptr->getFocalLength (), focal_length_y = focal_length_x;
float original_angular_resolution = asinf (0.5f*float (width)/float (focal_length_x)) / (0.5f*float (width));
float desired_angular_resolution = angular_resolution;
range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
focal_length_x, focal_length_y, desired_angular_resolution);
depth_image_mutex.unlock ();
got_new_range_image = !range_image_planar.points.empty ();
}
if (!got_new_range_image)
continue;
// --------------------------------
// -----Extract NARF keypoints-----
// --------------------------------
if (set_unseen_to_max_range)
range_image_planar.setUnseenToMaxRange ();
narf_keypoint_detector.setRangeImage (&range_image_planar);
pcl::PointCloud<int> keypoint_indices;
double keypoint_extraction_start_time = pcl::getTime();
narf_keypoint_detector.compute (keypoint_indices);
double keypoint_extraction_time = pcl::getTime()-keypoint_extraction_start_time;
std::cout << "Found "<<keypoint_indices.points.size ()<<" key points. "
<< "This took "<<1000.0*keypoint_extraction_time<<"ms.\n";
// ----------------------------------------------
// -----Show keypoints in range image widget-----
// ----------------------------------------------
range_image_widget.showRangeImage (range_image_planar, 0.5f, 10.0f);
//for (size_t i=0; i<keypoint_indices.points.size (); ++i)
//range_image_widget.markPoint (keypoint_indices.points[i]%range_image_planar.width,
//keypoint_indices.points[i]/range_image_planar.width,
//pcl::visualization::Vector3ub (0,255,0));
// -------------------------------------
// -----Show keypoints in 3D viewer-----
// -------------------------------------
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud
(range_image_planar_ptr, 0, 0, 0);
if (!viewer.updatePointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image"))
viewer.addPointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
keypoints_cloud.points.resize (keypoint_indices.points.size ());
for (size_t i=0; i<keypoint_indices.points.size (); ++i)
keypoints_cloud.points[i].getVector3fMap () =
range_image_planar.points[keypoint_indices.points[i]].getVector3fMap ();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler_keypoints
(keypoints_cloud_ptr, 0, 255, 0);
if (!viewer.updatePointCloud (keypoints_cloud_ptr, color_handler_keypoints, "keypoints"))
viewer.addPointCloud (keypoints_cloud_ptr, color_handler_keypoints, "keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
}
interface->stop ();
}
static Result execute_testcase (struct InfoForExecutor &ie)
{
pthread_t tid = pthread_self ();
LoggerPtr logger = Logger :: getLogger (HARNESS_LOGGER_NAME);
string tid_str = iTos ((uint64_t)tid);
string worker_tag = LOGGER_TAG_WORKER;
if (ie.selftesting == false)
worker_tag = worker_tag + '[' + tid_str + ']';
else
worker_tag = worker_tag + "[]";
LogString saved_context;
LOGGER_PUSH_NDCTAG (worker_tag);
int retValue;
Result result = RESULT_SYSTEM_EXCEPTION;
string result_str = "FAILED";
long int sTime = 0, eTime = 0;
string failureReason("Test case execution failed, Check log file.");
scidbtestharness::executors::ExecutorFactory f;
scidbtestharness :: executors :: Executor *caseexecutor = 0;
try
{
prepare_filepaths (ie, true);
string actualrfile_backup = ie.actual_rfile + ".bak";
string difffile_backup = ie.diff_file + ".bak";
string logfile_backup = ie.log_file + ".bak";
if (ie.keepPreviousRun)
{
/* rename all the files with extra extension .bak */
if (Is_regular (ie.actual_rfile))
{
bfs::remove (actualrfile_backup);
bfs::rename (ie.actual_rfile, actualrfile_backup);
}
if (Is_regular (ie.diff_file))
{
bfs::remove (difffile_backup);
bfs::rename (ie.diff_file, difffile_backup);
}
if (Is_regular (ie.log_file))
{
bfs::remove (logfile_backup);
bfs::rename (ie.log_file, logfile_backup);
}
}
else
{
/* remove actual files as well as backup files */
bfs::remove (ie.actual_rfile);
bfs::remove (ie.diff_file);
bfs::remove (ie.log_file);
bfs::remove (actualrfile_backup);
bfs::remove (difffile_backup);
bfs::remove (logfile_backup);
}
if ((caseexecutor = f.getExecutor (g_executor_type)) == NULL)
{
throw ConfigError (FILE_LINE_FUNCTION, ERR_CONFIG_INVALID_EXECUTOR_TYPE);
}
sTime = time (0);
ie.logger_name = tid_str;
complete_es_mutex.lock(); /* lock */
g_test_count++;
ie.test_sequence_number = g_test_count;
ie.tid = tid;
ie.testID = converttoid (ie.rootDir, ie.tcfile);
complete_es_mutex.unlock(); /* unlock */
/* test case execution by the executor.
* Now all the exceptions from 'defaultexecutor' are being locally handled by it and
* only return code SUCCESS/FAILURE is only being returned. Hence resolving the issue of crash
* during pthread_mutex_destroy() at the end of harness execution. */
retValue = caseexecutor->execute (ie);
eTime = time (0);
delete caseexecutor;
caseexecutor = 0;
LOG4CXX_DEBUG (logger, "Executor returned : " << (retValue == SUCCESS ? "SUCCESS" : "FAILURE"));
/* lock */
complete_es_mutex.lock();
if (retValue == SUCCESS)
{
if (ie.record) // PASS
{
bfs::remove (ie.expected_rfile);
bfs::rename (ie.actual_rfile, ie.expected_rfile);
result = RESULT_RECORDED;
result_str = "RECORDED";
failureReason = "";
complete_es.testcasesPassed++;
}
else
{
int rv;
LOG4CXX_DEBUG (logger, "Going to compare the files now.");
if ((rv = diff (ie.expected_rfile, ie.actual_rfile, ie.diff_file)) == DIFF_FILES_MATCH) // PASS
{
LOG4CXX_DEBUG (logger, "Files Match");
result = RESULT_PASS;
result_str = "PASS";
failureReason = "";
complete_es.testcasesPassed++;
}
else if (rv == DIFF_FILES_DIFFER) // FAIL
{
LOG4CXX_DEBUG (logger, "Files Differ");
result = RESULT_FILES_DIFFER;
result_str = "FILES_DIFFER";
failureReason = "Expected output and Actual Output differ. Check .diff file.";
complete_es.testcasesFailed++;
}
else // SYSTEM_EXCEPTION
{
LOG4CXX_DEBUG (logger, "Either \"diff\" command failed or some other problem");
result = RESULT_SYSTEM_EXCEPTION;
result_str = "DIFF_COMMAND_FAILED";
failureReason = "Either \"diff\" command failed or some other problem";
complete_es.testcasesFailed++;
}
}
}
else // ANY EXCEPTION, error
{
/* ANY EXCEPTION, error because of which executor failed to execute the test case
* primarily because errors like, problem in opening file, .test file parsing error, failure in connecting to scidb etc.*/
result = RESULT_SYSTEM_EXCEPTION;
result_str = "EXECUTOR_FAILED";
LOG4CXX_ERROR (logger, "Test case execution failed. Canceling further execution of this test case. Check respective log file.");
failureReason = "Test case execution failed. Check log file.";
complete_es.testcasesFailed++;
}
} // END try
catch (harnessexceptions :: SystemError &e)// SYSTEM_EXCEPTION
{
/* errors like, .test file does not exists or could not be opened etc.*/
eTime = time (0);
LOG4CXX_ERROR (logger, e.what ());
result = RESULT_SYSTEM_EXCEPTION;
result_str = "FAILED_ON_EXCEPTION";
LOG4CXX_ERROR (logger, "Worker failed to execute the job completely.");
failureReason = "Worker failed to execute the job completely. Check log file.";
complete_es.testcasesFailed++;
}
# if 0
catch (harnessexceptions :: ConfigError &e)// CONFIG_EXCEPTION
{
/* TODO : this catch() is not required now as all the exceptions from 'defaultexecutor'
* are being locally handled by it and only return code SUCCESS/FAILURE is only being returned.*/
/* errors like, connection string is not given or port number is not valid etc.*/
eTime = time (0);
LOG4CXX_ERROR (logger, e.what ());
result = RESULT_CONFIG_EXCEPTION;
result_str = "FAILED_ON_EXCEPTION";
LOG4CXX_ERROR (logger, "Worker failed to execute the job completely.");
failureReason = "Worker failed to execute the job completely. Check log file.";
complete_es.testcasesFailed++;
}
catch (harnessexceptions :: ExecutorError &e)// SYSTEM_EXCEPTION
{
/* TODO : this catch() is not required now as all the exceptions from 'defaultexecutor'
* are being locally handled by it and only return code SUCCESS/FAILURE is only being returned.*/
/* most of the exceptions thrown by CAPIs */
eTime = time (0);
if (caseexecutor)
{
delete caseexecutor;
caseexecutor = 0;
}
LOG4CXX_ERROR (logger, e.what ());
result = RESULT_SYSTEM_EXCEPTION;
result_str = "FAILED_ON_EXECUTOR_EXCEPTION";
LOG4CXX_ERROR (logger, "Test case execution failed because of some Executor exception. Canceling further execution of this test case."
" Check respective log file.");
failureReason = "Test case execution failed because of some Executor exception. Check log file.";
complete_es.testcasesFailed++;
}
# endif
g_test_ei.testID = ie.testID;
g_test_ei.description = "";
g_test_ei.sTime = sTime;
g_test_ei.eTime = eTime;
g_test_ei.result = result;
g_test_ei.failureReason = failureReason;
if (ie.expected_rfile.length() > 0 && !bfs::exists (ie.expected_rfile))
ie.expected_rfile = "";
if (ie.actual_rfile.length() > 0 && !bfs::exists (ie.actual_rfile))
ie.actual_rfile = "";
if (ie.timerfile.length() > 0 && !bfs::exists (ie.timerfile))
ie.timerfile = "";
if (ie.diff_file.length() > 0 && !bfs::exists (ie.diff_file))
ie.diff_file = "";
if (ie.log_file.length() > 0 && !bfs::exists (ie.log_file))
ie.log_file = "";
time_t rawtime;
time ( &rawtime );
std::string _ctime = ctime (&rawtime);
_ctime = _ctime.substr(0,_ctime.size()-1);
/* throw this line on console only if other log is going to the harness.log file */
if (strcasecmp (ie.logDestination.c_str (), LOGDESTINATION_CONSOLE) != 0)
cout << "[" << ie.test_sequence_number << "][" << _ctime << "]: " << ie.testID << " .............................................................. " << result_str << endl << std::flush;
else
LOG4CXX_INFO (logger, ie.testID << " .............................................................. " << result_str);
IndividualTestInfo ti(ie, g_test_ei);
g_rptr->writeTestcaseExecutionInfo (ti);
g_rptr->writeIntermediateRunStat (complete_es.testcasesPassed, complete_es.testcasesFailed);
/* unlock */
complete_es_mutex.unlock();
LOGGER_POP_NDCTAG;
return result;
}
void Run()
{ //cout << "here0" << endl;
int w, h;
// Create camera instances
_cam = CLEyeCreateCamera(_cameraGUID, _mode, _resolution, _fps);
if(_cam == NULL) return;
// Get camera frame dimensions
CLEyeCameraGetFrameDimensions(_cam, w, h);
IplImage *pCapImage;
// Create the OpenCV images
pCapImage = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, _isColor ? 4 : 1);
// Set some camera parameters
CLEyeSetCameraParameter(_cam, CLEYE_GAIN, 0);
CLEyeSetCameraParameter(_cam, CLEYE_EXPOSURE, 511);
// Start capturing
CLEyeCameraStart(_cam);
Sleep(100);
tMin = 1000;
tMax = 0;
tAvg = 0;
curr = 0;
measuredCnt = 0;
printf("\n");
//cout << "here1" << endl;
// Capture camera images
PBYTE tempBuffer;
cvGetImageRawData(pCapImage, &tempBuffer);
CLEyeCameraGetFrame(_cam, tempBuffer);
Mat imgTemp(pCapImage);
mtx.lock();
imgLines = Mat::zeros( imgTemp.size(), CV_8UC4 );
imgCapture = Mat::zeros( imgTemp.size(), CV_8UC4 );
mtx.unlock();
//cout << "here2" << endl;
// image capturing loop
while(_running)
{
//oldFrameTimestamp = frameTimestamp;
//oldFrameTimestamp = getTickCount();
//cout << "here3" << endl;
PBYTE pCapBuffer;
// Capture camera images
cvGetImageRawData(pCapImage, &pCapBuffer);
CLEyeCameraGetFrame(_cam, pCapBuffer);
//IplImage* imgHSV = cvCreateImage(cvGetSize(pCapImage), IPL_DEPTH_8U, 3);
//cvCvtColor(pCapImage, imgHSV, CV_BGR2HSV); //Change the color format from BGR to HSV
//IplImage* imgThresholded = cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1);
//cvInRangeS(imgHSV, cvScalar(minH,minS,minV), cvScalar(maxH,maxS,maxV), imgThresholded);
Mat imgCap(pCapImage); //creates 8UC4 Mat
mtx.try_lock();
imgCap.copyTo(imgCapture); //copy to shared variable
mtx.unlock();
//frameTimestamp = getTickCount();
//double t = ((double)(frameTimestamp - oldFrameTimestamp))/getTickFrequency() * 1000;
//cout << "Frame interval: " << t << endl;
}
Sleep(1000);
// Stop camera capture
CLEyeCameraStop(_cam);
// Destroy camera object
CLEyeDestroyCamera(_cam);
// Destroy the allocated OpenCV image
cvReleaseImage(&pCapImage);
_cam = NULL;
}
// IMPORTANT: We need to receive the point clouds in local coordinates (scanner or robot)
void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq)
{
// if the future has completed, use the new map
processNewMapIfAvailable(); // This call lock the tf publication
cerr << "received new map" << endl;
timer t;
processingNewCloud = true;
BoolSetter stopProcessingSetter(processingNewCloud, false);
// Convert point cloud
const size_t goodCount(newPointCloud->features.cols());
if (goodCount == 0)
{
ROS_ERROR("I found no good points in the cloud");
return;
}
// Dimension of the point cloud, important since we handle 2D and 3D
const int dimp1(newPointCloud->features.rows());
if(!(newPointCloud->descriptorExists("stamps_Msec") && newPointCloud->descriptorExists("stamps_sec") && newPointCloud->descriptorExists("stamps_nsec")))
{
const float Msec = round(stamp.sec/1e6);
const float sec = round(stamp.sec - Msec*1e6);
const float nsec = round(stamp.nsec);
const PM::Matrix desc_Msec = PM::Matrix::Constant(1, goodCount, Msec);
const PM::Matrix desc_sec = PM::Matrix::Constant(1, goodCount, sec);
const PM::Matrix desc_nsec = PM::Matrix::Constant(1, goodCount, nsec);
newPointCloud->addDescriptor("stamps_Msec", desc_Msec);
newPointCloud->addDescriptor("stamps_sec", desc_sec);
newPointCloud->addDescriptor("stamps_nsec", desc_nsec);
cout << "Adding time" << endl;
}
ROS_INFO("Processing new point cloud");
{
timer t; // Print how long take the algo
// Apply filters to incoming cloud, in scanner coordinates
inputFilters.apply(*newPointCloud);
ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
}
string reason;
// Initialize the transformation to identity if empty
if(!icp.hasMap())
{
// we need to know the dimensionality of the point cloud to initialize properly
publishLock.lock();
TOdomToMap = PM::TransformationParameters::Identity(dimp1, dimp1);
TOdomToMap(2,3) = mapElevation;
publishLock.unlock();
}
// Fetch transformation from scanner to odom
// Note: we don't need to wait for transform. It is already called in transformListenerToEigenMatrix()
PM::TransformationParameters TOdomToScanner;
try
{
TOdomToScanner = PointMatcher_ros::eigenMatrixToDim<float>(
PointMatcher_ros::transformListenerToEigenMatrix<float>(
tfListener,
scannerFrame,
odomFrame,
stamp
), dimp1);
}
catch(tf::ExtrapolationException e)
{
ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - stamp << endl << e.what() );
return;
}
ROS_DEBUG_STREAM("TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << TOdomToScanner);
ROS_DEBUG_STREAM("TOdomToMap(" << odomFrame<< " to " << mapFrame << "):\n" << TOdomToMap);
const PM::TransformationParameters TscannerToMap = TOdomToMap * TOdomToScanner.inverse();
ROS_DEBUG_STREAM("TscannerToMap (" << scannerFrame << " to " << mapFrame << "):\n" << TscannerToMap);
// Ensure a minimum amount of point after filtering
const int ptsCount = newPointCloud->features.cols();
if(ptsCount < minReadingPointCount)
{
ROS_ERROR_STREAM("Not enough points in newPointCloud: only " << ptsCount << " pts.");
return;
}
// Initialize the map if empty
if(!icp.hasMap())
{
ROS_INFO_STREAM("Creating an initial map");
mapCreationTime = stamp;
setMap(updateMap(newPointCloud.release(), TscannerToMap, false));
// we must not delete newPointCloud because we just stored it in the mapPointCloud
return;
}
// Check dimension
if (newPointCloud->features.rows() != icp.getInternalMap().features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud->features.rows()-1 << " while map is " << icp.getInternalMap().features.rows()-1);
return;
}
try
{
// Apply ICP
PM::TransformationParameters Ticp;
Ticp = icp(*newPointCloud, TscannerToMap);
Ticp = transformation->correctParameters(Ticp);
// extract corrections
PM::TransformationParameters Tdelta = Ticp * TscannerToMap.inverse();
// ISER
//{
// // remove roll and pitch
// Tdelta(2,0) = 0;
// Tdelta(2,1) = 0;
// Tdelta(2,2) = 1;
// Tdelta(0,2) = 0;
// Tdelta(1,2) = 0;
// Tdelta(2,3) = 0; //z
// Tdelta.block(0,0,3,3) = transformation->correctParameters(Tdelta.block(0,0,3,3));
// Ticp = Tdelta*TscannerToMap;
// ROS_DEBUG_STREAM("Ticp:\n" << Ticp);
//}
// Ensure minimum overlap between scans
const double estimatedOverlap = icp.errorMinimizer->getOverlap();
ROS_INFO_STREAM("Overlap: " << estimatedOverlap);
if (estimatedOverlap < minOverlap)
{
ROS_ERROR_STREAM("Estimated overlap too small, ignoring ICP correction!");
return;
}
// Compute tf
publishStamp = stamp;
publishLock.lock();
TOdomToMap = Ticp * TOdomToScanner;
PM::TransformationParameters Terror = TscannerToMap.inverse() * Ticp;
cerr << "Correcting translation error of " << Terror.block(0,3, 3,1).norm() << " m" << endl;
// Add transformation to path
path.push_back(Ticp);
errors.push_back(Terror);
// Publish tf
if(publishMapTf == true)
{
tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, stamp));
}
publishLock.unlock();
processingNewCloud = false;
ROS_DEBUG_STREAM("TOdomToMap:\n" << TOdomToMap);
// Publish odometry
if (odomPub.getNumSubscribers())
odomPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(Tdelta, mapFrame, stamp));
// TODO: check that, might be wrong....
// Publish error on odometry
if (odomErrorPub.getNumSubscribers())
odomErrorPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(TOdomToMap, mapFrame, stamp));
// ***Debug:
//debugPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(transformation->compute(*newPointCloud, Ticp), mapFrame, mapCreationTime));
// ***
// check if news points should be added to the map
if (
mapping &&
((estimatedOverlap < maxOverlapToMerge) || (icp.getInternalMap().features.cols() < minMapPointCount)) &&
(!mapBuildingInProgress)
)
{
cout << "map Creation..." << endl;
// make sure we process the last available map
mapCreationTime = stamp;
ROS_INFO("Adding new points to the map in background");
mapBuildingTask = MapBuildingTask(boost::bind(&Mapper::updateMap, this, newPointCloud.release(), Ticp, true));
mapBuildingFuture = mapBuildingTask.get_future();
mapBuildingThread = boost::thread(boost::move(boost::ref(mapBuildingTask)));
mapBuildingInProgress = true;
}
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return;
}
//Statistics about time and real-time capability
int realTimeRatio = 100*t.elapsed() / (stamp.toSec()-lastPoinCloudTime.toSec());
ROS_INFO_STREAM("[TIME] Total ICP took: " << t.elapsed() << " [s]");
if(realTimeRatio < 80)
ROS_INFO_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
else
ROS_WARN_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
lastPoinCloudTime = stamp;
}
// Called when a thread wants to indicate that it has stopped.
void threadStopped()
{
runningLock.lock();
runningThreads--;
runningLock.unlock();
}
void lock()
{
mutex.lock();
}
namespace pei
{
static boost::mutex sMutex;
static bool isInitialized(false);
//////////////////////////////////////////////////////////////////////////
//
TtFontAsset::TtFontAsset( const char* name, size_t size )
: FontAsset( name, size )
, m_FontName(name)
, m_FontSize(size)
{
sMutex.lock();
if ( !isInitialized ) {
pei::TtFontFactory().Init();
isInitialized = true;
}
sMutex.unlock();
}
TtFontAsset::TtFontAsset( const TtFontAsset& fa )
: FontAsset(fa.m_FontName.c_str(), fa.m_FontSize)
, m_FontName(fa.m_FontName)
, m_FontSize(fa.m_FontSize)
, m_TtFont(fa.m_TtFont)
{
}
TtFontAsset::~TtFontAsset()
{
}
bool TtFontAsset::CanHandle()
{
m_TtFont = pei::TtFontFactory().CreateTtFont( m_FontName.c_str(), m_FontSize );
return m_TtFont != NULL;
}
bool TtFontAsset::Open()
{
if ( m_TtFont == NULL ) {
m_TtFont = pei::TtFontFactory().CreateTtFont( m_FontName.c_str(), m_FontSize );
}
return m_TtFont != NULL;
}
void TtFontAsset::Close()
{
m_TtFont = TtFontPtr();
}
int TtFontAsset::GetMemoryUsage()
{
return 0;
}
std::string TtFontAsset::GetFormatString() const
{
return ".ttf";
}
const char* TtFontAsset::GetResourceDirectory() const
{
return "";
}
SurfacePtr TtFontAsset::RenderText( const char *text, const pei::Color& color /*= pei::Color(255,255,255)*/ )
{
#ifdef _HAS_SDL_TTF_
if ( m_TtFont ) {
// this is a hack! We swap r & g and correct the color format manually
// SDL_ttf renders ARGB, but we need ABGR (or LE RGBA)
SDL_Color c = { color.b, color.g, color.r, color.a };
SDL_Surface * s = TTF_RenderText_Blended( (TTF_Font*)m_TtFont->GetFontHandle(), text, c );
// swap r&g in the format description
s->format->Rmask = 0x000000ff; s->format->Rshift = 0;
s->format->Bmask = 0x00ff0000; s->format->Bshift = 16;
return SurfacePtr( new pei::SDL::SurfaceWrapper(s) );
}
#endif
return SurfacePtr();
}
void TtFontAsset::RenderText( SurfacePtr dest, int& x, int& y, const char* text, const pei::Color& color /*= pei::XColor(255,255,255)*/ )
{
// assert ( pFont );
if (!m_TtFont || dest.get() == NULL ) {
return;
}
#ifdef _HAS_SDL_TTF_
SDL_Surface *text_surface = NULL;
// this is a hack! We swap r & g and correct the color format manually
// SDL_ttf renders ARGB, but we need ABGR (or LE RGBA) to match RGBA texture format - performace reasons!
SDL_Color c = { color.b, color.g, color.r, color.a };
if ( (text_surface = TTF_RenderText_Blended( (TTF_Font*)m_TtFont->GetFontHandle(), text, c )) != NULL )
{
// swap r&g in the format description
text_surface->format->Rmask = 0x000000ff; text_surface->format->Rshift = 0;
text_surface->format->Bmask = 0x00ff0000; text_surface->format->Bshift = 16;
pei::SurfacePtr txt( new pei::SDL::SurfaceWrapper(text_surface));
pei::Blitter blitter(pei::PixOpPtr(new PixOpAlphaBlitSrcAlpha(txt->GetFormat(),dest->GetFormat()) ) );
blitter.Blit( txt, dest, x, y, txt->GetWidth(), txt->GetHeight(), 0, 0 );
int tw, th;
TTF_SizeText( (TTF_Font*)m_TtFont->GetFontHandle(), text, &tw, &th);
Uint32 font_height = TTF_FontHeight((TTF_Font*)m_TtFont->GetFontHandle()); // - a - 2*d;
y += font_height;
x += tw;
}
#endif
}
void TtFontAsset::RenderTextA( SurfacePtr dest, int x, int y, const char* text, const pei::Color& color /*= pei::XColor(255,255,255)*/ )
{
// assert ( pFont );
if (!m_TtFont || dest.get() == NULL ) {
return;
}
#ifdef _HAS_SDL_TTF_
SDL_Surface *text_surface = NULL;
// this is a hack! We swap r & g and correct the color format manually
// SDL_ttf renders ARGB, but we need ABGR (or LE RGBA) to match RGBA texture format - performace reasons!
SDL_Color c = { color.b, color.g, color.r, color.a };
if ( (text_surface = TTF_RenderText_Blended( (TTF_Font*)m_TtFont->GetFontHandle(), text, c )) != NULL )
{
// swap r&g in the format description
text_surface->format->Rmask = 0x000000ff; text_surface->format->Rshift = 0;
text_surface->format->Bmask = 0x00ff0000; text_surface->format->Bshift = 16;
pei::SurfacePtr txt( new pei::SDL::SurfaceWrapper(text_surface));
pei::Blitter blitter(pei::PixOpPtr(new PixOpAlphaBlitSrcAlpha(txt->GetFormat(),dest->GetFormat()) ) );
blitter.Blit( txt, dest, x, y, txt->GetWidth(), txt->GetHeight(), 0, 0 );
}
#endif
}
pei::Rectangle TtFontAsset::GetTextDimensions( const char* text ) const
{
Rectangle rc;
if ( m_TtFont ) {
int w(0),h(0);
#ifdef _HAS_SDL_TTF_
TTF_SizeText( static_cast<TTF_Font*>(m_TtFont->GetFontHandle()), text, &w, &h);
#endif
rc.w() = w;
rc.h() = h;
}
return rc;
}
};
void unlock()
{
mutex.unlock();
}
void callbackConfig(pcl_hand_arm_detector::PclHandArmDetectorConfig &config, uint32_t level)
{
mutex_config.lock();
g_config = config;
mutex_config.unlock();
}
// Used for keeping track on things that use multithreading
void reportReady2()
{
mutex_lock.lock();
threads_ready2++;
mutex_lock.unlock();
}
int main (int argc, char** argv)
{
if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
return print_help();
// Algorithm parameters:
std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml";
float min_confidence = -1.5;
float min_height = 1.3;
float max_height = 2.3;
float voxel_size = 0.06;
Eigen::Matrix3f rgb_intrinsics_matrix;
rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics
// Read if some parameters are passed from command line:
pcl::console::parse_argument (argc, argv, "--svm", svm_filename);
pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
pcl::console::parse_argument (argc, argv, "--min_h", min_height);
pcl::console::parse_argument (argc, argv, "--max_h", max_height);
// Read Kinect live stream:
PointCloudT::Ptr cloud (new PointCloudT);
bool new_cloud_available_flag = false;
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
boost::bind (&cloud_cb_, _1, cloud, &new_cloud_available_flag);
interface->registerCallback (f);
interface->start ();
// Wait for the first frame:
while(!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
new_cloud_available_flag = false;
cloud_mutex.lock (); // for not overwriting the point cloud
// Display pointcloud:
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Add point picking callback to viewer:
struct callback_args cb_args;
PointCloudT::Ptr clicked_points_3d (new PointCloudT);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;
// Spin until 'Q' is pressed:
viewer.spin();
std::cout << "done." << std::endl;
cloud_mutex.unlock ();
// Ground plane estimation:
Eigen::VectorXf ground_coeffs;
ground_coeffs.resize(4);
std::vector<int> clicked_points_indices;
for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
clicked_points_indices.push_back(i);
pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;
// Initialize new viewer:
pcl::visualization::PCLVisualizer viewer("PCL Viewer"); // viewer initialization
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Create classifier for people detection:
pcl::people::PersonClassifier<pcl::RGB> person_classifier;
person_classifier.loadSVMFromFile(svm_filename); // load trained SVM
// People detection app initialization:
pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector; // people detection object
people_detector.setVoxelSize(voxel_size); // set the voxel size
people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters
people_detector.setClassifier(person_classifier); // set person classifier
people_detector.setHeightLimits(min_height, max_height); // set person classifier
// people_detector.setSensorPortraitOrientation(true); // set sensor orientation to vertical
// For timing:
static unsigned count = 0;
static double last = pcl::getTime ();
// Main loop:
while (!viewer.wasStopped())
{
if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available
{
new_cloud_available_flag = false;
// Perform people detection on the new cloud:
std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters
people_detector.setInputCloud(cloud);
people_detector.setGround(ground_coeffs); // set floor coefficients
people_detector.compute(clusters); // perform people detection
ground_coeffs = people_detector.getGround(); // get updated floor coefficients
// Draw cloud and people bounding boxes in the viewer:
viewer.removeAllPointClouds();
viewer.removeAllShapes();
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
unsigned int k = 0;
for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
{
if(it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold
{
// draw theoretical person bounding box in the PCL viewer:
it->drawTBoundingBox(viewer, k);
k++;
}
}
std::cout << k << " people found" << std::endl;
viewer.spinOnce();
// Display average framerate:
if (++count == 30)
{
double now = pcl::getTime ();
std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl;
count = 0;
last = now;
}
cloud_mutex.unlock ();
}
}
return 0;
}
virtual bool on_expose_event(GdkEventExpose* )
{
Glib::RefPtr<Gdk::Drawable> drawable = get_window();
vc.get_frame(pixbuf);
drawable->draw_pixbuf(get_style()->get_bg_gc(Gtk::STATE_NORMAL),
pixbuf->scale_simple(get_width(), get_height(), Gdk::INTERP_BILINEAR),
0, 0, 0, 0, -1 , -1, Gdk::RGB_DITHER_NONE, 0, 0);
Gdk::Point rect[4], barcode_center;
string str;
bool res = decode_barcode(pixbuf, rect, str, 100);
if (res)
{
Cairo::RefPtr<Cairo::Context> cr = drawable->create_cairo_context();
cr->set_line_width(3);
cr->set_source_rgb(1, 0.0, 0.0);
cr->move_to(rect[0].get_x(), rect[0].get_y() );
cr->line_to(rect[1].get_x(), rect[1].get_y() );
cr->line_to(rect[2].get_x(), rect[2].get_y() );
cr->line_to(rect[3].get_x(), rect[3].get_y() );
cr->line_to(rect[0].get_x(), rect[0].get_y() );
cr->stroke();
BarcodePropertyEstimation distance(get_width(), get_height(),
rect[0].get_x(), rect[0].get_y(),
rect[1].get_x(), rect[1].get_y(),
rect[2].get_x(), rect[2].get_y(),
rect[3].get_x(), rect[3].get_y());
Glib::RefPtr<Pango::Layout> pango_layout = Pango::Layout::create(cr);
Pango::FontDescription desc("Arial Rounded MT Bold");
desc.set_size(14 * PANGO_SCALE);
pango_layout->set_font_description(desc);
pango_layout->set_alignment(Pango::ALIGN_CENTER);
double dir = distance.calcDirection();
string dir_str;
if (dir > 2.3)
dir_str = "right";
else if (dir < -2.3)
dir_str = "left";
else
dir_str = "front";
double dist = distance.calcDistance();
string text = "Object ID: " + str +
"\nDistance: " + lexical_cast<string>(int(dist * 100) / 100) + "." +
lexical_cast<string>(int(dist * 100) % 100) +
"\nDirection: " + dir_str;
pango_layout->set_text(text);
Gdk::Point* max_x, *min_x, *max_y, *min_y;
boost::function<bool (Gdk::Point, Gdk::Point)> compare;
compare = bind(less<int>(), bind(&Gdk::Point::get_x, _1), bind(&Gdk::Point::get_x, _2));
max_x = max_element(rect, rect + 4, compare);
min_x = min_element(rect, rect + 4, compare);
compare = bind(less<int>(), bind(&Gdk::Point::get_y, _1), bind(&Gdk::Point::get_y, _2));
max_y = max_element(rect, rect + 4, compare);
min_y = min_element(rect, rect + 4, compare);
barcode_center = Gdk::Point(
(max_x->get_x() + min_x->get_x()) / 2,
(max_y->get_y() + min_y->get_y()) / 2);
const Pango::Rectangle extent = pango_layout->get_pixel_logical_extents();
cr->move_to(get_width() - extent.get_width() - 10,
get_height() - extent.get_height() - 10);
pango_layout->show_in_cairo_context(cr);
DetectedBarcode barcode(str, dir_str, distance.calcDistance());
vector<DetectedBarcode> vec;
vec.push_back(barcode);
ostringstream ostr;
xmlpp::Document document1;
listDetectedBarcodesToXML(document1.create_root_node("ObjectList"), vec);
document1.write_to_stream(ostr, "UTF-8");
mut.lock();
strXML = ostr.str();
mut.unlock();
}
return true;
}
void releaseContext() {
update_mutex.unlock();
}
void free_adjlst(struct adjlst<edge_t> * adjl) {
free_mutex.lock();
free(adjl->edges);
free(adjl);
free_mutex.unlock();
}