Access vtkActor construction object and append new data - c++

I am working with VTK-6.1 and I want to create a single vtkActor using multiple vtkUnstructuredGrid. First I create the actor from a grid...
vtkSmartPointer<vtkActor> create_actor()
{
vtkSmartPointer<vtkUnstructuredGrid> grid = create_grid_a();
auto mapper = vtkSmartPointer<vtkDataSetMapper>::New();
mapper->SetInputData(grid);
auto actor = vtkSmartPointer<vtkActor>::New();
actor->SetMapper(mapper);
actor->GetProperty()->SetColor(0.5, 0.5, 0.5);
actor->GetProperty()->SetEdgeVisibility(1);
return actor;
}
and then I (try) to update actor using another grid:
void update_actor(<vtkSmartPointer<vtkActor> actor)
{
// get current grid
vtkAlgorithm* algorithm = actor->GetMapper()->GetInputAlgorithm();
actor_grid = dynamic_cast<vtkUnstructuredGrid>(algorithm);
// create new grid
vtkSmartPointer<vtkUnstructuredGrid> new_grid = create_grid_b();
// combine grids
auto append_filter = vtkSmartPointer<vtkAppendFilter>::New();
append_filter->AddInputData(actor_grid);
append_filter->AddInputData(new_grid);
append_filter->Update();
// update actor
auto mapper = vtkSmartPointer<vtkDataSetMapper>::New();
mapper->SetInputData(append_filter->GetOutput());
actor->SetMapper(mapper);
}
The problem is that the actor does not contain the combination of grids.
Notes:
(create_grid_x() functions are a little too complex to provide but they
certainly work because first actor is created correctly)
SafeDownCast as shown in 1 is not working too.
References:
vtkActor reverse access
Appending data

The problematic section was the retrieval of vtkUnstructuredGrid from vtkActor.
// get current grid
vtkDataSet* actor_data = actor->getMapper()->GetInput();
...instead of using vtkAlgorithm

Related

Transform static object to map frame

i have a very simple and maybe stupid question, sorry i am new to this kinf of stuff.
I had a file of points, published them to a topic as a pointcloud and subscribed from another node to the topic to modify the poincloud and published it again. Everthing is fine so far. The problem is that the pointcloud (the object) ist not in the origin when i visualize it in RVIZ. It is somewhere at the edge of the platform in RVIZ. How can i make the pointcloud move to the origin in a easy way?
So far i have tried some solutions with the tf2 package. Tried to transform the frame of the object to the map frame, where i want to have the object at the ent. But doenst seem to work. I am missing something. The question is, is this the right and easiest approach for this or is there a better way. If its the right approach, what am i missing?
I put this code in my callback function:
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = this->get_clock()->now();
transform.header.frame_id = "object_frame";
transform.child_frame_id = "map";
// Set the position of the object relative to the Rviz frame
transform.transform.translation.x = 0.0;
transform.transform.translation.y = 0.0;
transform.transform.translation.z = 0.0;
// Set the orientation of the object relative to the Rviz frame
transform.transform.rotation.x = 0.0;
transform.transform.rotation.y = 0.0;
transform.transform.rotation.z = 0.0;
transform.transform.rotation.w = 1.0;
tf_broadcaster->sendTransform(transform);
And the following to my publisher function:
void Preprocessor::publish_pointcloud_supervoxel()
{
// Convert the PointCloud to a PointCloud2 message
auto pcl_msg_supervoxel = std::make_shared<sensor_msgs::msg::PointCloud2>();
//sensor_msgs::msg::PointCloud2 pcl_msg_supervoxel;
pcl::toROSMsg(*colored_supervoxel_cloud, *pcl_msg_supervoxel);
//pcl_msg_supervoxel->width = adjacent_supervoxel_centers.size();
pcl_msg_supervoxel->header.frame_id = "map";
pcl_msg_supervoxel->header.stamp = this->get_clock()->now();
// Publish the message
supervoxel_publisher->publish(*pcl_msg_supervoxel);
}
Not knowing your rviz setup, have you checked if the Global options:fixed frame in rviz is set to the correct frame?
You can try a static transform and see if that is reflected in rviz. This would be in the launch file.
static_transform_node = Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments=["x", "y", "z", "roll", "pitch", "yaw", "parent_frame_id", "child_frame_id"],
output="screen",
)
Note: the quotes in the arguments are required
ref: https://wiki.ros.org/tf2_ros#static_transform_publisher

How are you supposed to use a controller with a URDF

I'm trying to use the LQR Controller to move the wheels on this really simple URDF linked here
I'm trying to understand how to use the LQR controller for a model loaded from a URDF so this is a mix of the LQR example and the PR2 Example
systems::DiagramBuilder<double> builder;
auto pair = AddMultibodyPlantSceneGraph(
&builder,
std::make_unique<MultibodyPlant<double>>(
FLAGS_mbp_discrete_update_period));
MultibodyPlant<double>& plant = pair.plant;
const std::string full_name ="doublependulum/fetch/urdf/freight.urdf";
auto parser = multibody::Parser(&plant);
parser.package_map().PopulateFromFolder("/root/");
parser.AddModelFromFile(full_name);
// Add model of the ground.
const double static_friction = 0.5;
const Vector4<double> green(0.5, 1.0, 0.5, 1.0);
plant.RegisterVisualGeometry(plant.world_body(), RigidTransformd(),
geometry::HalfSpace(), "GroundVisualGeometry",
green);
// For a time-stepping model only static friction is used.
const multibody::CoulombFriction<double> ground_friction(static_friction,
static_friction);
plant.RegisterCollisionGeometry(plant.world_body(), RigidTransformd(),
geometry::HalfSpace(),
"GroundCollisionGeometry", ground_friction);
plant.Finalize();
plant.set_penetration_allowance(FLAGS_penetration_allowance);
// Set the speed tolerance (m/s) for the underlying Stribeck friction model
// For two points in contact, this is the maximum allowable drift speed at the
// edge of the friction cone, an approximation to true stiction.
plant.set_stiction_tolerance(FLAGS_stiction_tolerance);
const drake::multibody::Body<double>& base = plant.GetBodyByName("base_link");
ConnectContactResultsToDrakeVisualizer(&builder, plant);
geometry::DrakeVisualizer::AddToBuilder(&builder, pair.scene_graph);
auto diagram = builder.Build();
/// Need to add actuators to the models in this form
// Create a context for this system:
std::unique_ptr<systems::Context<double>> diagram_context =
diagram->CreateDefaultContext();
systems::Context<double>& plant_context =
diagram->GetMutableSubsystemContext(plant, diagram_context.get());
Eigen::MatrixXd Q(2, 2);
Q << 10, 0, 0, 1;
Eigen::MatrixXd R(1, 1);
R << 1;
auto controller =
builder.AddSystem(systems::controllers::LinearQuadraticRegulator(
plant, plant_context, Q, R));
builder.Connect(plant.get_state_output_port(),
controller->get_input_port());
builder.Connect(controller->get_output_port(), plant.get_input_port());
However, I get a runtime error:
what(): System::FixInputPortTypeCheck(): expected value of type drake::geometry::QueryObject<drake::AutoDiffXd> for input port 'geometry_query' (index 0) but the actual type was drake::geometry::QueryObject<double>. (System ::plant)
Can someone explain how to use the LQR controller for a Model
There are a few problems here. The first, and the one generating the error, is the fact that you've passed a plant_context (only) into the LinearQuadraticRegulator. Because your system has collision geometry, you need a SceneGraph to be connected for the dynamics to be evaluated. You would want to pass the diagram containing both the MultibodyPlant and the SceneGraph into the LQR call.
But the real problem is going to be deeper than that. LQR is not going to work out of the box for you on this model. That's not a Drake issue, it's a math issue. The system you've described not only has collision dynamics, but also is not going to be controllable in the linearization. When people use LQR to stabilizing wheeled robots, they do it in a minimal coordinates which assumes that the wheels are attached to the ground at a point. In drake, that would mean writing your own LeafSystem with the vehicle dynamics.

Qt3D - Instance rendering of 2 squares. PrimitiveType? InstanceCount?

I'd like to display on the screen 2 squares using instance rendering.
The problem is :
When I draw 2 squares, they are connected. I can't draw multiple separate square.
I know it has to do with PrimitiveType : i'm using Qt3DRender::QGeometryRenderer::LineLoop or Qt3DRender::QGeometryRenderer::TriangleFan but is there a way to tell Qt to separate my data buffer in multiple instances ? so that the PrimitiveType apply on each instances and not all the vertices.
I got a function that create shapes :
Qt3DCore::QEntity* SceneBuilder::createShapeInstancing(const QList<QVector3D> & listVertices, int nbPointGeometry, const QColor color, Qt3DCore::QEntity * rootEntity)
{
// Material
Qt3DExtras::QPhongMaterial *material = new Qt3DExtras::QPhongMaterial(rootEntity);
material->setAmbient(color);
// Custom entity
Qt3DCore::QEntity *customMeshEntity = new Qt3DCore::QEntity(rootEntity);
// Custom Mesh
Qt3DRender::QGeometryRenderer *customMeshRenderer = new Qt3DRender::QGeometryRenderer(rootEntity);
Qt3DRender::QGeometry *customGeometry = new Qt3DRender::QGeometry(customMeshRenderer);
Qt3DRender::QBuffer *vertexDataBuffer = new Qt3DRender::QBuffer(Qt3DRender::QBuffer::VertexBuffer, customGeometry);
int nbPointGeometry = nbPoint;
QByteArray vertexBufferData;
vertexBufferData.resize(listVertices.size() * nbPointGeometry * sizeof(QVector3D));
QVector3D *posData = reinterpret_cast<QVector3D *>(vertexBufferData.data());
QList<QVector3D>::const_iterator it;
for (it = listVertices.begin(); it != listVertices.end(); it++)
{
*posData = *it;
++posData;
}
vertexDataBuffer->setData(vertexBufferData);
//Attributes
Qt3DRender::QAttribute *positionAttribute = new Qt3DRender::QAttribute();
positionAttribute->setName(Qt3DRender::QAttribute::defaultPositionAttributeName());
positionAttribute->setAttributeType(Qt3DRender::QAttribute::VertexAttribute);
positionAttribute->setBuffer(vertexDataBuffer);
positionAttribute->setVertexBaseType(Qt3DRender::QAttribute::Float);
positionAttribute->setVertexSize(3);
positionAttribute->setByteOffset(0);
positionAttribute->setByteStride((0 + 3) * sizeof(float));
positionAttribute->setCount(listVertices.size());
//positionAttribute->setDivisor(1);
customGeometry->addAttribute(positionAttribute);
customMeshRenderer->setGeometry(customGeometry);
//customMeshRenderer->setInstanceCount(listVertices.size()/nbPoint);
if (nbPointGeometry == 1)
customMeshRenderer->setPrimitiveType(Qt3DRender::QGeometryRenderer::Points);
else if (nbPointGeometry == 2)
customMeshRenderer->setPrimitiveType(Qt3DRender::QGeometryRenderer::Lines);
else if (nbPointGeometry == 3)
customMeshRenderer->setPrimitiveType(Qt3DRender::QGeometryRenderer::Triangles);
else
customMeshRenderer->setPrimitiveType(Qt3DRender::QGeometryRenderer::LineLoop);
customMeshEntity->addComponent(customMeshRenderer);
customMeshEntity->addComponent(material);
return customMeshEntity;
}
When i call this function, i put my 2 squares inside the listVertices variables. So that all my squares are display in one draw call.
But the shapes are one way or another connected. Is there a way to remove that ? I'm not using InstanceCount or Divisor but i don't know how it works. I made some tests with it but nothing worked
Actually, I just found an example of instanced rendering here.
They subclassed QSphereGeometry and performed the instanced rendering in a shader. You could create a RectangleGeometry which holds the vertices for one triangle and equip it with the same functionality as the InstancedGeometry in the example.
Result:

Atached Actor Mesh dont stick to Parent Actor

Context:
I'm creating some Actors (Shields) using C++ in ActorComponent (ShieldComponent) which is attached to Actor (Ship).
When I move my Ship, Actors (Shields) stay in place on world (don't move with Ship).
What can be important: I don't call CreateShieldActor in Constructor, so I can't use ConstructorHelpers::FObjectFinder instead of StaticLoadObject/LoadObject to load Mesh/Material.
My C++ code for creation looks like this:
AShieldPart* UShieldComponent::CreateShieldActor(AActor* Owner, FString MeshName)
{
//Create Shield Actor
FVector Location = Owner->GetActorLocation();
FRotator Rotator(0, 0, 0);
FVector Scale(10, 10, 10);
FActorSpawnParameters SpawnInfo;
SpawnInfo.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn;
SpawnInfo.Owner = Owner;
FTransform Transform(Rotator, Location, Scale);
UClass * MeshClass = AShieldPart::StaticClass();
AShieldPart* ShieldPart = GetWorld()->SpawnActor<AShieldPart>(MeshClass, Transform, SpawnInfo);
//Attach Mesh to Shield Actor
ShieldPart->AttachToActor(Owner, FAttachmentTransformRules(EAttachmentRule::KeepRelative, false));
//Set Mesh for Shield
ShieldPart->SetNewMesh(MeshName);
//Set Material
UMaterial* Material = Cast<UMaterial>(StaticLoadObject(UMaterial::StaticClass(), NULL, TEXT("/some/path")));
ShieldPart->MeshComponent->SetMaterial(0, Material);
return ShieldPart;
}
AShieldPart::AShieldPart()
{
// Set this actor to call Tick() every frame. You can turn this off to improve performance if you don't need it.
PrimaryActorTick.bCanEverTick = true;
RootComponent = CreateDefaultSubobject<USceneComponent>(TEXT("RootComponent"));
SetRootComponent(RootComponent);
MeshComponent = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("MeshComponent"));
MeshComponent->SetupAttachment(RootComponent);
}
void AShieldPart::SetNewMesh(FString MeshName)
{
UStaticMesh* Mesh = Cast<UStaticMesh>(StaticLoadObject(UStaticMesh::StaticClass(), NULL, *MeshName));
//UStaticMesh* Mesh = LoadObject<UStaticMesh>(nullptr, *MeshName);
MeshComponent->SetStaticMesh(Mesh);
MeshComponent->SetCollisionProfileName(TEXT("BlockAll"));
MeshComponent->SetNotifyRigidBodyCollision(true);
MeshComponent->SetSimulatePhysics(true);
MeshComponent->SetEnableGravity(false);
MeshComponent->RegisterComponent();
}
That it would be more interesting, when I create that Actors (Shields) using BP all works fine...
The solution turned out to be eliminating the USceneComponent as a RootElement. The main element must be UStaticMeshComponent, then it can attached to the parent element (Ship).
AShieldPart::AShieldPart()
{
// Set this actor to call Tick() every frame. You can turn this off to improve performance if you don't need it.
PrimaryActorTick.bCanEverTick = true;
// Creating MeshComponent and attach to RootComponent
MeshComponent = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("MeshComponent"));
RootComponent = MeshComponent;
MeshComponent->AttachToComponent(RootComponent, FAttachmentTransformRules(EAttachmentRule::SnapToTarget, true));
...

VTK pipeline update

I use VTK-6.2, C++ (gcc-4.7.2) on Linux and I have the following VTK pipeline setup (please ignore implementation, details and focus on the pipeline: cone->filter->mapper->actor):
// cone/initialize
vtkConeSource cone;
// add cone(s) to filter
vtkAppendFilter filter;
filter.AddInputData(cone.GetOutput());
// add filter to mapper
vtkDataSetMapper mapper;
mapper.SetInputData(filter->GetOutput());
// actor
vtkActor actor;
actor.SetMapper(mapper);
The scene renders fine.
The Problem
I want to update the original data (i.e. the cones) and the actor to be rendered correctly.
How do I access the original cone data if I have just the actors? Does this guarantee that the actors will be updated too? Because when I decided to keep track of the original data (via pointers: the whole implementation is with vtkSmartPointers) and then change some of their attributes, the pipeline did not update. Shouldn't it update automatically?
(When I change the actor (e.g. their visibility), the scene renders fine)
Forgive me, I am not a VTK expert and the pipelines are confusing. Maybe one approach would be to simplify my pipeline.
Thanks
[update]
According to this answer to a similar post, the original data (vtkConeSource) are transformed (to vtkUnstructuredGrid when added in the vtkAppendFilter) so even if I keep track of the original data, changing them is useless.
VTK pipelines are demand-driven pipelines. They do not update automatically even if one of the elements of the pipeline is modified. We need to explicitly call the Update() function on the last vtkAlgorithm( or its derived class object) of the pipeline to update the entire pipeline. The correct way to set up a pipeline is when we are connecting two objects which are derived from vtkAlgorithm type is to use
currAlgoObj->SetInputConnection( prevAlgoObj->GetOutputPort() )
instead of
currAlgoObj->SetInputData( prevAlgo->GetOutput() )
Then we can update the pipeline using the pointer to the actor object by doing actor->GetMapper()->Update() like shown in the example below.
In this example, we will create a cone from a cone source, pass it through vtkAppendFilter and then change the height of the original cone source and render it in another window to see the updated cone. (You will have to close the first render window to see the updated cone in second window.)
#include <vtkConeSource.h>
#include <vtkDataSetMapper.h>
#include <vtkActor.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkSmartPointer.h>
#include <vtkAppendFilter.h>
int main(int, char *[])
{
// Set up the data pipeline
auto cone = vtkSmartPointer<vtkConeSource>::New();
cone->SetHeight( 1.0 );
auto appf = vtkSmartPointer<vtkAppendFilter>::New();
appf->SetInputConnection( cone->GetOutputPort() );
auto coneMapper = vtkSmartPointer<vtkDataSetMapper>::New();
coneMapper->SetInputConnection( appf->GetOutputPort() );
auto coneActor = vtkSmartPointer<vtkActor>::New();
coneActor->SetMapper( coneMapper );
// We need to update the pipeline otherwise nothing will be rendered
coneActor->GetMapper()->Update();
// Connect to the rendering portion of the pipeline
auto renderer = vtkSmartPointer<vtkRenderer>::New();
renderer->AddActor( coneActor );
renderer->SetBackground( 0.1, 0.2, 0.4 );
auto renderWindow = vtkSmartPointer<vtkRenderWindow>::New();
renderWindow->SetSize( 200, 200 );
renderWindow->AddRenderer(renderer);
auto renderWindowInteractor =
vtkSmartPointer<vtkRenderWindowInteractor>::New();
renderWindowInteractor->SetRenderWindow(renderWindow);
renderWindowInteractor->Start();
// Change cone property
cone->SetHeight( 10.0 );
//Update the pipeline using the actor object
coneActor->GetMapper()->Update();
auto renderer2 = vtkSmartPointer<vtkRenderer>::New();
renderer2->AddActor( coneActor );
renderer2->SetBackground( 0.1, 0.2, 0.4 );
auto renderWindow2 = vtkSmartPointer<vtkRenderWindow>::New();
renderWindow2->SetSize( 200, 200 );
renderWindow2->AddRenderer(renderer2);
auto renderWindowInteractor2 =
vtkSmartPointer<vtkRenderWindowInteractor>::New();
renderWindowInteractor2->SetRenderWindow(renderWindow2);
renderWindowInteractor2->Start();
return EXIT_SUCCESS;
}