DrivePath::DrivePath(const ros::NodeHandle &nh) :
m_nh(nh), m_client(m_nh, "/move_base", true)
{
m_nh.param("loop_rate", m_loop_rate, 10.0);
std::string waypoints;
m_nh.param("waypoints", waypoints, std::string(""));
assert(waypoints.length() > 0);
parseWaypoints(waypoints);
while (!m_client.waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the move_base action server to come up");
}
m_execute_path = false;
m_go_service = m_nh.advertiseService("/go", &DrivePath::goCallback, this);
}
int main(int argc, char* argv[]) {
// Initialize shared memory
if (ipc.init(false))
exit(1);
// Initialize nominal beaconContainer
parseWaypoints();
// Create camera semaphore/ mutex
sem_init(&semCam, 0, 0);
if (pthread_mutex_init(&mutexImageData, NULL) != 0) {
printf("\n Camera mutex init failed\n");
exit(1);
}
// Start camera thread
pthread_t tid;
int err = pthread_create(&tid, NULL, &camThread, NULL);
if (err != 0) {
printf("\nCan't create camera thread :[%s]", strerror(err));
exit(1);
}
lastPhotogramMicros = getMicroSec();
// Main loop
for(;;photogram++) {
TRACE(DEBUG, "\nPHOTOGRAM %d, milliseconds: %ld\n", photogram, getMillisSinceStart());
// Update time variables
photogramMicros = getMicroSec();
lastPhotogramSpentTime = getSpent(photogramMicros - lastPhotogramMicros);
microsSinceStart += lastPhotogramSpentTime;
lastPhotogramMicros = photogramMicros;
// Get drone roll, pitch and yaw
attitudeT attitude, delayedAttitude;
ipc.getAttitude(attitude);
attitudeSignal.push(attitude, lastPhotogramSpentTime);
delayedAttitude = attitudeSignal.getSignal();
droneRoll = delayedAttitude.roll * 0.01;
dronePitch = delayedAttitude.pitch * 0.01;
droneYaw = delayedAttitude.yaw * 0.01;
// Update estimated servo values
rollStatusServo.update(lastPhotogramSpentTime);
pitchStatusServo.update(lastPhotogramSpentTime);
if (rollStatusServo.inBigStep() || pitchStatusServo.inBigStep()) {
sem_wait(&semCam);
continue;
}
// Update estimated camera roll and pitch
roll = droneRoll + rollStatusServo.getEstimatedAngle();
pitch = dronePitch + pitchStatusServo.getEstimatedAngle();
// Switch beacon containers to preserve one history record
if (beaconContainer == &beaconContainerA) {
beaconContainer = &beaconContainerB;
oldBeaconContainer = &beaconContainerA;
}
else {
beaconContainer = &beaconContainerA;
oldBeaconContainer = &beaconContainerB;
}
beaconContainer->clean();
// Wait until new frame is received
sem_wait(&semCam);
// Where the magic happens
findBeacons();
groupBeacons();
findWaypoints();
computePosition();
}
StopCamera();
printf("Finished.\n");
return 0;
}
bool IOMap::loadMap(Map* map, const std::string& fileName)
{
int64_t start = OTSYS_TIME();
OTB::Loader loader{fileName, OTB::Identifier{{'O', 'T', 'B', 'M'}}};
auto& root = loader.parseTree();
PropStream propStream;
if (!loader.getProps(root, propStream)) {
setLastErrorString("Could not read root property.");
return false;
}
OTBM_root_header root_header;
if (!propStream.read(root_header)) {
setLastErrorString("Could not read header.");
return false;
}
uint32_t headerVersion = root_header.version;
if (headerVersion <= 0) {
//In otbm version 1 the count variable after splashes/fluidcontainers and stackables
//are saved as attributes instead, this solves alot of problems with items
//that is changed (stackable/charges/fluidcontainer/splash) during an update.
setLastErrorString("This map need to be upgraded by using the latest map editor version to be able to load correctly.");
return false;
}
if (headerVersion > 2) {
setLastErrorString("Unknown OTBM version detected.");
return false;
}
if (root_header.majorVersionItems < 3) {
setLastErrorString("This map need to be upgraded by using the latest map editor version to be able to load correctly.");
return false;
}
if (root_header.majorVersionItems > Item::items.majorVersion) {
setLastErrorString("The map was saved with a different items.otb version, an upgraded items.otb is required.");
return false;
}
if (root_header.minorVersionItems < CLIENT_VERSION_810) {
setLastErrorString("This map needs to be updated.");
return false;
}
if (root_header.minorVersionItems > Item::items.minorVersion) {
std::cout << "[Warning - IOMap::loadMap] This map needs an updated items.otb." << std::endl;
}
std::cout << "> Map size: " << root_header.width << "x" << root_header.height << '.' << std::endl;
map->width = root_header.width;
map->height = root_header.height;
if (root.children.size() != 1 || root.children[0].type != OTBM_MAP_DATA) {
setLastErrorString("Could not read data node.");
return false;
}
auto& mapNode = root.children[0];
if (!parseMapDataAttributes(loader, mapNode, *map, fileName)) {
return false;
}
for (auto& mapDataNode : mapNode.children) {
if (mapDataNode.type == OTBM_TILE_AREA) {
if (!parseTileArea(loader, mapDataNode, *map)) {
return false;
}
} else if (mapDataNode.type == OTBM_TOWNS) {
if (!parseTowns(loader, mapDataNode, *map)) {
return false;
}
} else if (mapDataNode.type == OTBM_WAYPOINTS && headerVersion > 1) {
if (!parseWaypoints(loader, mapDataNode, *map)) {
return false;
}
} else {
setLastErrorString("Unknown map node.");
return false;
}
}
std::cout << "> Map loading time: " << (OTSYS_TIME() - start) / (1000.) << " seconds." << std::endl;
return true;
}