int main(void)
{
int rtc_fd, ret_val, i, garbage;
struct mp1_blink_struct blink_struct;
if(mp1_set_video_mode() == NULL) {
return -1;
}
for(i = 0; i < (80*25*2); i+= 2)
{
vmem_base_addr[i] = ' ';
vmem_base_addr[i+1] = 7;
}
rtc_fd = open("/dev/rtc", O_RDWR);
ret_val = ioctl(rtc_fd, RTC_ADD, (unsigned long)0);
add_frames(file0, file1, rtc_fd);
ret_val = ioctl(rtc_fd, RTC_IRQP_SET, 16);
ret_val = ioctl(rtc_fd, RTC_PIE_ON, 0);
for(i=0; i<WAIT; i++)
read(rtc_fd, &garbage, 4);
blink_struct.on_char = 'I';
blink_struct.off_char = 'M';
blink_struct.on_length = 7;
blink_struct.off_length = 6;
blink_struct.location = 6*80+60;
ioctl(rtc_fd, RTC_ADD, (unsigned long)&blink_struct);
for(i=0; i<WAIT; i++)
read(rtc_fd, &garbage, 4);
ioctl(rtc_fd, RTC_SYNC, (40 << 16 | (6*80+60)));
for(i=0; i<WAIT; i++)
read(rtc_fd, &garbage, 4);
blink_struct.location = 60;
ret_val = ioctl(rtc_fd, RTC_FIND, (unsigned long)&blink_struct);
ioctl(rtc_fd, RTC_REMOVE, 6*80+60);
for(i=0; i<WAIT; i++)
read(rtc_fd, &garbage, 4);
for(i=0; i<80*25; i++)
ioctl(rtc_fd, RTC_REMOVE, i);
ret_val = ioctl(rtc_fd, RTC_PIE_OFF, 0);
close(rtc_fd);
exit(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);
}
void AudioBuffer::add_frames(const AudioBuffer &_source)
{
add_frames(_source, _source.frames());
}
