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