void OccupancyPredictor::predictFromGrids(const QHash<QString, OccupancyGrid> &grids)
{
for (const QString& date: grids.keys())
{
updatePrediction(grids[date]);
}
}
void ElevationMapping::mapUpdateTimerCallback(const ros::TimerEvent&)
{
ROS_WARN("Elevation map is updated without data from the sensor.");
boost::recursive_mutex::scoped_lock scopedLock(map_.getRawDataMutex());
stopMapUpdateTimer();
Time time = Time::now();
// Update map from motion prediction.
if (!updatePrediction(time)) {
ROS_ERROR("Updating process noise failed.");
resetMapUpdateTimer();
return;
}
// Publish elevation map.
map_.publishRawElevationMap();
if (isContinouslyFusing_ && map_.hasFusedMapSubscribers()) {
map_.fuseAll(true);
map_.publishFusedElevationMap();
}
resetMapUpdateTimer();
}
Vector KalmanFilterBase::getPrediction()
{
updatePrediction();
return oc_.xbar;
}
/*
* Compute the refinement of `oldPredicted' with `newPredicted', maintaining
* the invariant that `refined <= proven'.
*/
Type refinePrediction(Type oldPredicted, Type newPredicted, Type proven) {
auto refined = oldPredicted & newPredicted;
if (refined == TBottom) refined = newPredicted;
return updatePrediction(refined, proven);
}
void ElevationMapping::pointCloudCallback(
const sensor_msgs::PointCloud2& rawPointCloud)
{
stopMapUpdateTimer();
boost::recursive_mutex::scoped_lock scopedLock(map_.getRawDataMutex());
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud.
// TODO Double check with http://wiki.ros.org/hydro/Migration
pcl::PCLPointCloud2 pcl_pc;
pcl_conversions::toPCL(rawPointCloud, pcl_pc);
PointCloud<PointXYZRGB>::Ptr pointCloud(new PointCloud<PointXYZRGB>);
pcl::fromPCLPointCloud2(pcl_pc, *pointCloud);
Time time;
time.fromNSec(1000 * pointCloud->header.stamp);
ROS_DEBUG("ElevationMap received a point cloud (%i points) for elevation mapping.", static_cast<int>(pointCloud->size()));
// Update map location.
updateMapLocation();
// Update map from motion prediction.
if (!updatePrediction(time)) {
ROS_ERROR("Updating process noise failed.");
resetMapUpdateTimer();
return;
}
// Get robot pose covariance matrix at timestamp of point cloud.
boost::shared_ptr<geometry_msgs::PoseWithCovarianceStamped const> poseMessage = robotPoseCache_.getElemBeforeTime(time);
if (!poseMessage)
{
ROS_ERROR("Could not get pose information from robot for time %f. Buffer empty?", time.toSec());
return;
}
Eigen::Matrix<double, 6, 6> robotPoseCovariance = Eigen::Map<const Eigen::MatrixXd>(poseMessage->pose.covariance.data(), 6, 6);
// Process point cloud.
PointCloud<PointXYZRGB>::Ptr pointCloudProcessed(new PointCloud<PointXYZRGB>);
Eigen::VectorXf measurementVariances;
if (!sensorProcessor_->process(pointCloud, robotPoseCovariance, pointCloudProcessed,
measurementVariances)) {
ROS_ERROR("Point cloud could not be processed.");
resetMapUpdateTimer();
return;
}
// Add point cloud to elevation map.
if (!map_.add(pointCloudProcessed, measurementVariances)) {
ROS_ERROR("Adding point cloud to elevation map failed.");
resetMapUpdateTimer();
return;
}
// Publish elevation map.
map_.publishRawElevationMap();
if (isContinouslyFusing_ && map_.hasFusedMapSubscribers()) {
map_.fuseAll(true);
map_.publishFusedElevationMap();
}
resetMapUpdateTimer();
}
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) const
{
if(!_fullPredictionUpdate && !_prediction.empty())
{
return updatePrediction(_prediction, memory, uKeys(_posterior), ids);
}
UDEBUG("");
UASSERT(memory &&
_predictionLC.size() >= 2 &&
ids.size());
UTimer timer;
timer.start();
UTimer timerGlobal;
timerGlobal.start();
std::map<int, int> idToIndexMap;
for(unsigned int i=0; i<ids.size(); ++i)
{
UASSERT_MSG(ids[i] != 0, "Signature id is null ?!?");
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
}
//int rows = prediction.rows;
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
// Each prior is a column vector
UDEBUG("_predictionLC.size()=%d",_predictionLC.size());
std::set<int> idsDone;
for(unsigned int i=0; i<ids.size(); ++i)
{
if(idsDone.find(ids[i]) == idsDone.end())
{
if(ids[i] > 0)
{
// Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0);
std::list<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
{
if(memory->isInSTM(iter->first))
{
neighbors.erase(iter++);
}
else
{
if(iter->second == 0)
{
idsLoopMargin.push_back(iter->second);
}
++iter;
}
}
// should at least have 1 id in idsMarginLoop
if(idsLoopMargin.size() == 0)
{
UFATAL("No 0 margin neighbor for signature %d !?!?", ids[i]);
}
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
float sum = 0.0f; // sum values added
sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
idsDone.insert(*iter);
this->normalize(prediction, i, sum, ids[0]<0);
}
}
else
{
// Set the virtual place prior
if(_virtualPlacePrior > 0)
{
if(cols>1) // The first must be the virtual place
{
((float*)prediction.data)[i] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(cols-1);
for(int j=1; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
else
{
// Only for some tests...
// when _virtualPlacePrior=0, set all priors to the same value
if(cols>1)
{
float val = 1.0/cols;
for(int j=0; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
}
}
}
ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
return prediction;
}
/*inline*/ void KalmanFilterBase::updatePredictedMeasurement()
{
updatePrediction();
predictedMeasurement_=simulateSensor_(oc_.xbar,this->x_.getTime()+1);
}
static void trainSetSpeed(const int speed, const int stopTime, const int delayer, Driver* me) {
char msg[4];
msg[1] = (char)me->trainNum;
if (me->lastSensorActualTime > 0) {
// a/d related stuff
int newSpeed = speed >=0 ? speed : 0;
int now = Time(me->timeserver) * 10;
if (me->speed == newSpeed) {
// do nothing
}
else if (me->speed == 0) {
// accelerating from 0
int v0 = getVelocity(me);
int v1 = me->v[newSpeed][ACCELERATE];
int t0 = now + 8; // compensate for time it takes to send to train
int t1 = now + 8 + me->a[newSpeed];
poly_init(&me->adPoly, t0, t1, v0, v1);
me->isAding = 1;
me->lastReportDist = 0;
me->adEndTime = t1;
} else if (newSpeed == 0) {
// decelerating to 0
int v0 = getVelocity(me);
int v1 = me->v[newSpeed][DECELERATE];
int t0 = now + 8; // compensate for time it takes to send to train
int t1 = now + 8 + getStoppingTime(me);
poly_init(&me->adPoly, t0, t1, v0, v1);
me->isAding = 1;
me->lastReportDist = 0;
me->adEndTime = t1;
}
}
TrainDebug(me, "Train Setting Speed %d", speed);
if (speed >= 0) {
if (delayer) {
TrainDebug(me, "Reversing speed.------- %d", speed);
msg[0] = 0xf;
msg[1] = (char)me->trainNum;
msg[2] = (char)speed;
msg[3] = (char)me->trainNum;
Putstr(me->com1, msg, 4);
//TrainDebug(me, "Next Sensor: %d %d", me->nextSensorIsTerminal, me->lastSensorIsTerminal);
// Update prediction
if (me->nextSensorIsTerminal) {
me->nextSensorBox = me->nextSensorBox == EX ? EN : EX;
//TrainDebug(me, "LAst Sensor: %d ", me->lastSensorVal);
} else {
int action = me->nextSensorVal%2 == 1 ? 1 : -1;
me->nextSensorVal = me->nextSensorVal + action;
}
if (me->lastSensorIsTerminal) {
me->lastSensorBox = me->lastSensorBox == EX ? EN : EX;
} else {
int action = me->lastSensorVal%2 == 1 ? 1 : -1;
me->lastSensorVal = me->lastSensorVal + action;
}
float distTemp = me->distanceFromLastSensor;
me->distanceFromLastSensor = me->distanceToNextSensor;
me->distanceToNextSensor = distTemp;
char valTemp = me->nextSensorVal;
me->nextSensorVal = me->lastSensorVal;
me->lastSensorVal = valTemp;
char boxTemp = me->nextSensorBox;
me->nextSensorBox = me->lastSensorBox;
me->lastSensorBox = boxTemp;
if (me->nextSensorIsTerminal || me->lastSensorIsTerminal){
char isTemp = me->nextSensorIsTerminal;
me->nextSensorIsTerminal = me->lastSensorIsTerminal;
me->lastSensorIsTerminal = isTemp;
}
// Reserve the track above train and future (covers case of init)
// Update prediction
updatePrediction(me);
int reserveStatus = reserveMoreTrack(me, 0, me->d[speed][ACCELERATE][MAX_VAL]); // moving
if (reserveStatus == RESERVE_FAIL) {
reroute(me);
}
} else {
//TrainDebug(me, "Set speed. %d %d", speed, me->trainNum);
msg[0] = (char)speed;
Putstr(me->com1, msg, 2);
if (speed == 0) {
int delayTime = stopTime + 500;
Reply(me->stopDelayer, (char*)&delayTime, 4);
}
}
if (speed > me->speed) {
me->speedDir = ACCELERATE;
} else if (speed < me->speed) {
me->speedDir = DECELERATE;
}
me->speed = speed;
} else {
//TrainDebug(me, "Reverse... %d ", me->speed);
DriverMsg delayMsg;
delayMsg.type = SET_SPEED;
delayMsg.timestamp = stopTime + 500;
if (me->speedAfterReverse == -1) {
delayMsg.data2 = (signed char)me->speed;
} else {
delayMsg.data2 = (signed char)me->speedAfterReverse;
}
//TrainDebug(me, "Using delayer: %d for %d", me->delayer, stopTime);
Reply(me->delayer, (char*)&delayMsg, sizeof(DriverMsg));
msg[0] = 0;
msg[1] = (char)me->trainNum;
Putstr(me->com1, msg, 2);
me->speed = 0;
me->speedDir = DECELERATE;
}
}
void driver() {
Driver me;
initDriver(&me, 1);
unsigned int naggCount = 0;
unsigned int updateStoppingDistanceCount = 0;
for (;;) {
int tid = -1;
DriverMsg msg;
msg.data2 = -1;
msg.data3 = -1;
msg.replyTid = -1;
Receive(&tid, (char*)&msg, sizeof(DriverMsg));
if (tid != me.delayer && tid != me.stopDelayer) {
Reply(tid, (char*)1, 0);
}
const int replyTid = msg.replyTid;
switch (msg.type) {
case SET_SPEED: {
//TrainDebug(&me, "Set speed from msg");
trainSetSpeed(msg.data2,
getStoppingTime(&me),
(msg.data3 == DELAYER),
&me);
if (msg.data3 != DELAYER) {
//TrainDebug(&me, "Replied to %d", replyTid);
Reply(replyTid, (char*)1, 0);
sendUiReport(&me);
break;
} else if (me.route.length != 0) {
// Delayer came back. Reverse command completed
me.stopCommited = 0; // We're moving again.
// We've completed everything up to the reverse node.
me.routeRemaining = me.stopNode+1;
me.previousStopNode = me.routeRemaining;
me.distanceFromLastSensorAtPreviousStopNode = me.distanceFromLastSensor;
// Calculate the next stop node.
updateStopNode(&me);
me.nextSetSwitchNode = -1;
updateSetSwitch(&me);
// if the reverse is last node, nothing to do
// if it isn't.. it should speed up again.
}
}
case DELAYER: {
//TrainDebug(&me, "delayer come back.");
break;
}
case STOP_DELAYER: {
// To prevent the first receive from this delayer
if (me.lastSensorActualTime > 0 && me.speed == 0 && !me.isAding) {
TrainDebug(&me, "releasing reserveration");
int reserveStatus = reserveMoreTrack(&me, 1, 0);
if (reserveStatus == RESERVE_FAIL) {
TrainDebug(&me, "WARNING: unable to reserve during init");
}
}
break;
}
case SENSOR_TRIGGER: {
// only handle sensor reports in primary + secondary prediction if not position finding
int sensorReportValid = 0;
TrackLandmark conditionLandmark;
int condition;
int isSensorReserved = QueryIsSensorReserved(&me, msg.data2, msg.data3);
if (me.positionFinding) {
sensorReportValid = 1;
me.lastSensorUnexpected = 1;
//FinishPositionFinding(me.trainNum, me.trainController);
} else if (isSensorReserved) {
//TrainDebug(&me, "Predictions.");
for (int i = 0; i < me.numPredictions; i ++) {
TrackLandmark predictedSensor = me.predictions[i].sensor;
//printLandmark(&me, &predictedSensor);
if (predictedSensor.type == LANDMARK_SENSOR && predictedSensor.num1 == msg.data2 && predictedSensor.num2 == msg.data3) {
sensorReportValid = 1;
if (i != 0) {
TrainDebug(&me, "Trigger Secondary");
// secondary prediction, need to do something about them
conditionLandmark = me.predictions[i].conditionLandmark;
condition = me.predictions[i].condition;
me.lastSensorUnexpected = 1;
if (conditionLandmark.type == LANDMARK_SWITCH) {
TrackMsg setSwitch;
setSwitch.type = UPDATE_SWITCH_STATE;
TrainDebug(&me, "UPDATE SWITCH STATE");
setSwitch.landmark1 = conditionLandmark;
setSwitch.data = condition;
Send(me.trackManager, (char*)&setSwitch, sizeof(TrackMsg), (char *)1, 0);
}
// Stop and then try to reroute.
reroute(&me);
} else {
me.lastSensorUnexpected = 0;
}
}
}
}
if (sensorReportValid) {
updateRoute(&me, msg.data2, msg.data3);
me.lastSensorBox = msg.data2; // Box
me.lastSensorVal = msg.data3; // Val
me.lastSensorIsTerminal = 0;
me.lastSensorActualTime = msg.timestamp;
dynamicCalibration(&me);
me.lastSensorPredictedTime = me.nextSensorPredictedTime;
TrackNextSensorMsg trackMsg;
QueryNextSensor(&me, &trackMsg);
// Reserve the track above train and future (covers case of init)
for (int i = 0; i < trackMsg.numPred; i++) {
me.predictions[i] = trackMsg.predictions[i];
}
me.numPredictions = trackMsg.numPred;
int reserveStatus = reserveMoreTrack(&me, me.positionFinding, getStoppingDistance(&me));
if (reserveStatus == RESERVE_FAIL) {
if (!me.positionFinding) {
reroute(&me);
} else {
TrainDebug(&me, "WARNING: unable to reserve during init");
}
}
TrackSensorPrediction primaryPrediction = me.predictions[0];
me.calibrationStart = msg.timestamp;
me.calibrationDistance = primaryPrediction.dist;
int dPos = 50 * getVelocity(&me) / 100000.0;
me.lastSensorDistanceError = -(int)me.distanceToNextSensor - dPos;
me.distanceFromLastSensor = dPos;
me.distanceToNextSensor = primaryPrediction.dist - dPos;
me.lastPosUpdateTime = msg.timestamp;
if (primaryPrediction.sensor.type != LANDMARK_SENSOR &&
primaryPrediction.sensor.type != LANDMARK_END) {
TrainDebug(&me, "QUERY_NEXT_SENSOR_FROM_SENSOR ..bad");
}
me.nextSensorIsTerminal = (primaryPrediction.sensor.type == LANDMARK_END);
me.nextSensorBox = primaryPrediction.sensor.num1;
me.nextSensorVal = primaryPrediction.sensor.num2;
me.nextSensorPredictedTime =
msg.timestamp + me.distanceToNextSensor*100000 /
getVelocity(&me);
updatePosition(&me, msg.timestamp);
sendUiReport(&me);
if (me.positionFinding) {
trainSetSpeed(0, getStoppingTime(&me), 0, &me); // Found position, stop.
me.positionFinding = 0;
me.currentlyLost = 0;
}
}
break;
}
case NAVIGATE_NAGGER: {
updatePosition(&me, msg.timestamp);
if (me.routeRemaining != -1) {
if (!me.stopCommited) {
if (shouldStopNow(&me)) {
if (me.route.nodes[me.stopNode].num == REVERSE) {
//TrainDebug(&me, "Navi reversing.");
const int speed = -1;
trainSetSpeed(speed, getStoppingTime(&me), 0, &me);
}
else {
//TrainDebug(&me, "Navi Nagger stopping.");
const int speed = 0; // Set speed zero.
trainSetSpeed(speed, getStoppingTime(&me), 0, &me);
me.route.length = 0; // Finished the route.
me.testMode = 0;
}
me.stopCommited = 1;
me.useLastSensorNow = 0;
me.stopNow = 0;
me.stopSensorHit = 0;
} else {
if ((++updateStoppingDistanceCount & 15) == 0) updateStopNode(&me);
}
}
}
if (me.nextSetSwitchNode != -1 && (++me.setSwitchNaggerCount & 3) == 0) {
trySetSwitch_and_getNextSwitch(&me);
}
if (me.rerouteCountdown-- == 0) {
if (me.testMode) {
int reserveStatus = reserveMoreTrack(&me, 0, me.d[8][ACCELERATE][MAX_VAL]); // moving
if (reserveStatus == RESERVE_FAIL) {
reroute(&me);
} else {
me.nextSetSwitchNode = -1;
updateSetSwitch(&me);
trainSetSpeed(8, 0, 0, &me);
}
} else {
// reroute
if (me.route.length != 0) {
setRoute(&me, &(me.routeMsg));
}
}
}
if ((++naggCount & 15) == 0) sendUiReport(&me);
break;
}
case SET_ROUTE: {
Reply(replyTid, (char*)1, 0);
me.routeMsg = msg;
setRoute(&me, &msg);
break;
}
case BROADCAST_UPDATE_PREDICTION: {
updatePrediction(&me);
int reserveStatus = reserveMoreTrack(&me, 0, getStoppingDistance(&me)); // moving
if (reserveStatus == RESERVE_FAIL) {
reroute(&me);
}
break;
}
case BROADCAST_TEST_MODE: {
me.testMode = 1;
setRoute(&me, &msg);
break;
}
case FIND_POSITION: {
me.positionFinding = 1;
trainSetSpeed(5, 0, 0, &me);
break;
}
default: {
TrainDebug(&me, "Not suppported train message type.");
}
}
}
}
