Commit 8c17ef24 authored by roangel's avatar roangel
Browse files

Halfway there to plot what we get from message

parent b425cfc0
......@@ -107,6 +107,8 @@ private:
myGraphicsScene* scene;
void _init();
std::vector<Marker*> markers_vector;
Marker* marker;
#ifdef CATKIN_MAKE
......
......@@ -67,9 +67,7 @@ void MainGUIWindow::_init()
scene->setSceneRect(-100 * FROM_METERS_TO_UNITS, -100 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS);
marker = new Marker(1 * FROM_METERS_TO_UNITS, - 1 * FROM_METERS_TO_UNITS);
// marker->setPos(0,0);
// setPosMarkers(1, 1);
// scene->addItem(marker);
marker->setPos(100, -200);
ui->graphicsView->setScene(scene);
......@@ -90,6 +88,30 @@ void MainGUIWindow::_init()
#ifdef CATKIN_MAKE
void MainGUIWindow::setPosMarkers(const UnlabeledMarkersArray::ConstPtr& p_msg)
{
// TODO: Create/edit vector of markers depending on p_msg.
// Delete previous markers and plot new? or change position of related indexes?
if(p_msg->markers.size() < markers_vector.size()) // some markers have dissapeared
{
for(int i = p_msg->markers.size(); i < markers_vector.size(); i++)
{
scene->removeItem(markers_vector[i]);
}
markers_vector.erase(markers_vector.begin() + p_msg->markers.size(), markers_vector.end());
}
for(int i = 0; i < p_msg->markers.size(); i++) // here, or new markers message is equal to current messages, or greater (some new markers)
{
if(i >= markers_vector.size()) //some new markers coming
{
markers_vector[i] = new Marker(p_msg->markers[i].x * FROM_METERS_TO_UNITS, p_msg->markers[i].y * FROM_METERS_TO_UNITS);
scene->addItem(markers_vector[i]);
}
else
{
markers_vector[i]->setPosMarker(QPointF(p_msg->markers[i].x * FROM_METERS_TO_UNITS, p_msg->markers[i].y * FROM_METERS_TO_UNITS));
}
}
// marker->setPosMarker(scene->mapFromWorldToScene(QPointF(FROM_METERS_TO_UNITS * x, FROM_METERS_TO_UNITS * y)));
}
#endif
......
......@@ -48,10 +48,10 @@ void rosNodeThread::messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg
for(int i = 0; i < p_msg->markers.size(); i++)
{
const UnlabeledMarker &marker = p_msg->markers[i];
emit newViconData(p_msg);
ROS_INFO_STREAM("index: " << marker.index << " x: " << marker.x <<
" y: " << marker.y << " z: " << marker.z);
}
emit newViconData(p_msg);
}
// void rosNodeThread::messageCallback(const ViconData& data) // When a message arrives to the topic, this callback is executed
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment