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 AttitudeIndicatorPlugin::handleMessage(const topic_tools::ShapeShifter::ConstPtr& msg)
{
if (IS_INSTANCE(msg, nav_msgs::Odometry))
{
AttitudeCallbackOdom(msg->instantiate<nav_msgs::Odometry>());
}
else if (IS_INSTANCE(msg, sensor_msgs::Imu))
{
AttitudeCallbackImu(msg->instantiate<sensor_msgs::Imu>());
}
else if (IS_INSTANCE(msg, geometry_msgs::Pose))
{
AttitudeCallbackPose(msg->instantiate<geometry_msgs::Pose>());
}
else
{
PrintError("Unknown message type: " + msg->getDataType());
}
}
void RobustTopicRelay::MessageCallback(
const std::string topic_name, const topic_tools::ShapeShifter::ConstPtr &message) {
RobustTopicRelay::RelayedTopic &relayed_topic = relayed_topics_[topic_name];
{
boost::mutex::scoped_lock lock(mutex_);
if (!relayed_topic.connected) {
ROS_DEBUG("Connected to topic: %s", topic_name.c_str());
relayed_topic.connected = true;
}
ros::Time next_expiration_time = ros::Time::now() + relayed_topic.expected_delay;
expiring_topics_.erase(relayed_topic.expiration_time);
relayed_topic.expiration_time = next_expiration_time;
expiring_topics_[next_expiration_time] = topic_name;
}
if (!relayed_topic.publisher) {
relayed_topic.publisher = message->advertise(
node_handle_, relayed_topic.output_topic_name, 10);
}
relayed_topic.publisher.publish(message);
}