void consume(int sleep){
tlog("Started to consume");
while (active){
int obj;
if (mainQueue->timedPop(obj, 50000)){
tlog("Consuming: "<<obj);
}else{
tlog("Nothing to consume!");
}
ThreadBase::sleep(sleep);
}
}
// The thread function fills the queue with data
void operator () (){
int data=0;
while (true){
// Produce a string and store in the queue
std::string str = "Producer ["+boost::lexical_cast<std::string>(m_id+1)+"]: produced data "+boost::lexical_cast<std::string>(++data)+".";
m_queue->Enqueue(str);
str+="\n";
cout<<str;
// Sleep one second
boost::this_thread::sleep(boost::posix_time::seconds(1));
}
}
void produce(int sleep){
tlog("Started to produce");
int prod = nextNum();
while (active){
if (mainQueue->timedPush(prod, 50000)){
prod++;
tlog("Produced: "<<prod);
} else {
tlog("Full. Can't produce!");
}
ThreadBase::sleep(sleep);
}
}
/// need this //// void callback(const PointCloud::ConstPtr & msg)
void callback(const PointCloud::ConstPtr & msg)
{ /*
pcl::PointCloud<pcl::PointXYZRGB>::Ptr input(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*msg, *input);
*/
ThreadQueue.Enqueue(msg);
/*
avoidance.run(input, cmd_vel_pub_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr reoriented(new pcl::PointCloud<pcl::PointXYZRGB>);
translate(currPos[1], currPos[2], currPos[0], -currOri[0], input, reoriented);
cout << currPos[0] << " " << currPos[1] << " " << currPos[2] << " " << 0.8124*(currOri[0] * 180) - 4.7564 << endl;
if (targetmsg == true)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr target(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*PCmap, *target);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGB>);
mapping(reoriented, target, PCmap);
pcl::ApproximateVoxelGrid<PointT> grid;
{
grid.setLeafSize(0.05, 0.05, 0.05);
grid.setInputCloud(temp);
grid.filter(*PCmap);
stepNum = 2;
}
viewer.showCloud(PCmap);
}
else
{
PCmap = reoriented;
//wait for next cloud
targetmsg = true;
}
*/
}
void writeSerialThread()
{
while (true)
{
try
{
_serialConnection->write (_writeQueue.dequeue());
// Signal interrupt point
boost::this_thread::interruption_point();
}
catch (const boost::thread_interrupted&)
{
break;
}
}
}
// Functions
void changeMode()
{
if (_runMode == M_OFF)
{
_writeQueue.enqueue ("off\n");
}
else if (_runMode == M_IDLE)
{
//ROS_INFO("Changed to IDLE");
switch (_errorMode)
{
case M_SAFE:
_writeQueue.enqueue ("idle\n");
break;
case M_PROXIMITYALERT:
_writeQueue.enqueue ("idleSlow\n");
break;
case M_COLLIDING:
_writeQueue.enqueue ("idleStop\n");
break;
default:
break;
}
}
else if (_runMode == M_AUTO)
{
//ROS_INFO("Changed to AUTO");
switch (_errorMode)
{
case M_SAFE:
_writeQueue.enqueue ("run\n");
break;
case M_PROXIMITYALERT:
_writeQueue.enqueue ("runSlow\n");
break;
case M_COLLIDING:
_writeQueue.enqueue ("runStop\n");
break;
default:
break;
}
}
else if (_runMode == M_MANUAL)
{
//ROS_INFO("Changed to MANUAL");
switch (_errorMode)
{
case M_SAFE:
_writeQueue.enqueue ("manual\n");
break;
case M_PROXIMITYALERT:
_writeQueue.enqueue ("manualSlow\n");
break;
case M_COLLIDING:
_writeQueue.enqueue ("manualStop\n");
break;
default:
_writeQueue.enqueue ("manual\n");
break;
}
}
}
// The thread function reads data from the queue
void operator () (){
while (true){
// Get the data from the queue and print it
std::string str = "Consumer ["+boost::lexical_cast<std::string>(m_id+1)+"] consumed: ("+m_queue->Dequeue()+")\n";
cout<<str;
// Make sure we can be interrupted
boost::this_thread::interruption_point();
}
}
