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;
}
