Skip to content
Snippets Groups Projects
Commit 8c17ef24 authored by roangel's avatar roangel
Browse files

Halfway there to plot what we get from message

parent b425cfc0
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment