void VoNode::processUserActions()
{
char input = remote_input_.c_str()[0];
remote_input_ = "";
if(user_input_thread_ != NULL)
{
char console_input = user_input_thread_->getInput();
if(console_input != 0)
input = console_input;
}
switch(input)
{
case 'q':
quit_ = true;
printf("SVO user input: QUIT\n");
break;
case 'r':
vo_->reset();
printf("SVO user input: RESET\n");
break;
case 's':
vo_->start();
printf("SVO user input: START\n");
break;
default: ;
}
}
void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
{
cv::Mat img;
try {
img = cv_bridge::toCvShare(msg, "mono8")->image;
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
}
processUserActions();
double diff1 = msg->header.stamp.toSec() - time_start.toSec();
// std::cout<<"diff1: "<< diff1 <<std::endl;
if((diff1 > time_window.toSec()) && state == SSTART){
vo_->reset();
vo_->start();
state = SGOT_KEY_1;
}
vo_->addImage(img, msg->header.stamp.toSec());
svo_scale_ = vo_->svo_scale_;
if((vo_->stage() == FrameHandlerMono::STAGE_SECOND_FRAME) && state == SGOT_KEY_1)
{ std::cout<<"the first frame at time: "<<msg->header.stamp.toSec()<<std::endl;
time_first = msg->header.stamp;
state = SGOT_KEY_2;
}
if((vo_->stage() == FrameHandlerMono::STAGE_DEFAULT_FRAME) && state == SGOT_KEY_2)
{ //std::cout<<"the second frame at time: "<<msg->header.stamp.toSec()<<std::endl;
time_second = msg->header.stamp;
state = SGETTING_SCALE;
}
double diff2 = msg->header.stamp.toSec() - time_second.toSec();
// std::cout<<"diff2: "<< diff2 <<std::endl;
// std::cout<<"state: "<< state <<std::endl;
if((diff2 > time_window.toSec()) && state == SGETTING_SCALE){
std::cout<<"time_first: "<< time_first <<std::endl;
std::cout<<"time_second: "<< time_second <<std::endl;
if(VoNode::initCb()){
state = SDEFAULT;
}
}
visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
if(publish_markers_){
visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map(), state == SDEFAULT, svo_scale_, our_scale_);
}
if(publish_dense_input_)
visualizer_.exportToDense(vo_->lastFrame());
if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
usleep(100000);
}