Пример #1
0
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);
}