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 HeaderManipulation::processShapeShifter(topic_tools::ShapeShifter &shape_shifter)
{
ROS_DEBUG("HeaderManipulation processShapeShifter");
uint8_t msg_buffer[shape_shifter.size()];
ros::serialization::OStream o_stream(msg_buffer, shape_shifter.size());
manipulateRawData(msg_buffer);
ros::serialization::IStream i_stream(msg_buffer, shape_shifter.size());
shape_shifter.read(i_stream);
}
bool example03(char* buffer, const unsigned int buffer_size)
{
const std::string file_name = "data.txt";
const std::size_t rounds = 1000;
{
strtk::binary::writer writer(buffer,buffer_size);
writer.clear();
person p;
p.id = 12345678901234567890ULL;
p.name = "Mr. Rumpelstilzchen";
p.age = 0;
p.height = 123.4567;
p.weight = 765.345f;
p.is_insane = true;
for (unsigned int i = 0; i < rounds; ++i)
{
if (!writer(p))
{
std::cout << "example03() - Failed to write person:" << i << std::endl;
return false;
}
p.id++;
p.age++;
p.height += 1.23;
p.weight += 4.567f;
p.is_insane = !p.is_insane;
}
std::ofstream o_stream(file_name.c_str(),std::ios::binary);
if (!o_stream)
{
std::cout << "example03() - ERROR Could not open file!(1)" << std::endl;
return false;
}
writer(o_stream);
o_stream.close();
}
{
strtk::binary::reader reader(buffer,buffer_size);
reader.clear();
const std::size_t length = strtk::fileio::file_size(file_name);
std::ifstream i_stream(file_name.c_str(),std::ios::binary);
if (!i_stream)
{
std::cout << "example03() - ERROR Could not open file!(2)" << std::endl;
return false;
}
reader(i_stream,length);
reader.reset();
person p_expected;
p_expected.id = 12345678901234567890ULL;
p_expected.name = "Mr. Rumpelstilzchen";
p_expected.age = 0;
p_expected.height = 123.4567;
p_expected.weight = 765.345f;
p_expected.is_insane = true;
person p_in;
for (unsigned int i = 0; i < rounds; ++i)
{
p_in.clear();
if (!reader(p_in))
{
std::cout << "example03() - Failed to read person:" << i << std::endl;
return false;
}
if (p_in != p_expected)
{
std::cout << "example03() - Comparison between expected and read failed @ " << i << std::endl;
return false;
}
p_expected.id++;
p_expected.age++;
p_expected.height += 1.23;
p_expected.weight += 4.567f;
p_expected.is_insane = !p_expected.is_insane;
}
}
return true;
}
