void MoveDemoApplication::createScene(void)
{
// Set the default lighting.
mSceneMgr->setAmbientLight( ColourValue( 1.0f, 1.0f, 1.0f ) );
createAxes();
createGrid();
createPath();
// Create the entity
mEntity = mSceneMgr->createEntity( "Robot", "robot.mesh" );
mNode = mSceneMgr->getRootSceneNode( )->createChildSceneNode( "RobotNode", Vector3( 0.0f, 0.0f, 25.0f ) );
mNode->attachObject( mEntity );
mNode->showBoundingBox(true);
// Create the walking list
mWalkList.push_back( Vector3( 550.0f, 0.0f, 50.0f ) );
mWalkList.push_back( Vector3(-100.0f, 0.0f, -200.0f ) );
mWalkList.push_back( Vector3( 0.0f, 0.0f, 25.0f ) );
// Create objects so we can see movement
Entity *ent;
SceneNode *node;
ent = mSceneMgr->createEntity( "ninja", "ninja.mesh" );
node = mSceneMgr->getRootSceneNode( )->createChildSceneNode( "ninja", Vector3( 50.0f, 0.0f, -50.0f ) );
node->attachObject( ent );
node->yaw( Degree(-180) );
ent = mSceneMgr->createEntity( "Knot1", "knot.mesh" );
node = mSceneMgr->getRootSceneNode( )->createChildSceneNode( "Knot1Node",
Vector3( 0.0f, -10.0f, 25.0f ) );
node->attachObject( ent );
node->setScale( 0.1f, 0.1f, 0.1f );
ent = mSceneMgr->createEntity( "Knot2", "knot.mesh" );
node = mSceneMgr->getRootSceneNode( )->createChildSceneNode( "Knot2Node",
Vector3( 550.0f, -10.0f, 50.0f ) );
node->attachObject( ent );
node->setScale( 0.1f, 0.1f, 0.1f );
ent = mSceneMgr->createEntity( "Knot3", "knot.mesh" );
node = mSceneMgr->getRootSceneNode( )->createChildSceneNode( "Knot3Node",
Vector3(-100.0f, -10.0f,-200.0f ) );
node->attachObject( ent );
node->setScale( 0.1f, 0.1f, 0.1f );
// Set the camera to look at our handywork
mCamera->setPosition( 90.0f, 280.0f, 535.0f );
mCamera->pitch( Degree(-30.0f) );
mCamera->yaw( Degree(-15.0f) );
//! TEST ONLY
//AnimationState *mAnimationState; // The current animation state of the object
//mAnimationState = mEntity->getAnimationState("Idle");
//mAnimationState->setLoop(true);
//mAnimationState->setEnabled(true);
}
void TrackDisplay::update(float wall_dt, float ros_dt) {
V_TrackMsg local_queue;
{
boost::mutex::scoped_lock lock(queue_mutex_);
local_queue.swap(message_queue_);
}
for (size_t t = 0; t < local_queue.size(); t++) {
const articulation_msgs::TrackMsg::ConstPtr& track_message =
local_queue[t];
for(size_t l=0;l<lines[track_message->id].size();l++)
lines[track_message->id][l]->clear();
recycleLines.insert(recycleLines.begin(),
lines[track_message->id].begin(),
lines[track_message->id].end());
lines[track_message->id].clear();
}
for (size_t t = 0; t < local_queue.size(); t++) {
const articulation_msgs::TrackMsg::ConstPtr& track_message =
local_queue[t];
btTransform framePose;
btVector3 lineScale(lineWidth_, lineWidth_, lineWidth_);
transform(track_message, framePose);
int channel_w = -1;
int channel_h = -1;
for (size_t i = 0; i < track_message->channels.size(); i++) {
if (track_message->channels[i].name == "width")
channel_w = (int) i;
if (track_message->channels[i].name == "height")
channel_h = (int) i;
}
Ogre::Vector3 old_pos(0,0,0);
for (size_t i = 0; i < track_message->pose.size(); i++) {
const geometry_msgs::Pose& geo_pose = track_message->pose[i];
btTransform rectangle_pose(btQuaternion(geo_pose.orientation.x,
geo_pose.orientation.y, geo_pose.orientation.z,
geo_pose.orientation.w), btVector3(geo_pose.position.x,
geo_pose.position.y, geo_pose.position.z));
Ogre::Vector3 pos, scale;
Ogre::Quaternion orient;
transform(framePose * rectangle_pose, lineScale, pos, orient, scale);
btVector3 color(color_.r_, color_.g_, color_.b_); // fixed color
double f =track_message->id / 7.0;
color = modifyColor( color, trackColor_, f - floor(f) );
color = modifyColor( color, poseColor_, i / (double)track_message->pose.size() );
if(displayStyle_ == ds_line) {
if(i==0) old_pos = pos;
createLine(pos, old_pos, scale,
color,
lines[track_message->id],false);
old_pos = pos;
} else
if(displayStyle_ == ds_cross_line) {
if(i==0) old_pos = pos;
createLine(pos, old_pos, scale,
color,
lines[track_message->id],true);
old_pos = pos;
} else
if(displayStyle_ == ds_axes) {
createAxes(pos, orient, scale,
color,
lines[track_message->id]);
} else
if(displayStyle_ == ds_rectangle) {
createRectangle(pos, orient, scale,
channel_w==-1? lineWidth_*5 : track_message->channels[channel_w].values[i],
channel_h==-1? lineWidth_*5 : track_message->channels[channel_h].values[i],
color,
lines[track_message->id]);
}
}
}
}
void RendererImplementationBase3D::initialize()
{
if (m_isInitialized)
{
return;
}
// -- lights --
m_lightKit = vtkSmartPointer<vtkLightKit>::New();
m_lightKit->SetKeyLightIntensity(1.0);
// -- render (window) --
m_renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
m_viewportSetups.resize(renderView().numberOfSubViews());
for (unsigned int subViewIndex = 0; subViewIndex < renderView().numberOfSubViews(); ++subViewIndex)
{
auto & viewport = m_viewportSetups[subViewIndex];
auto renderer = vtkSmartPointer<vtkRenderer>::New();
renderer->SetBackground(1, 1, 1);
renderer->RemoveAllLights();
m_lightKit->AddLightsToRenderer(renderer);
viewport.renderer = renderer;
m_renderWindow->AddRenderer(renderer);
auto titleWidget = vtkSmartPointer<vtkTextWidget>::New();
viewport.titleWidget = titleWidget;
titleWidget->SetDefaultRenderer(viewport.renderer);
titleWidget->SetCurrentRenderer(viewport.renderer);
auto titleRepr = vtkSmartPointer<vtkTextRepresentation>::New();
auto titleActor = titleRepr->GetTextActor();
titleActor->GetTextProperty()->SetColor(0, 0, 0);
titleActor->GetTextProperty()->SetVerticalJustificationToTop();
FontHelper::configureTextProperty(*titleActor->GetTextProperty());
titleRepr->GetPositionCoordinate()->SetValue(0.2, .85);
titleRepr->GetPosition2Coordinate()->SetValue(0.6, .10);
titleRepr->GetBorderProperty()->SetColor(0.2, 0.2, 0.2);
titleWidget->SetRepresentation(titleRepr);
titleWidget->SetTextActor(titleActor);
titleWidget->SelectableOff();
viewport.axesActor = createAxes();
renderer->AddViewProp(viewport.axesActor);
}
// -- interaction --
m_interactorStyle = vtkSmartPointer<CameraInteractorStyleSwitch>::New();
m_interactorStyle->SetCurrentRenderer(m_viewportSetups.front().renderer);
m_interactorStyle->addStyle("InteractorStyleTerrain", vtkSmartPointer<InteractorStyleTerrain>::New());
m_interactorStyle->addStyle("InteractorStyleImage", vtkSmartPointer<InteractorStyleImage>::New());
// correctly show axes and labels based on the interaction style and resulting camera setup
m_interactorStyle->AddObserver(InteractorStyleSwitch::StyleChangedEvent,
this, &RendererImplementationBase3D::updateAxes);
m_pickerHighlighter = vtkSmartPointer<PickerHighlighterInteractorObserver>::New();
connect(m_pickerHighlighter, &PickerHighlighterInteractorObserver::pickedInfoChanged,
&m_renderView, &AbstractRenderView::showInfoText);
connect(m_pickerHighlighter, &PickerHighlighterInteractorObserver::dataPicked,
[this] (const VisualizationSelection & selection)
{
if (selection.isEmpty())
{
m_renderView.clearSelection();
}
else
{
m_renderView.setVisualizationSelection(selection);
}
});
connect(m_pickerHighlighter, &PickerHighlighterInteractorObserver::geometryChanged,
this, &RendererImplementation::render);
m_cursorCallback = std::make_unique<RenderWindowCursorCallback>();
m_cursorCallback->setRenderWindow(m_renderWindow);
m_isInitialized = true;
}
