INSA/dumber
1
0
Fork 0
mirror of https://github.com/yoboujon/dumber.git synced 2025-06-08 13:50:49 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Fully working positions with multiple robots

Browse source
This commit is contained in:
Robin Marin-Muller 2024-05-15 19:37:20 +02:00
parent ce68e09679
commit d89d36d040
1 changed files with 17 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

29
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -653,6 +653,7 @@ void Tasks::ImageCameraTask(void * arg)
std::vector<MessagePosition*> msgPos = {}; std::vector<MessagePosition*> msgPos = {};
MessageImg *msgImg = nullptr; MessageImg *msgImg = nullptr;
bool pe = false; bool pe = false;
std::list<Position> listPos = {};
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
@ -691,26 +692,30 @@ void Tasks::ImageCameraTask(void * arg)
if(!a.IsEmpty()) if(!a.IsEmpty())
img->DrawArena(a); img->DrawArena(a);
// Check position // Gathering position
auto listPos = img->SearchRobot(a); if(pe)
img->DrawAllRobots(listPos);
// Sending the image to the monitor
if(!listPos.empty())
{ {
// Gather every robot available
listPos = img->SearchRobot(a);
// For every position, draw the robot and send the message
for(auto l : listPos) for(auto l : listPos)
{
img->DrawRobot(l);
msgPos.emplace_back(new MessagePosition(MESSAGE_CAM_POSITION, l)); msgPos.emplace_back(new MessagePosition(MESSAGE_CAM_POSITION, l));
}
} }
msgImg= new MessageImg(MESSAGE_CAM_IMAGE, img); msgImg= new MessageImg(MESSAGE_CAM_IMAGE, img);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
if(!listPos.empty()) // Writing every message position
{ for(auto mp : msgPos)
for(auto mg : msgPos) monitor.Write(mp);
monitor.Write(mg); // Writing every message image
msgPos.clear();
}
monitor.Write(msgImg); monitor.Write(msgImg);
rt_mutex_release(&mutex_monitor); rt_mutex_release(&mutex_monitor);
// clearing the vector because all its values are unassigned
msgPos.clear();
} }
} }
} }