string OriCommand::cmd_merge(strstream &str)
{
#if defined(DEBUG) || defined(ORI_PERF)
Stopwatch sw = Stopwatch();
sw.start();
#endif /* DEBUG */
FUSE_PLOG("Command: merge");
string error;
ObjectHash hash;
strwstream resp;
// Parse Command
str.readHash(hash);
RWKey::sp lock = priv->nsLock.writeLock();
error = priv->merge(hash);
lock.reset();
if (error != "") {
resp.writeUInt8(0);
resp.writePStr(error);
} else {
resp.writeUInt8(1);
}
#if defined(DEBUG) || defined(ORI_PERF)
sw.stop();
FUSE_PLOG("merge with: %s", hash.hex().c_str());
FUSE_PLOG("merge elapsed %lluus", sw.getElapsedTime());
#endif /* DEBUG */
return resp.str();
}
string OriCommand::cmd_snapshot(strstream &str)
{
#if defined(DEBUG) || defined(ORI_PERF)
Stopwatch sw = Stopwatch();
sw.start();
#endif /* DEBUG */
FUSE_PLOG("Command: snapshot");
uint8_t hasMsg, hasName;
string msg, name;
Commit c;
strwstream resp;
// Parse Command
hasMsg = str.readUInt8();
hasName = str.readUInt8();
if (hasMsg) {
str.readLPStr(msg);
c.setMessage(msg);
}
if (hasName) {
str.readLPStr(name);
c.setSnapshot(name);
}
RWKey::sp lock = priv->nsLock.writeLock();
ObjectHash hash = priv->commit(c);
lock.reset();
if (hash.isEmpty()) {
resp.writeUInt8(0);
} else {
resp.writeUInt8(1);
resp.writeHash(hash);
}
#if defined(DEBUG) || defined(ORI_PERF)
sw.stop();
if (hash.isEmpty())
FUSE_PLOG("snapshot not taken");
FUSE_PLOG("snapshot result: %s", hash.hex().c_str());
FUSE_PLOG("snapshot elapsed %lluus", sw.getElapsedTime());
#endif /* DEBUG */
return resp.str();
}
string OriCommand::cmd_status(strstream &str)
{
#if defined(DEBUG) || defined(ORI_PERF)
Stopwatch sw = Stopwatch();
sw.start();
#endif /* DEBUG */
FUSE_PLOG("Command: status");
map<string, OriFileState::StateType> diff;
map<string, OriFileState::StateType>::iterator it;
strwstream resp;
RWKey::sp lock = priv->nsLock.writeLock();
diff = priv->getDiff();
lock.reset();
resp.writeUInt32(diff.size());
for (it = diff.begin(); it != diff.end(); it++) {
char type = '!';
if (it->second == OriFileState::Created)
type = 'A';
else if (it->second == OriFileState::Modified)
type = 'M';
else if (it->second == OriFileState::Deleted)
type = 'D';
else
ASSERT(false);
resp.writeUInt8(type);
resp.writeLPStr(it->first);
}
#if defined(DEBUG) || defined(ORI_PERF)
sw.stop();
FUSE_PLOG("status elapsed %lluus", sw.getElapsedTime());
#endif /* DEBUG */
return resp.str();
}
void DouglasPeuckerApproximation::approximate(const std::vector <FrameWithId>& in,
std::vector <FrameWithId>& out, double epsilon) {
#ifdef DEBUG
Stopwatch stopwatch;
stopwatch.start();
#endif
std::vector <FrameWithId> inputCopy;
inputCopy = in;
out.clear();
int counter = 0;
douglasPeucker(inputCopy, out, counter, epsilon);
#ifdef DEBUG_0
stopwatch.stop();
LOG("Douglas-Peucker approximation algorithm:");
LOG(" - input size: %u", inputCopy.size());
LOG(" - epsilon: %f", epsilon);
LOG(" - output size: %u", out.size());
LOG(" - iterations : %d", counter);
LOG(" - duration : %f ms", stopwatch.getElapsedTime());
#endif
}
void ChaikinCurveApproximation::approximate(const std::vector <FrameWithId>& in, std::vector <FrameWithId>& out, unsigned int lod) {
#ifdef DEBUG
Stopwatch stopwatch;
stopwatch.start();
#endif
std::vector <FrameWithId> inputCopy;
inputCopy = in;
unsigned int counter = 0;
for (unsigned int i = 0; i < lod; ++i) {
out.clear();
counter += chaikinCurve(inputCopy, out);
inputCopy = out;
}
#ifdef DEBUG
stopwatch.stop();
LOG("Chaikin curve approximation algorithm:");
LOG(" - input size: %lu", in.size());
LOG(" - level of detail: %d", lod);
LOG(" - output size: %lu", out.size());
LOG(" - iterations : %d", counter);
LOG(" - duration : %f ms", stopwatch.getElapsedTime());
#endif
}
string OriCommand::cmd_pull(strstream &str)
{
#if defined(DEBUG) || defined(ORI_PERF)
Stopwatch sw = Stopwatch();
sw.start();
#endif /* DEBUG */
FUSE_PLOG("Command: pull");
bool success;
string srcPath;
string error;
ObjectHash hash;
RemoteRepo srcRepo = RemoteRepo();
strwstream resp;
// Parse Command
str.readPStr(srcPath);
if (srcPath == "") {
map<string, Peer> peers = priv->getRepo()->getPeers();
map<string, Peer>::iterator it = peers.find("origin");
if (it == peers.end()) {
error = "No default repository to pull from.";
goto error;
}
srcPath = (*it).second.getUrl();
}
try {
success = srcRepo.connect(srcPath);
} catch (exception &e) {
error = e.what();
goto error;
}
if (success) {
hash = srcRepo->getHead();
// XXX: Change to a repo lock
RWKey::sp lock = priv->nsLock.writeLock();
priv->getRepo()->pull(srcRepo.get());
// XXX: Refcounts need to be done incrementally or rebuilt after
lock.reset();
} else {
error = "Connection failed!";
}
error:
if (hash.isEmpty()) {
resp.writeUInt8(0);
resp.writePStr(error);
} else {
resp.writeUInt8(1);
resp.writeHash(hash);
}
#if defined(DEBUG) || defined(ORI_PERF)
sw.stop();
FUSE_PLOG("pull up to: %s", hash.hex().c_str());
FUSE_PLOG("pull elapsed %lluus", sw.getElapsedTime());
#endif /* DEBUG */
return resp.str();
}
bool CollisionCheckingRos::collisionCheck(const std::vector <FrameWithId>& path,
const FrameWithId& actualPose, double interpolationStep, unsigned int numberOfSteps) {
bool collision = false;
if (path.empty())
return collision;
costmap_2d::Costmap2D costmapCopy;
std::vector <geometry_msgs::Point> orientedFootprint;
costmap->getOrientedFootprint(orientedFootprint);
costmap->clearRobotFootprint();
costmap->getCostmapCopy(costmapCopy);
base_local_planner::CostmapModel collisionChecker(costmapCopy);
std::vector <FrameWithId> prunedPath;
utilities::prunePath(path, actualPose, prunedPath);
LinearInterpolation interpolator;
std::vector <FrameWithId> interpolatedPath;
size_t counter = interpolator.interpolate(prunedPath, interpolatedPath, interpolationStep, numberOfSteps);
#ifdef DEBUG
Stopwatch stopwatch;
stopwatch.start();
#endif
size_t i;
for (i = 0; i < counter; ++i) {
const KDL::Frame& frame = interpolatedPath[i].getFrame();
double r, p, y;
frame.M.GetRPY(r, p, y);
geometry_msgs::PoseStamped pose;
conversions::frameToPoseStampedRos(frame, pose);
std::vector <geometry_msgs::Point> orientedFootprint;
costmap->getOrientedFootprint(frame.p.x(), frame.p.y(), 0, orientedFootprint);
double circumscribedRadius = costmap->getCircumscribedRadius();
double inscribedRadius = costmap->getInscribedRadius();
double c = collisionChecker.footprintCost(pose.pose.position,
orientedFootprint,
inscribedRadius,
circumscribedRadius);
if (c < 0) {
ROS_INFO("Collision at pose: %f, %f", pose.pose.position.x, pose.pose.position.y);
collision = true;
break;
}
}
#ifdef DEBUG_0
stopwatch.stop();
LOG("Collision checking using costmap2d_ros:");
LOG(" - collision: %d", collision);
LOG(" - step size: %f", interpolationStep);
LOG(" - iterations : %u", i);
LOG(" - duration : %f ms", stopwatch.getElapsedTime());
#endif
return collision;
}
