/** 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;
}
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);
}
/** Try to write a value to the pipe
\param[in] value is what we want to write
\param[in] blocking specify if the call wait for the operation
to succeed
\return true on success
\todo provide a && version
*/
bool write(const T &value, bool blocking = false) {
// Lock the pipe to avoid being disturbed
std::unique_lock<std::mutex> ul { cb_mutex };
TRISYCL_DUMP_T("Write pipe full = " << full()
<< " value = " << value);
if (blocking)
/* If in blocking mode, wait for the not full condition, that
may be changed when a read is done */
read_done.wait(ul, [&] { return !full(); });
else if (full())
return false;
cb.push_back(value);
TRISYCL_DUMP_T("Write pipe front = " << cb.front()
<< " back = " << cb.back()
<< " cb.begin() = " << (void *)&*cb.begin()
<< " cb.size() = " << cb.size()
<< " cb.end() = " << (void *)&*cb.end()
<< " reserved_for_reading() = " << reserved_for_reading()
<< " reserved_for_writing() = " << reserved_for_writing());
// Notify the clients waiting to read something from the pipe
write_done.notify_all();
return true;
}
void image_obj_callback(const autoware_msgs::image_obj::ConstPtr& image_obj_msg) {
pthread_mutex_lock(&mutex);
image_obj_ringbuf.push_front(*image_obj_msg);
//vscan_image is empty
if (vscan_image_ringbuf.begin() == vscan_image_ringbuf.end()) {
pthread_mutex_unlock(&mutex);
ROS_INFO("vscan_image ring buffer is empty");
return;
}
buf_flag = true;
// image_obj > vscan_image
if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
vscan_image_buf = vscan_image_ringbuf.front();
boost::circular_buffer<autoware_msgs::image_obj>::iterator it = image_obj_ringbuf.begin();
if (image_obj_ringbuf.size() == 1) {
image_obj_buf = *it;
pthread_mutex_unlock(&mutex);
return;
} else {
for (it++; it != image_obj_ringbuf.end(); it++) {
if (fabs_time_diff(&(vscan_image_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(vscan_image_ringbuf.front().header), &(it->header))) {
image_obj_buf = *(it-1);
break;
}
}
if (it == image_obj_ringbuf.end()) {
image_obj_buf = image_obj_ringbuf.back();
}
}
} else {
image_obj_buf = image_obj_ringbuf.front();
boost::circular_buffer<autoware_msgs::PointsImage>::iterator it = vscan_image_ringbuf.begin();
if (vscan_image_ringbuf.size() == 1) {
vscan_image_buf = *it;
pthread_mutex_unlock(&mutex);
return;
}
for (it++; it != vscan_image_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_obj_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_obj_ringbuf.front().header), &(it->header))) {
vscan_image_buf = *(it-1);
break;
}
}
if (it == vscan_image_ringbuf.end()) {
vscan_image_buf = vscan_image_ringbuf.back();
}
}
pthread_mutex_unlock(&mutex);
}
void image_obj_ranged_callback(const cv_tracker::image_obj_ranged::ConstPtr& image_obj_ranged_msg) {
pthread_mutex_lock(&mutex);
image_obj_ranged_ringbuf.push_front(*image_obj_ranged_msg);
//image_raw is empty
if (image_raw_ringbuf.begin() == image_raw_ringbuf.end()) {
pthread_mutex_unlock(&mutex);
ROS_INFO("image_raw ring buffer is empty");
return;
}
buf_flag = true;
// image_obj_ranged > image_raw
if (get_time(&(image_obj_ranged_ringbuf.front().header)) >= get_time(&(image_raw_ringbuf.front().header))) {
image_raw_buf = image_raw_ringbuf.front();
boost::circular_buffer<cv_tracker::image_obj_ranged>::iterator it = image_obj_ranged_ringbuf.begin();
if (image_obj_ranged_ringbuf.size() == 1) {
image_obj_ranged_buf = *it;
pthread_mutex_unlock(&mutex);
return;
} else {
for (it++; it != image_obj_ranged_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_raw_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_raw_ringbuf.front().header), &(it->header))) {
image_obj_ranged_buf = *(it-1);
break;
}
}
if (it == image_obj_ranged_ringbuf.end()) {
image_obj_ranged_buf = image_obj_ranged_ringbuf.back();
}
}
} else {
image_obj_ranged_buf = image_obj_ranged_ringbuf.front();
boost::circular_buffer<sensor_msgs::Image>::iterator it = image_raw_ringbuf.begin();
if (image_raw_ringbuf.size() == 1) {
image_raw_buf = *it;
pthread_mutex_unlock(&mutex);
return;
}
for (it++; it != image_raw_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &(it->header))) {
image_raw_buf = *(it-1);
break;
}
}
if (it == image_raw_ringbuf.end()) {
image_raw_buf = image_raw_ringbuf.back();
}
}
pthread_mutex_unlock(&mutex);
}
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);
}
bool publish() {
if (buf_flag) {
pthread_mutex_lock(&mutex);
//image_obj is empty
if (image_obj_ringbuf.begin() == image_obj_ringbuf.end()) {
pthread_mutex_unlock(&mutex);
ROS_INFO("image_obj ring buffer is empty");
return false;
}
//vscan_image is empty
if (vscan_image_ringbuf.begin() == vscan_image_ringbuf.end()) {
pthread_mutex_unlock(&mutex);
ROS_INFO("vscan_image ring buffer is empty");
return false;
}
// image_obj > vscan_image
if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
p_vscan_image_buf = &(vscan_image_ringbuf.front());
boost::circular_buffer<autoware_msgs::image_obj>::iterator it = image_obj_ringbuf.begin();
if (image_obj_ringbuf.size() == 1) {
p_image_obj_buf = &*it;
publish_msg(p_image_obj_buf, p_vscan_image_buf);
pthread_mutex_unlock(&mutex);
return true;
} else {
for (it++; it != image_obj_ringbuf.end(); it++) {
if (fabs_time_diff(&(vscan_image_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(vscan_image_ringbuf.front().header), &(it->header))) {
p_image_obj_buf = &*(it-1);
break;
}
}
if (it == image_obj_ringbuf.end()) {
p_image_obj_buf = &(image_obj_ringbuf.back());
}
}
}
// image_obj < vscan_image
else {
p_image_obj_buf = &(image_obj_ringbuf.front());
boost::circular_buffer<autoware_msgs::PointsImage>::iterator it = vscan_image_ringbuf.begin();
if (vscan_image_ringbuf.size() == 1) {
p_vscan_image_buf = &*it;
publish_msg(p_image_obj_buf, p_vscan_image_buf);
pthread_mutex_unlock(&mutex);
return true;
}
for (it++; it != vscan_image_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_obj_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_obj_ringbuf.front().header), &(it->header))) {
p_vscan_image_buf = &*(it-1);
break;
}
}
if (it == vscan_image_ringbuf.end()) {
p_vscan_image_buf = &(vscan_image_ringbuf.back());
}
}
publish_msg(p_image_obj_buf, p_vscan_image_buf);
if (image_obj_ranged_flag == true){
buf_flag = false;
image_obj_ranged_flag = false;
pthread_mutex_unlock(&flag_mutex);
image_obj_ringbuf.clear();
vscan_image_ringbuf.clear();
}
return true;
} else {
return false;
}
}
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());
}
}
bool publish() {
if (buf_flag) {
//image_obj_ranged is empty
if (image_obj_ranged_ringbuf.begin() == image_obj_ranged_ringbuf.end()) {
ROS_INFO("image_obj_ranged ring buffer is empty");
return false;
}
//image_raw is empty
if (image_raw_ringbuf.begin() == image_raw_ringbuf.end()) {
ROS_INFO("image_raw ring buffer is empty");
return false;
}
// image_obj_ranged > image_raw
if (get_time(&(image_obj_ranged_ringbuf.front().header)) >= get_time(&(image_raw_ringbuf.front().header))) {
p_image_raw_buf = &(image_raw_ringbuf.front());
boost::circular_buffer<cv_tracker::image_obj_ranged>::iterator it = image_obj_ranged_ringbuf.begin();
if (image_obj_ranged_ringbuf.size() == 1) {
p_image_obj_ranged_buf = &*it;
publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
if (image_obj_tracked_flag == true){
buf_flag = false;
image_obj_tracked_flag = false;
image_obj_ranged_ringbuf.clear();
image_raw_ringbuf.clear();
}
return true;
} else {
for (it++; it != image_obj_ranged_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_raw_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_raw_ringbuf.front().header), &(it->header))) {
p_image_obj_ranged_buf = &*(it-1);
break;
}
}
if (it == image_obj_ranged_ringbuf.end()) {
p_image_obj_ranged_buf = &(image_obj_ranged_ringbuf.back());
}
}
}
// image_obj_ranged < image_raw
else {
p_image_obj_ranged_buf = &(image_obj_ranged_ringbuf.front());
boost::circular_buffer<sensor_msgs::Image>::iterator it = image_raw_ringbuf.begin();
if (image_raw_ringbuf.size() == 1) {
p_image_raw_buf = &*it;
publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
if (image_obj_tracked_flag == true){
buf_flag = false;
image_obj_tracked_flag = false;
image_obj_ranged_ringbuf.clear();
image_raw_ringbuf.clear();
}
return true;
}
for (it++; it != image_raw_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &(it->header))) {
p_image_raw_buf = &*(it-1);
break;
}
}
if (it == image_raw_ringbuf.end()) {
p_image_raw_buf = &(image_raw_ringbuf.back());
}
}
publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
if (image_obj_tracked_flag == true){
buf_flag = false;
image_obj_tracked_flag = false;
image_obj_ranged_ringbuf.clear();
image_raw_ringbuf.clear();
}
return true;
} else {
return false;
}
}
ros::Time front(void) {
return execution.front();
}