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; }