Diffs Node::getDiffs (const RobotState& last, const RobotState& current)
{
Diffs diffs;
const ros::Time now = ros::Time::now();;
// Use the fact that JointState is empty iff this is the first time around
if (last.isEmpty() || now >= last_keyframe_+keyframe_interval_)
{
last_keyframe_ = now;
if (last.isEmpty())
cerr << "Last is empty, so using all diffs" << endl;
else
cerr << "Keyframe interval elapsed, so saving entire joint state" << endl;
diffs.is_keyframe = true;
for (size_t i=0; i<current.joint_state->position.size(); i++)
{
diffs.new_joint_states.push_back(Diff(i, current.joint_state->position[i]));
}
diffs.pose_diff = true;
diffs.new_pose = current.pose;
}
else
{
for (size_t i=0; i<current.joint_state->position.size(); i++)
{
if (fabs(current.joint_state->position[i]-
last.joint_state->position[i])>distance_threshold_ &&
!joint_ignored_[i])
{
cerr << "Joint " << current.joint_state->name[i] << " value " <<
current.joint_state->position[i] << " differs from " <<
last.joint_state->position[i] << endl;
diffs.new_joint_states.push_back(Diff(i, current.joint_state->position[i]));
}
}
if ((current.pose.get() && !last.pose.get()) ||
(!current.pose.get() && last.pose.get()) ||
((current.pose.get() && last.pose.get() &&
poseDistance(*current.pose, *last.pose)>distance_threshold_)))
{
cerr << "Current pose: " << (bool)current.pose.get() << "; last pose: "
<< (bool)last.pose.get() << endl;
diffs.pose_diff = true;
diffs.new_pose = current.pose;
}
}
return diffs;
}
void ICLSLAM::handleScan(double timestamp, const PointScan &pscan)
{
doBackup(timestamp);
if(doICL(timestamp)) {
SegmentScan sm(pscan);
bool skip = false;
if(outdoor) {
Rototranslation rt(currentPose);
freeContent();
fforeach(const LineSegment &s, sm.getSegments()) {
segments.append(new LineSegment(rt * s));
}
fforeach(const Frontier &f, sm.getFrontiers()) {
if(f.length() > 0.2) frontiers.append(new Frontier(rt * f));
}
if(takeAMeasure(timestamp)) {
poses.append(TimedPose(timestamp, currentPose));
lastMeasurePose = lastPose;
lastMeasureTimestamp = timestamp;
}
return;
}
ICLImpl icl(sm, segments, currentPose);
icl.run();
Eigen::Vector3d z = icl.measure();
#ifndef SLAM_SKIP_DEBUG
ldbg << "ins: " << ins.getPose() << endl;
ldbg << "currentPose: " << currentPose << endl;
ldbg << "z: " << z << endl;
#endif
Eigen::Vector3d diff = currentPose.vectorForm() - z;
if(std::abs(wrap(diff[2])) >= M_PI / 12 || SQUARE(diff[0]) + SQUARE(diff[1]) >= 3) {
for(int i = 0; i < 10; i++) {
Rototranslation perturbation(
Random::normal(0, 1 / 3.),
Random::normal(0, 1 / 3.),
Random::normal(0, SQUARE(M_PI / 6) / 3.));
ICLImpl icl(sm, segments, perturbation * currentPose);
icl.run();
const Eigen::Vector3d &z1 = icl.measure();
if(poseDistance(z1, currentPose) < poseDistance(z, currentPose)) {
#ifndef SLAM_SKIP_DEBUG
ldbg << "Cambiato" << endl;
#endif
z = z1;
}
}
}
diff = currentPose.vectorForm() - z;
if(std::abs(wrap(diff[2])) >= M_PI / 12 || SQUARE(diff[0]) + SQUARE(diff[1]) >= 3) {
z = currentPose;
skipCount++;
skip = true;
}
if(skip && skipCount <= MAX_SKIP_COUNT) {
return;
} else {
skipCount = 0;
}
#ifndef SLAM_SKIP_DEBUG
ldbg << "ins: " << ins.getPose() << endl;
ldbg << "currentPose: " << currentPose << endl;
ldbg << "z: " << z << endl;
#endif
Pose guess = currentPose;
currentPose = z;
if(!almostEqual(ins.getPose().theta(), 0, 0.02) ||
!almostEqual(ins.getPose().phi(), 0, 0.02)) {
return;
}
SegmentScan rotoscan = Rototranslation(z) * sm;
#ifndef SLAM_SKIP_DEBUG
ldbg << endl << "counter=" << counter++;
ldbg << endl << "guess=" << guess << endl;
#endif
//printMap(rotoscan.getSegments(), "scan");
//printMap(segments, "walls");
//printMap(frontiers, "frontiers");
//ldbg << "plot(xwalls, ywalls, 'b', xfrontiers, yfrontiers, 'g', xscan, yscan, 'r');" << endl;
//printMapCPP(sm.getSegments(), "scan");
//printMapCPP(segments, "segments");
//ldbg << "Graphics[" << rotoscan.getSegments() << ",Dashed," << rotoscan.getFrontiers() << "]" << endl;
//ldbg << "Graphics[{" << segments << ",Dashed," << frontiers << ",Opacity[0.1],"
// << rotoscan.toPolygon() << "}]" << endl;
mergeSegments(rotoscan);
if(takeAMeasure(timestamp)) {
mergeFrontiers(rotoscan);
poses.append(TimedPose(timestamp, currentPose));
lastMeasurePose = lastPose;
lastMeasureTimestamp = timestamp;
}
if(timestamp - lastThinningTimestamp >= SLAM_MAP_THINNING_INTERVAL) {
#ifndef SLAM_SKIP_DEBUG
ldbg << "Map Thinning" << endl;
#endif
mapThinning();
lastThinningTimestamp = timestamp;
}
#ifndef SLAM_SKIP_DEBUG
//ldbg << endl << "ListPlot[" << pscan << "]" << endl;
ldbg << endl << "Graphics[" << sm.getSegments() << "]";
ldbg << endl << "Graphics[{" << segments << ",Dashed," << frontiers << ",Opacity[0.1],"
<< rotoscan.toPolygon() << "}]" << endl;
#endif
/*printMap(rotoscan.getSegments(), "scan");
printMap(segments, "walls");
printMap(frontiers, "frontiers");
ldbg << "plot(xwalls, ywalls, 'b', xfrontiers, yfrontiers, 'g', xscan, yscan, 'r');" << endl;
*/
//if(counter == 1508)
// exit(0);
Pose p = Rototranslation(initialPose) * currentPose;
emit newRobotPose(TimedPose(lastPose.timestamp(), p));
}
}
