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