int
main(void)
{
// [0,42,{key=>"value"},["1.2.3","4.5.6"]]
char *buf1 = "\x94\x00\x2a\x81\xa3\x6b\x65\x79\xa5\x76\x61\x6c\x75\x65\x92\xa5\x31\x2e\x32\x2e\x33\xa5\x34\x2e\x35\x2e\x36";
int buf1_len = 27;
// ["ModeratelyLongStringLongerThan32Bytes",1,-2,{dict=>42}]
char *buf2 = "\x94\xda\x00\x25\x4d\x6f\x64\x65\x72\x61\x74\x65\x6c\x79\x4c\x6f"
"\x6e\x67\x53\x74\x72\x69\x6e\x67\x4c\x6f\x6e\x67\x65\x72\x54\x68"
"\x61\x6e\x33\x32\x42\x79\x74\x65\x73\x01\xfe\x81\xa4\x64\x69\x63"
"\x74\x2a";
int buf2_len = 50;
msgpack_unpacker unpacker;
msgpack_unpacker_init(&unpacker, MSGPACK_UNPACKER_INIT_BUFFER_SIZE);
while (buf1_len) {
add_input_bytes(&unpacker, buf1, 3);
buf1_len -= 3;
buf1 += 3;
printf("buf1 len is now %d\n", buf1_len);
}
while (buf2_len) {
add_input_bytes(&unpacker, buf2, 2);
buf2_len -= 2;
buf2 += 2;
printf("buf2 len is now %d\n", buf2_len);
}
return 0;
}
void VelodynePlaybackServer::readPacket(double t, double max_age) {
// read a new packet
velodyne::Velodyne::UDPPacket pkt;
try {
velodyne_.readPacket(velodyne_file_, &pkt);
}
catch (vlr::Ex<>& e) {
std::cout << e.what() << std::endl;
set_eof(true);
return;
}
if (std::abs(last_timestamp_ - t) > max_age) {
last_timestamp_ = t;
return;
}
set_last_packet_time(pkt.timestamp);
off64_t fpos = vlr::cio::ftell(velodyne_file_->fp);
add_input_bytes(fpos - last_packet_fpos_);
last_packet_fpos_ = fpos;
// project the beams, and publish
for (int i = 0; i < velodyne::Packet::NUM_BLOCKS; i++) {
packet_.block[i].encoder = pkt.scan[i].encoder;
packet_.block[i].block = pkt.scan[i].block;
packet_.block[i].timestamp = pkt.timestamp;
for (int32_t b = 0; b < velodyne::BlockRaw::NUM_BEAMS; b++) {
packet_.block[i].laser[b].distance = pkt.scan[i].range[b];
packet_.block[i].laser[b].intensity = pkt.scan[i].intensity[b];
}
// drc::GlobalPose tpose = pose_queue_.pose(t);
drc::GlobalPose tpose = pose_queue_.pose(pkt.timestamp);
driving_common::Pose pose;
pose.x = tpose.x();
pose.y = tpose.y();
pose.z = tpose.z();
pose.yaw = tpose.yaw();
pose.pitch = tpose.pitch();
pose.roll = tpose.roll();
velodyne_.projectMeasurement(velodyne_.config(), pkt.scan[i], projected_packet_.block[i], pose);
projected_packet_.block[i].timestamp = pkt.timestamp;
add_output_bytes(sizeof(velodyne::Packet));
add_frames(1);
}
packet_.header.stamp = ros::Time(pkt.timestamp);
last_timestamp_ = pkt.timestamp;
packet_pub_.publish(packet_);
projected_packet_pub_.publish(projected_packet_);
packet_.spin_count++;
packet_.header.seq++;
// static uint32_t width=1000, height=1000;
// static cv::Mat img(height, width, CV_8UC1);
// static double z_thresh = 0.2;
// static double res = 0.1;
// static uint32_t fnum=0;
// data2Img(projected_packet_, z_thresh, res, 0, 0, img);
// std::stringstream filename;
// filename << "plbmap" << fnum << ".png";
// cv::imwrite(filename.str(), img);
// cv::imshow("Map", img);
// cv::waitKey(50);
// fnum++;
// usleep(50000);
}
