ErrorCode SenderThread::readHeartBeats() {
if (!enableHeartBeat_) {
return OK;
}
const auto now = Clock::now();
const int timeSinceLastHeartBeatMs = durationMillis(now - lastHeartBeatTime_);
const int heartBeatIntervalMs =
(options_.read_timeout_millis * kHeartBeatReadTimeFactor);
if (timeSinceLastHeartBeatMs <= heartBeatIntervalMs) {
return OK;
}
lastHeartBeatTime_ = now;
// time to read heart-beats
const int numRead = socket_->read(buf_, bufSize_,
/* don't try to read all the data */ false);
if (numRead <= 0) {
WTLOG(ERROR) << "Failed to read heart-beat " << numRead;
return SOCKET_READ_ERROR;
}
for (int i = 0; i < numRead; i++) {
const char receivedCmd = buf_[i];
if (receivedCmd != Protocol::HEART_BEAT_CMD) {
WTLOG(ERROR) << "Received " << receivedCmd
<< " instead of heart-beat cmd";
return PROTOCOL_ERROR;
}
}
if (!isTty_) {
WTLOG(INFO) << "Received " << numRead << " heart-beats";
}
return OK;
}
void detection::GraspPointDetector::detect(const CloudPtr &input_object)
{
clock_t beginCallback = clock();
// If no cloud or no new images since last time, do nothing.
if (input_object->size() == 0) return;
world_obj_ = input_object->makeShared();
pub_cluster_.publish(world_obj_);
// Check if computation succeeded
if (doProcessing())
ROS_INFO("[%g ms] Detection Success", durationMillis(beginCallback));
else
ROS_ERROR("[%g ms] Detection Failed", durationMillis(beginCallback));
}
ErrorCode SenderThread::readNextReceiverCmd() {
int numUnackedBytes = socket_->getUnackedBytes();
int timeToClearSendBuffer = 0;
Clock::time_point startTime = Clock::now();
while (true) {
int numRead = socket_->read(buf_, 1);
if (numRead == 1) {
return OK;
}
if (getThreadAbortCode() != OK) {
return ABORT;
}
if (numRead == 0) {
WTPLOG(ERROR) << "Got unexpected EOF, reconnecting";
return SOCKET_READ_ERROR;
}
WDT_CHECK_LT(numRead, 0);
ErrorCode errCode = socket_->getReadErrCode();
WTLOG(ERROR) << "Failed to read receiver cmd " << numRead << " "
<< errorCodeToStr(errCode);
if (errCode != WDT_TIMEOUT) {
// not timed out
return SOCKET_READ_ERROR;
}
int curUnackedBytes = socket_->getUnackedBytes();
if (numUnackedBytes < 0 || curUnackedBytes < 0) {
WTLOG(ERROR) << "Failed to read number of unacked bytes, reconnecting";
return SOCKET_READ_ERROR;
}
WDT_CHECK_GE(numUnackedBytes, curUnackedBytes);
if (curUnackedBytes == 0) {
timeToClearSendBuffer = durationMillis(Clock::now() - startTime);
break;
}
if (curUnackedBytes == numUnackedBytes) {
WTLOG(ERROR) << "Number of unacked bytes did not change, reconnecting "
<< curUnackedBytes;
return SOCKET_READ_ERROR;
}
WTLOG(INFO) << "Read receiver command failed, but number of unacked "
"bytes decreased, retrying socket read "
<< numUnackedBytes << " " << curUnackedBytes;
numUnackedBytes = curUnackedBytes;
}
// we are assuming that sender and receiver tcp buffer sizes are same. So, we
// expect another timeToClearSendBuffer milliseconds for receiver to clear its
// buffer
int readTimeout = timeToClearSendBuffer + options_.drain_extra_ms;
WTLOG(INFO) << "Send buffer cleared in " << timeToClearSendBuffer
<< "ms, waiting for " << readTimeout
<< "ms for receiver buffer to clear";
// readWithTimeout internally checks for abort periodically
int numRead = socket_->readWithTimeout(buf_, 1, readTimeout);
if (numRead != 1) {
WTLOG(ERROR) << "Failed to read receiver cmd " << numRead;
return SOCKET_READ_ERROR;
}
return OK;
}
int64_t WdtSocket::ioWithAbortCheck(F readOrWrite, T tbuf, int64_t numBytes,
int timeoutMs, bool tryFull) {
WDT_CHECK(threadCtx_.getAbortChecker() != nullptr)
<< "abort checker can not be null";
bool checkAbort = (threadCtx_.getOptions().abort_check_interval_millis > 0);
auto startTime = Clock::now();
int64_t doneBytes = 0;
int retries = 0;
while (doneBytes < numBytes) {
const int64_t ret =
readOrWrite(fd_, tbuf + doneBytes, numBytes - doneBytes);
if (ret < 0) {
// error
if (errno != EINTR && errno != EAGAIN) {
PLOG(ERROR) << "non-retryable error encountered during socket io "
<< fd_ << " " << doneBytes << " " << retries;
return (doneBytes > 0 ? doneBytes : ret);
}
} else if (ret == 0) {
// eof
WVLOG(1) << "EOF received during socket io. fd : " << fd_
<< ", finished bytes : " << doneBytes
<< ", retries : " << retries;
return doneBytes;
} else {
// success
doneBytes += ret;
if (!tryFull) {
// do not have to read/write entire data
return doneBytes;
}
}
if (checkAbort && threadCtx_.getAbortChecker()->shouldAbort()) {
WLOG(ERROR) << "transfer aborted during socket io " << fd_ << " "
<< doneBytes << " " << retries;
return (doneBytes > 0 ? doneBytes : -1);
}
if (timeoutMs > 0) {
int duration = durationMillis(Clock::now() - startTime);
if (duration >= timeoutMs) {
WLOG(INFO) << "socket io timed out after " << duration
<< " ms, retries " << retries << " fd " << fd_
<< " doneBytes " << doneBytes;
return (doneBytes > 0 ? doneBytes : -1);
}
}
retries++;
}
WVLOG_IF(1, retries > 1) << "socket io for " << doneBytes << " bytes took "
<< retries << " retries";
return doneBytes;
}
bool detection::GraspPointDetector::doProcessing()
{
ROS_INFO_ONCE("'Do Processing' called!");
clock_t begin = clock();
geometry_msgs::PoseStamped initial_pose = r_arm_group_.getCurrentPose(r_arm_group_.getEndEffectorLink());
/** COMPUTE BOUNDINGBOX AND PUBLISH A TF FRAME **/
boost::mutex::scoped_lock bounding_box_lock(update_bounding_box_mutex_);
obj_bounding_box_->computeAndPublish(world_obj_, table_plane_, tf_broadcaster);
planar_obj_ = obj_bounding_box_->getPlanarObj();
draw_bounding_box_ = true;
/** SAMPLING GRASP POSITIONS **/
sampler.configure(obj_bounding_box_);
// Find all the samples positions
// We call grasp filter to make a pre-filtering step, to avoid generate clearly unfeasible grasps samples
if(!sampler.generateSamples(obj_bounding_box_, &GraspFilter::generateSideSamples, &GraspFilter::generateTopSamples))
{
bounding_box_lock.unlock();
return false;
}
draw_sampled_grasps_ = true;
bounding_box_lock.unlock();
pub_samples_.publish(sampler.getSamples());
ROS_INFO("[%g ms] Grasping sampling", durationMillis(begin));
/** FILTERING UNFEASIBLE GRASPS **/
// We need the table bounding box to create a collision object in moveit
table_bounding_box_->buildPlanar(table_cloud_);
try
{
begin = clock();
// Configure filter
grasp_filter_->configure(sampler.getSideGraspHeight(), sampler.getTopGraspHeight(), obj_bounding_box_,
table_bounding_box_);
// Remove infeasible ones
sampler.getTopGrasps()->clear(); //TODO: REMOVE THIS!!!
grasp_filter_->filterGraspingPoses(sampler.getSideGrasps(), sampler.getTopGrasps());
ROS_INFO("[%g ms] Grasping filtering", durationMillis(begin));
}
catch (ComputeFailedException ex)
{
ROS_WARN("%s", ex.what());
return false;
}
if (!grasp_filter_->feasibleGrasps()) return false;
/** GRASP RANKING **/
begin = clock();
grasp_ranker_.configure(obj_bounding_box_, grasp_filter_->getEndEffectorLink(), grasp_filter_->getShoulderLink());
grasp_ranker_.rankGraspingPoses(grasp_filter_->getSideGrasps(), grasp_filter_->getTopGrasps());
moveit_msgs::Grasp grasp_winner = grasp_ranker_.getSelectedGrasp();
ROS_INFO("[%g ms] Grasping ranking", durationMillis(begin));
// Show winner plan
pick(r_arm_group_, GRASPABLE_OBJECT(), grasp_winner, true);
if (selectChoice("Ready: 1=Pick, Any=Again") != 1) return false;
/** ACTUAL PICKING **/
begin = clock();
r_arm_group_.setPlanningTime(25.0);
r_arm_group_.setSupportSurfaceName(SUPPORT_TABLE());
bool result = pick(r_arm_group_, GRASPABLE_OBJECT(), grasp_winner, false);
ROS_INFO("[%g ms] Pick movement", durationMillis(begin));
r_arm_group_.setPoseTarget(initial_pose, r_arm_group_.getEndEffectorLink());
moveit::planning_interface::MoveGroup::Plan plan;
bool success = r_arm_group_.plan(plan);
if (success) r_arm_group_.move();
else ROS_INFO("Unable to return no initial position. No plan found");
return result;
}
