mtsMicronTrackerControllerQtComponent::mtsMicronTrackerControllerQtComponent(const std::string & taskName) :
mtsComponent(taskName)
{
ControllerWidget = new Ui::mtsMicronTrackerControllerQtWidget();
ControllerWidget->setupUi(&CentralWidget);
MTC.FrameLeft.SetSize(FrameSize);
MTC.FrameRight.SetSize(FrameSize);
FrameIndexed8 = QImage(FrameWidth, FrameHeight, QImage::Format_Indexed8);
Timer = new QTimer(this);
MTC.XPoints.resize(50);
MTC.XPointsProjectionLeft.resize(50);
MTC.XPointsProjectionRight.resize(50);
mtsInterfaceRequired * required = AddInterfaceRequired("Controller");
if (required) {
required->AddFunction("ToggleCapturing", MTC.Capture);
required->AddFunction("ToggleTracking", MTC.Track);
required->AddFunction("GetCameraFrameLeft", MTC.GetFrameLeft);
required->AddFunction("GetCameraFrameRight", MTC.GetFrameRight);
required->AddFunction("GetXPointsMaxNum", MTC.GetXPointsMaxNum);
required->AddFunction("GetXPoints", MTC.GetXPoints);
required->AddFunction("GetXPointsProjectionLeft", MTC.GetXPointsProjectionLeft);
required->AddFunction("GetXPointsProjectionRight", MTC.GetXPointsProjectionRight);
}
// connect Qt signals to slots
QObject::connect(ControllerWidget->ButtonTrack, SIGNAL(toggled(bool)),
this, SLOT(MTCTrackQSlot(bool)));
QObject::connect(ControllerWidget->ButtonScreenshot, SIGNAL(clicked()),
this, SLOT(ScreenshotQSlot()));
QObject::connect(this->Timer, SIGNAL(timeout()),
this, SLOT(UpdateFrames()));
}
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;
}
}
//-----------------------------------------------------------------------------
// Name: Render()
// Desc: Called once per frame, the call is the entry point for 3d
// rendering. This function sets up render states, clears the
// viewport, and renders the scene.
//-----------------------------------------------------------------------------
HRESULT CMyD3DApplication::Render()
{
// Set up viewing postion from ArcBall
SDrawElement *pdeCur;
D3DXMATRIXA16 mat;
pdeCur = m_pdeHead;
while (pdeCur != NULL)
{
pdeCur->pframeRoot->matRot = *m_ArcBall.GetRotationMatrix();
pdeCur->pframeRoot->matTrans = *m_ArcBall.GetTranslationMatrix();
pdeCur = pdeCur->pdeNext;
}
// Clear the viewport
m_pd3dDevice->Clear( 0, NULL, D3DCLEAR_TARGET | D3DCLEAR_ZBUFFER, D3DCOLOR_XRGB(89,135,179), 1.0f, 0 );
if (m_pdeHead == NULL)
{
return S_OK;
}
// Begin the scene
if( SUCCEEDED( m_pd3dDevice->BeginScene() ) )
{
UINT cTriangles = 0;
HRESULT hr;
SDrawElement *pdeCur;
D3DXMATRIXA16 mCur;
D3DXVECTOR3 vTemp;
D3DXMatrixTranslation(&m_mView, 0, 0, -m_pdeSelected->fRadius * 2.8f);
hr = m_pd3dDevice->SetTransform(D3DTS_VIEW, (D3DMATRIX*)&m_mView);
if(FAILED(hr))
return hr;
pdeCur = m_pdeHead;
while (pdeCur != NULL)
{
D3DXMatrixIdentity(&mCur);
hr = UpdateFrames(pdeCur->pframeRoot, mCur);
if (FAILED(hr))
return hr;
hr = DrawFrames(pdeCur->pframeRoot, cTriangles);
if (FAILED(hr))
return hr;
pdeCur = pdeCur->pdeNext;
}
// Show frame rate
m_pFont->DrawText( 2, 0, D3DCOLOR_ARGB(255,255,255,0), m_strFrameStats );
m_pFont->DrawText( 2, 20, D3DCOLOR_ARGB(255,255,255,0), m_strDeviceStats );
// End the scene.
m_pd3dDevice->EndScene();
}
return S_OK;
}
void DynamicChain2D::UpdateDynamics()
{
UpdateFrames();
Update_J();
}
