BOOL
WINAPI
StRegisteredWait_Unregister (
__inout PST_REGISTERED_WAIT RegisteredWait
)
{
ST_PARKER Parker;
PST_PARKER OldState;
SPIN_WAIT Spinner;
//
// If the wait is being unregistered from the user callback function,
// we just set the *ExecuteOnlyOnce* to TRUE and return.
//
if (RegisteredWait->ThreadId == GetCurrentThreadId()) {
RegisteredWait->ExecuteOnlyOnce = TRUE;
return TRUE;
}
//
// If the wait is registered, we must try to unregister it and return
// only after the wait is actually unregistered.
// If the wait was already unregistered or an unregister is taking place,
// we return FALSE.
//
InitializeSpinWait(&Spinner);
InitializeParker(&Parker, 0);
do {
if ((OldState = RegisteredWait->State) == INACTIVE) {
return FALSE;
}
if (OldState == ACTIVE) {
if (CasPointer(&RegisteredWait->State, ACTIVE, &Parker)) {
break;
}
continue;
}
if (!StateIsBusy(OldState)) {
return FALSE;
}
SpinOnce(&Spinner);
} while (TRUE);
//
// Try to cancel the wait's callback parker. If we succeed, call
// the unpark with status disposed. Otherwise, a callback is taking
// place and we must synchronize with its end.
//
if (TryCancelParker(&RegisteredWait->CbParker.Parker)) {
UnparkThread(&RegisteredWait->CbParker.Parker, WAIT_UNREGISTERED);
} else {
ParkThreadEx(&Parker, BUSY_SPINS, INFINITE, NULL);
}
//
// Set the registered wait object to inactive and return success.
//
RegisteredWait->State = INACTIVE;
return TRUE;
}
void Mapviz::Initialize()
{
if (!initialized_)
{
if (is_standalone_)
{
// If this Mapviz is running as a standalone application, it needs to init
// ROS and start spinning. If it's running as an rqt plugin, rqt will
// take care of that.
ros::init(argc_, argv_, "mapviz", ros::init_options::AnonymousName);
spin_timer_.start(30);
connect(&spin_timer_, SIGNAL(timeout()), this, SLOT(SpinOnce()));
}
node_ = new ros::NodeHandle("~");
// Create a sub-menu that lists all available Image Transports
image_transport::ImageTransport it(*node_);
std::vector<std::string> transports = it.getLoadableTransports();
QActionGroup* group = new QActionGroup(image_transport_menu_);
for (std::vector<std::string>::iterator iter = transports.begin(); iter != transports.end(); iter++)
{
QString transport = QString::fromStdString(*iter).replace(
QString::fromStdString(IMAGE_TRANSPORT_PARAM) + "/", "");
QAction* action = image_transport_menu_->addAction(transport);
action->setCheckable(true);
group->addAction(action);
}
connect(group, SIGNAL(triggered(QAction*)), this, SLOT(SetImageTransport(QAction*)));
tf_ = boost::make_shared<tf::TransformListener>();
tf_manager_ = boost::make_shared<swri_transform_util::TransformManager>();
tf_manager_->Initialize(tf_);
loader_ = new pluginlib::ClassLoader<MapvizPlugin>(
"mapviz", "mapviz::MapvizPlugin");
std::vector<std::string> plugins = loader_->getDeclaredClasses();
for (unsigned int i = 0; i < plugins.size(); i++)
{
ROS_INFO("Found mapviz plugin: %s", plugins[i].c_str());
}
canvas_->InitializeTf(tf_);
canvas_->SetFixedFrame(ui_.fixedframe->currentText().toStdString());
canvas_->SetTargetFrame(ui_.targetframe->currentText().toStdString());
ros::NodeHandle priv("~");
add_display_srv_ = node_->advertiseService("add_mapviz_display", &Mapviz::AddDisplay, this);
QProcessEnvironment env = QProcessEnvironment::systemEnvironment();
QString default_path = QDir::homePath();
if (env.contains(ROS_WORKSPACE_VAR))
{
// If the ROS_WORKSPACE environment variable is defined, try to read our
// config file out of that. If we can't read it, fall back to trying to
// read one from the user's home directory.
QString ws_path = env.value(ROS_WORKSPACE_VAR, default_path);
if (QFileInfo(ws_path + MAPVIZ_CONFIG_FILE).isReadable())
{
default_path = ws_path;
}
else
{
ROS_WARN("Could not load config file from ROS_WORKSPACE at %s; trying home directory...",
ws_path.toStdString().c_str());
}
}
default_path += MAPVIZ_CONFIG_FILE;
std::string config;
priv.param("config", config, default_path.toStdString());
bool auto_save;
priv.param("auto_save_backup", auto_save, true);
Open(config);
UpdateFrames();
frame_timer_.start(1000);
connect(&frame_timer_, SIGNAL(timeout()), this, SLOT(UpdateFrames()));
if (auto_save)
{
save_timer_.start(10000);
connect(&save_timer_, SIGNAL(timeout()), this, SLOT(AutoSave()));
}
connect(&record_timer_, SIGNAL(timeout()), this, SLOT(CaptureVideoFrame()));
bool print_profile_data;
priv.param("print_profile_data", print_profile_data, false);
if (print_profile_data)
{
profile_timer_.start(2000);
connect(&profile_timer_, SIGNAL(timeout()), this, SLOT(HandleProfileTimer()));
}
initialized_ = true;
}
}
