int readOneImpl()
{
if (!m_bReady)
return -1; // delimiter has been matched --> read fails
int nCurSize = m_cbuf.size();
if (m_bEOF) {
// EOF has been encountered --> returns the cbuf content
if (nCurSize>0) {
unsigned char val = m_cbuf.front();
m_cbuf.pop_front();
return val;
}
// EOF reached and cbuf is empty
return -1; // failed!!
}
if (nCurSize<m_nCbufLen) {
if (!fillCbuf())
return -1; // failed!!
}
unsigned char val = m_cbuf.front();
m_cbuf.pop_front();
fillOne();
return val;
}
boost::shared_ptr< const MapsBuffer::MapsRgb >
MapsBuffer::getFront(bool print)
{
boost::shared_ptr< const MapsBuffer::MapsRgb > depth_rgb;
{
boost::mutex::scoped_lock buff_lock (bmutex_);
while (buffer_.empty ())
{
if (is_done)
break;
{
boost::mutex::scoped_lock io_lock (io_mutex);
//std::cout << "No data in buffer_ yet or buffer is empty." << std::endl;
}
buff_empty_.wait (buff_lock);
}
depth_rgb = buffer_.front ();
buffer_.pop_front ();
}
if(print)
PCL_INFO("%d maps left in the buffer...\n", buffer_.size ());
return (depth_rgb);
}
/** Process the read reservations that are ready to be released in the
reservation queue
*/
void move_read_reservation_forward() {
// Lock the pipe to avoid nuisance
std::lock_guard<std::mutex> lg { cb_mutex };
for (;;) {
if (r_rid_q.empty())
// No pending reservation, so nothing to do
break;
if (!r_rid_q.front().ready)
/* If the first reservation is not ready to be released, stop
because it is blocking all the following in the queue
anyway */
break;
// Remove the reservation to be released from the queue
r_rid_q.pop_front();
std::size_t n_to_pop;
if (r_rid_q.empty())
// If it was the last one, remove all the reservation
n_to_pop = read_reserved_frozen;
else
// Else remove everything up to the next reservation
n_to_pop = r_rid_q.front().start - cb.begin();
// No longer take into account these reserved slots
read_reserved_frozen -= n_to_pop;
// Release the elements from the FIFO
while (n_to_pop--)
cb.pop_front();
// Notify the clients waiting for some room to write in the pipe
read_done.notify_all();
/* ...and process the next reservation to see if it is ready to
be released too */
}
}
/** Try to read a value from the pipe
\param[out] value is the reference to where to store what is
read
\param[in] blocking specify if the call wait for the operation
to succeed
\return true on success
*/
bool read(T &value, bool blocking = false) {
// Lock the pipe to avoid being disturbed
std::unique_lock<std::mutex> ul { cb_mutex };
TRISYCL_DUMP_T("Read pipe empty = " << empty());
if (blocking)
/* If in blocking mode, wait for the not empty condition, that
may be changed when a write is done */
write_done.wait(ul, [&] { return !empty(); });
else if (empty())
return false;
TRISYCL_DUMP_T("Read pipe front = " << cb.front()
<< " back = " << cb.back()
<< " reserved_for_reading() = " << reserved_for_reading());
if (read_reserved_frozen)
/** If there is a pending reservation, read the next element to
be read and update the number of reserved elements */
value = cb.begin()[read_reserved_frozen++];
else {
/* There is no pending read reservation, so pop the read value
from the pipe */
value = cb.front();
cb.pop_front();
}
TRISYCL_DUMP_T("Read pipe value = " << value);
// Notify the clients waiting for some room to write in the pipe
read_done.notify_all();
return true;
}
void take() {
while(flag) {
unique_lock<mutex> locker(m_mutex);
while(Q.empty())
full.wait(locker);
if(flag) {
assert(!Q.empty());
cout << "# " << Q.front() <<endl;
Q.pop_front();
empty.notify_all();
}
}
}
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr
PCDBuffer::getFront ()
{
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud;
{
boost::mutex::scoped_lock buff_lock (bmutex_);
while (buffer_.empty ())
{
if (is_done)
break;
{
boost::mutex::scoped_lock io_lock (io_mutex);
//std::cout << "No data in buffer_ yet or buffer is empty." << std::endl;
}
buff_empty_.wait (buff_lock);
}
cloud = buffer_.front ();
buffer_.pop_front ();
}
return (cloud);
}
void gpsFixCallBack (geometry_msgs::PoseStamped gpsPose)
{
geometry_msgs::PoseStamped gpsFixPoseEstimate;
// find poseAtRequestTime
bool odomMovementFound = false;
pair<geometry_msgs::Pose, ros::Time> prevFront;
if (cb.size() > 0)
{
odomMovementFound = true;
cout << "odom found" << endl;
prevFront = cb.front();
if (gpsPose.header.stamp >= cb.front().second)
{
while (gpsPose.header.stamp > prevFront.second && cb.size() > 0)
{
prevFront = cb.front();
cb.pop_front();
}
}
}
// calculate Odom difference
double deltaX = 0;
double deltaY = 0;
if (odomMovementFound)
{
deltaX = (current.first.position.x - prevFront.first.position.x);
deltaY = (current.first.position.y - prevFront.first.position.y);
}
gpsFixPoseEstimate.pose.position.x = gpsPose.pose.position.x + deltaX;
gpsFixPoseEstimate.pose.position.y = gpsPose.pose.position.y + deltaY;
tf::Quaternion gpsQaut, odomCurrentQuat, odomOldQuat;
if (odomMovementFound)
{
tf::quaternionMsgToTF (current.first.orientation, odomCurrentQuat);
tf::quaternionMsgToTF (prevFront.first.orientation, odomOldQuat);
}
tf::quaternionMsgToTF (gpsPose.pose.orientation, gpsQaut);
double deltaYaw = 0;
if (odomMovementFound)
{
deltaYaw = (tf::getYaw (odomCurrentQuat) - tf::getYaw (odomOldQuat));
}
double newYaw = tf::getYaw (gpsQaut) + deltaYaw;
gpsFixPoseEstimate.pose.orientation = tf::createQuaternionMsgFromYaw (newYaw);
cout << "new Yaw: " << newYaw << endl;
// create covariance
// publish new estimated pose
gpsFixPoseEstimate.header.stamp = ros::Time::now();
gpsFixPoseEstimate.header.frame_id = gpsFrame;
geometry_msgs::PoseStamped inMapFrame;
try
{
tfListener->transformPose ("/map", ros::Time::now(), gpsFixPoseEstimate, gpsFrame, inMapFrame);
geometry_msgs::PoseWithCovarianceStamped resultPose;
resultPose.pose.pose = inMapFrame.pose;
resultPose.header = inMapFrame.header;
if (odomMovementFound)
{
resultPose.pose.covariance[0] = sqrt (abs (current.first.position.x - prevFront.first.position.x)); // X
resultPose.pose.covariance[7] = sqrt (abs (current.first.position.y - prevFront.first.position.y)); // Y
resultPose.pose.covariance[35] = sqrt (abs (newYaw)); // Yaw
}
else
{
resultPose.pose.covariance[0] = 0.1;// X
resultPose.pose.covariance[7] = 0.1; // Y
resultPose.pose.covariance[35] = 0.1; // Yaw
}
gpsOdomCombinedLocalisationPublisher.publish (resultPose);
}
catch (tf::TransformException ex)
{
ROS_ERROR ("%s", ex.what());
}
}
