static int vo_x11_control (int cmd, void *arg)
{
switch (cmd)
{
case VO_TOGGLE_FULLSCREEN:
toggle_full_screen ();
break;
case VO_ZOOM:
vo_x11_zoom (*((int *) arg));
break;
case VO_DRAW_STRING:
vo_draw_string = 1;
vo_lock ();
memcpy (&osd, (osd_t *) arg, sizeof (osd_t));
vo_unlock ();
break;
case VO_ERASE_STRING:
vo_draw_string = 0;
break;
default:
av_log (NULL, AV_LOG_ERROR, "vo x11: cmd not support now!\n");
break;
}
return 0;
}
static void vo_x11_display (AVPicture * pic)
{
vo_lock ();
vo_x11_vfmt2rgb (my_pic, pic);
Ximg->data = my_pic->data[0];
XPutImage (Xdisplay, Xvowin, Xvogc, Ximg, 0, 0, 0, 0, dw, dh);
if (vo_draw_string)
x11_draw_string (&osd);
XFlush (Xdisplay);
vo_unlock ();
}
static int vo_gl_uninit (void)
{
vo_lock ();
glFinish ();
glFlush ();
glDeleteTextures (1, &texName);
avpicture_free (my_pic);
av_free (my_pic);
my_pic = NULL;
vo_lock_free ();
return 0;
}
static int vo_x11_uninit (void)
{
vo_lock ();
if (Xfont_120)
XFreeFont (Xdisplay, Xfont_120);
if (Xfont_240)
XFreeFont (Xdisplay, Xfont_240);
if (Xvogc)
{
XSetBackground (Xdisplay, Xvogc, 0);
XFreeGC (Xdisplay, Xvogc);
Xvogc = NULL;
}
if (Xvowin != None)
{
XClearWindow (Xdisplay, Xvowin);
XUnmapWindow (Xdisplay, Xvowin);
XDestroyWindow (Xdisplay, Xvowin);
Xvowin = None;
}
if (Ximg)
{
Ximg->data = NULL;
XDestroyImage (Ximg);
Ximg = NULL;
}
avpicture_free (my_pic);
av_free (my_pic);
my_pic = NULL;
vo_lock_free ();
return 0;
}
static void vo_x11_zoom (int mulriple)
{
unsigned long xswamask, event_mask;
XSetWindowAttributes xswa;
XSizeHints hint;
XVisualInfo xvinfo;
if (dlpctxp->pwidth * mulriple > screen_width || dlpctxp->pheight * mulriple > screen_height)
{
av_log (NULL, AV_LOG_INFO, "vo x11: zoom %d will > fullscrren\n", mulriple);
return;
}
vo_lock ();
if (Xvowin != None)
{
XClearWindow (Xdisplay, Xvowin);
XUnmapWindow (Xdisplay, Xvowin);
XDestroyWindow (Xdisplay, Xvowin);
Xvowin = None;
}
if (Ximg)
{
Ximg->data = NULL;
XDestroyImage (Ximg);
Ximg = NULL;
}
avpicture_free (my_pic);
av_free (my_pic);
xswamask = CWBackingStore | CWBorderPixel;
dw = dlpctxp->pwidth * mulriple;
dh = dlpctxp->pheight * mulriple;
dx = (screen_width - dw) / 2;
dy = (screen_height - dh) / 2;
Xdepth = XDefaultDepth (Xdisplay, 0);
if (!XMatchVisualInfo (Xdisplay, Xscreen, Xdepth, DirectColor, &xvinfo))
XMatchVisualInfo (Xdisplay, Xscreen, Xdepth, TrueColor, &xvinfo);
xswa.background_pixel = 0;
xswa.border_pixel = 0;
xswa.backing_store = Always;
xswa.bit_gravity = StaticGravity;
Xvowin = XCreateWindow (Xdisplay, Xrootwin, dx, dy, dw, dh, 0, Xdepth, CopyFromParent, CopyFromParent, xswamask, &xswa);
hint.x = dx;
hint.y = dy;
hint.width = dw;
hint.height = dh;
hint.flags = PPosition | PSize;
XSetStandardProperties (Xdisplay, Xvowin, Xtitle, Xtitle, None, NULL, 0, &hint);
XMapWindow (Xdisplay, Xvowin);
XClearWindow (Xdisplay, Xvowin);
event_mask = StructureNotifyMask | KeyPressMask | ExposureMask;
XSelectInput (Xdisplay, Xvowin, event_mask);
XSync (Xdisplay, False);
Ximg = XCreateImage (Xdisplay, xvinfo.visual, Xdepth, ZPixmap, 0, NULL, dw, dh, 8, 0);
if (dw < 600)
Xfont = Xfont_120;
else
Xfont = Xfont_240;
if (Xfont)
XSetFont (Xdisplay, Xvogc, Xfont->fid);
my_pic = av_mallocz (sizeof (AVPicture));
avpicture_alloc (my_pic, my_pic_fmt, dw, dh);
vo_unlock ();
}
static void toggle_full_screen (void)
{
vo_lock ();
av_log (NULL, AV_LOG_ERROR, "VO X11 not support full screen now!\n");
vo_unlock ();
}
// filter loop
void OdomEstimationNode::spin(const ros::TimerEvent& e)
{
ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec());
boost::mutex::scoped_lock odom_lock(odom_mutex_);
boost::mutex::scoped_lock imu_lock(imu_mutex_);
boost::mutex::scoped_lock vo_lock(vo_mutex_);
// check for timing problems
if ( (odom_initializing_ || odom_active_) && (imu_initializing_ || imu_active_) ){
double diff = fabs( Duration(odom_stamp_ - imu_stamp_).toSec() );
if (diff > 1.0) ROS_ERROR("Timestamps of odometry and imu are %f seconds apart.", diff);
}
// initial value for filter stamp; keep this stamp when no sensors are active
filter_stamp_ = Time::now();
// check which sensors are still active
if ((odom_active_ || odom_initializing_) &&
(Time::now() - odom_time_).toSec() > timeout_){
odom_active_ = false; odom_initializing_ = false;
ROS_INFO("Odom sensor not active any more");
}
if ((imu_active_ || imu_initializing_) &&
(Time::now() - imu_time_).toSec() > timeout_){
imu_active_ = false; imu_initializing_ = false;
ROS_INFO("Imu sensor not active any more");
}
if ((vo_active_ || vo_initializing_) &&
(Time::now() - vo_time_).toSec() > timeout_){
vo_active_ = false; vo_initializing_ = false;
ROS_INFO("VO sensor not active any more");
}
// only update filter when one of the sensors is active
if (odom_active_ || imu_active_ || vo_active_){
// update filter at time where all sensor measurements are available
if (odom_active_) filter_stamp_ = min(filter_stamp_, odom_stamp_);
if (imu_active_) filter_stamp_ = min(filter_stamp_, imu_stamp_);
if (vo_active_) filter_stamp_ = min(filter_stamp_, vo_stamp_);
// update filter
if ( my_filter_.isInitialized() ) {
bool diagnostics = true;
if (my_filter_.update(odom_active_, imu_active_, vo_active_, filter_stamp_, diagnostics)){
// output most recent estimate and relative covariance
my_filter_.getEstimate(output_);
pose_pub_.publish(output_);
ekf_sent_counter_++;
// broadcast most recent estimate to TransformArray
StampedTransform tmp;
my_filter_.getEstimate(ros::Time(), tmp);
if(!vo_active_)
tmp.getOrigin().setZ(0.0);
odom_broadcaster_.sendTransform(StampedTransform(tmp, tmp.stamp_, publish_name, "base_footprint"));
#ifdef __EKF_DEBUG_FILE__
// write to file
ColumnVector estimate;
my_filter_.getEstimate(estimate);
for (unsigned int i=1; i<=6; i++)
corr_file_ << estimate(i) << " ";
corr_file_ << endl;
#endif
}
if (!diagnostics)
ROS_WARN("Robot pose ekf diagnostics discovered a potential problem");
}
// initialize filer with odometry frame
if ( odom_active_ && !my_filter_.isInitialized()){
my_filter_.initialize(odom_meas_, odom_stamp_);
ROS_INFO("Kalman filter initialized with odom measurement");
}
else if ( vo_active_ && !my_filter_.isInitialized()){
my_filter_.initialize(vo_meas_, vo_stamp_);
ROS_INFO("Kalman filter initialized with vo measurement");
}
}
};
