void HeaderManipulation::inputCB(const topic_tools::ShapeShifter::ConstPtr &input) { ROS_DEBUG("HeaderManipulation inputCB"); ros::Time start_time(ros::Time::now()); // Initialize. if (!output_advertised_) { ROS_INFO("Output not advertised. Setting up publisher now."); ros::AdvertiseOptions opts("output", 1, input->getMD5Sum(), input->getDataType(), input->getMessageDefinition()); boost::mutex::scoped_lock pub_lock(pub_mutex_); generic_pub_ = private_nh_.advertise(opts); output_advertised_ = true; } // Copy shape shifter data to array. uint8_t msg_buffer[input->size()]; ros::serialization::OStream o_stream(msg_buffer, input->size()); input->write(o_stream); // Manipulate (header) data. manipulateRawData(msg_buffer); // Read data from array to StampedMsg boost::shared_ptr<StampedMsg> stamped_msg(new StampedMsg()); stamped_msg->second = start_time; ros::serialization::IStream i_stream(msg_buffer, input->size()); stamped_msg->first.read(i_stream); // Push StampedMsg to Buffer for multithreaded publishing. boost::mutex::scoped_lock buffer_lock(buffer_mutex_); stamped_msg_buffer_.push(stamped_msg); ROS_DEBUG("all done"); }
void PeopleEventRegister<T>::isPublishing(bool state) { boost::mutex::scoped_lock pub_lock(mutex_); isPublishing_ = state; }
void AudioEventRegister::isPublishing(bool state) { boost::mutex::scoped_lock pub_lock(processing_mutex_); isPublishing_ = state; }
void HeaderManipulation::publishMsg(const topic_tools::ShapeShifter &msg, const ros::Time &msg_in_time) { ROS_DEBUG("HeaderManipulation publishMsg Thread started with timestamp %f", msg_in_time.toSec()); ros::Time end_time(ros::Time::now()); ros::Time last_time; config_mutex_.lock(); ros::Time time_to_pub(msg_in_time + msg_delay_); config_mutex_.unlock(); do { last_time = end_time; ROS_DEBUG("waiting to publish msg from %f at %f", msg_in_time.toSec(), time_to_pub.toSec()); if (end_time > time_to_pub) { boost::mutex::scoped_lock pub_lock(pub_mutex_); ROS_DEBUG("publishing msg which should be held back till: %f", time_to_pub.toSec()); generic_pub_.publish(msg); return; } boost::mutex::scoped_lock config_lock(config_mutex_); publish_retry_rate_.sleep(); time_to_pub = msg_in_time + msg_delay_; end_time = ros::Time::now(); } while (last_time <= end_time); ROS_WARN("Detected jump back in time. Dropping msg. last: %f, end %f", last_time.toSec(), end_time.toSec()); return; }