bool FrameQueue::handleFrame(Connection::Ptr pConnection, Frame::Ptr pFrame) { poco_assert_dbg (pConnection == _pConnection); if ((_frameType == 0 || pFrame->type() == _frameType) && pFrame->channel() == _channel) { { Poco::FastMutex::ScopedLock lock(_mutex); int rounds = 0; while (_queue.size() == MAX_QUEUE_SIZE && rounds < 100) { Poco::ScopedUnlock<Poco::FastMutex> unlock(_mutex); Poco::Thread::sleep(5); rounds++; } if (_queue.size() < MAX_QUEUE_SIZE) { _queue.push_back(pFrame); _sema.set(); return true; } } } return false; }
bool handleFrame(Connection::Ptr pConnection, Frame::Ptr pFrame) { if (pFrame->type() == Frame::FRAME_TYPE_EVSU || pFrame->type() == Frame::FRAME_TYPE_EVUN) { Poco::RemotingNG::ORB& orb = Poco::RemotingNG::ORB::instance(); std::string suri(pFrame->payloadBegin(), pFrame->getPayloadSize()); Poco::URI dispURI(suri); dispURI.setAuthority(_pListener->endPoint()); dispURI.setFragment(""); Poco::RemotingNG::EventDispatcher::Ptr pEventDispatcher = orb.findEventDispatcher(dispURI.toString(), Transport::PROTOCOL); if (pFrame->type() == Frame::FRAME_TYPE_EVSU) { Poco::Timestamp expire; expire += _pListener->getEventSubscriptionTimeout().totalMicroseconds(); pEventDispatcher->subscribe(suri, suri, expire); } else { pEventDispatcher->unsubscribe(suri); } pConnection->returnFrame(pFrame); return true; } return false; }
void DmxClient::playFrame(Frame::Ptr frame) { _debug && std::cout << ": " << frame->width() << " x " << frame->height() << std::endl; for ( uint16_t y = 0; y < frame->height(); y++ ) { for ( uint16_t x = 0; x < frame->width(); x++ ) { uint16_t addr = posMap(x, y); if ( addr >= 0 ) { fullcircle::RGB_t pixel = frame->get_pixel(x, y); _buffer.SetChannel( addr , (pixel.red % 256)*_dimm ); _buffer.SetChannel( addr + 1, (pixel.green % 256)*_dimm ); _buffer.SetChannel( addr + 2, (pixel.blue % 256)*_dimm ); } } } if (!_olaClient->SendDmx(_universe, _buffer)) { std::cerr << "Sending data to DMX failed!" << std::endl; exit(1); } }
int Dialog::run_modal(const Frame::Ptr & parent) { int result; if(!m_is_setup) { setup_widget(); } gtkDialog().set_transient_for(parent->gtkWindow()); gtkDialog().set_default_response(Gtk::RESPONSE_CLOSE); result = gtkDialog().run(); gtkDialog().hide(); return result; }
bool handleFrame(Connection::Ptr pConnection, Frame::Ptr pFrame) { if (pFrame->type() == Frame::FRAME_TYPE_REQU && (pFrame->flags() & Frame::FRAME_FLAG_CONT) == 0) { Poco::SharedPtr<ChannelInputStream> pRequestStream = new ChannelInputStream(pConnection, pFrame->type(), pFrame->channel(), _pListener->getTimeout()); Poco::SharedPtr<ChannelOutputStream> pReplyStream; if ((pFrame->flags() & Frame::FRAME_FLAG_ONEWAY) == 0) { Poco::UInt16 flags(0); if (pFrame->flags() & Frame::FRAME_FLAG_DEFLATE) flags |= Frame::FRAME_FLAG_DEFLATE; pReplyStream = new ChannelOutputStream(pConnection, Frame::FRAME_TYPE_REPL, pFrame->channel(), flags); } ServerTransport::Ptr pServerTransport = new ServerTransport(*_pListener, pRequestStream, pReplyStream, (pFrame->flags() & Frame::FRAME_FLAG_DEFLATE) != 0); _pListener->connectionManager().threadPool().start(*pServerTransport); Poco::Thread::yield(); pServerTransport->waitReady(); pRequestStream->rdbuf()->queue()->handleFrame(pConnection, pFrame); return true; } else return false; }
/// Display stuff void publishStuff(bool all = true){ if (pubMarker.getNumSubscribers()>0){ // Publish Landmarks pubMarker.publish(odometry.getMapMarkers()); pubMarker.publish(odometry.getMapObservations()); // Publish Track (ie all path and poses estimated) pubMarker.publish(odometry.getTrackLineMarker()); } if (pubTrack.getNumSubscribers()>0){ pubTrack.publish(odometry.getTrackPoseMarker()); } if (all){ // Publish TF for each KF ROS_INFO("NOD = Publishing TF for each KF"); const Frame::Ptrs& kfs = odometry.getKeyFrames(); //const Frame::Ptr kf = kfs[0]; for(uint i=0; i<kfs.size(); ++i){ pubTF.sendTransform(kfs[i]->getStampedTransform()); } } // Publish TF for current frame const Frame::Ptr f = odometry.getLastFrame(); if (f->poseEstimated()){ pubTF.sendTransform(f->getStampedTransform()); pubTF.sendTransform(f->getStampedTransform(true)); } }
void RSDPA10::run() { // ----------- PA10 Robot Main State Machine ----------- plc_status = PLCserial.read(s+500); if (plc_status == "t") { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_SAFETY_SYSTEM_TRIPPED); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_SAFETY_SYSTEM_BLINK); plc_status = ""; } if(get_rsd_info(INFO_TYPE_MES_COMMAND) & MES_COMMAND_WAIT) { set_rsd_info(INFO_TYPE_MES_COMMAND, INFO_ACTION_CLEAR, MES_COMMAND_WAIT); if(mes_state == FREE) rw::common::Log::log().info() << "In MES State: " << "FREE" << std::endl; else if (mes_state == OUT_OF_BRICKS) rw::common::Log::log().info() << "In MES State: " << "OUT_OF_BRICKS" << std::endl; } else if(get_rsd_info(INFO_TYPE_MES_COMMAND) & MES_COMMAND_SORTBRICKS) { set_rsd_info(INFO_TYPE_MES_COMMAND, INFO_ACTION_CLEAR, MES_COMMAND_SORTBRICKS); rw::common::Log::log().info() << "In MES State: " << "SORTING" << std::endl; mes_state = SORTING; } else if(get_rsd_info(INFO_TYPE_MES_COMMAND) & MES_COMMAND_LOADBRICKS) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_ORDER_COMPLETE); set_rsd_info(INFO_TYPE_MES_COMMAND, INFO_ACTION_CLEAR, MES_COMMAND_LOADBRICKS); rw::common::Log::log().info() << "In MES State: " << "LOADING" << std::endl; mes_state = LOADING; } else if(get_rsd_info(INFO_TYPE_MES_COMMAND) & MES_COMMAND_ABORT) { set_rsd_info(INFO_TYPE_MES_COMMAND, INFO_ACTION_CLEAR, MES_COMMAND_ABORT); rw::common::Log::log().info() << "In MES State: " << "FREE" << std::endl; mes_state = FREE; } if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_ORDER_COMPLETE) { rw::common::Log::log().info() << "In MES State: " << "ORDER_SORTED" << std::endl; mes_state = ORDER_SORTED; } else if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_OUT_OF_BRICKS) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_OUT_OF_BRICKS); rw::common::Log::log().info() << "In MES State: " << "OUT_OF_BRICKS" << std::endl; mes_state = OUT_OF_BRICKS; } if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_RESETJOINTS) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_RESETJOINTS); linearPath.clear(); rw::common::Log::log().info() << "In State: " << "RESET" << std::endl; mes_state = SORTING; control_state = RESET; } switch(mes_state) { case INIT_ROS: rw::common::Log::log().info() << "In MES State: " << "ROS_INIT" << std::endl; _ROS->start(); mes_state = FREE; break; case FREE: set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_LOADING); set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_ERROR); set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_OUTOFBRICKS); set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_SORTING); set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_ORDERSORTED); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_ORDER_COMPLETE); set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_FREE); break; case SORTING: set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_FREE); set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_SORTING); if(!orderQueue.empty() && !workingOnOrder) { currentOrder = orderQueue.front(); nrOfBricksInOrder = currentOrder.get_blueBricks() + currentOrder.get_redBricks() + currentOrder.get_yellowBricks(); workingOnOrder = true; startBatchTime = ros::Time::now().toSec(); LINFO << "Working on Order: " << currentOrder.get_orderID(); } switch (control_state) { case INIT: { if (linearPath.empty()) { rw::common::Log::log().info() << "In State: " << "INIT" << std::endl; Q gripperInitOpenQ(1, 0.020); sendQtoGripper(gripperInitOpenQ); //Start conveyor belt PLCserial.write("f2"); // Issue move command for moving robot to ready position and switch state to MOVING_TO_READY_POS Frame::Ptr objectFrame = _rwWorkCell->findFrame("Lego1x6"); MovableFrame::Ptr objectMovFrame = objectFrame.cast<MovableFrame>(); Vector3D<> newPos(-0.60, 0.135, 0.007); RPY<> newRot(0, 0, -Pi); Transform3D<> newTarget = Transform3D<>(newPos, newRot); objectMovFrame->setTransform(newTarget, _state); Transform3D<> baseToObject = _device->baseTframe(_rwWorkCell->findFrame("Lego1x6"), _state); // Clear any previous planned path before generating a new linearPath.clear(); moveToTarget(baseToObject, -0.15, false); } //if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) { target_type = READY; moveRobot(); last_state = INIT; rw::common::Log::log().info() << "In State: " << "MOVING_TO_READY_POS" << std::endl; control_state = MOVING_TO_READY_POS; //} } break; case MOVING_TO_READY_POS: // Is ready position is reached? if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_READY_REACHED) { Q gripperInitOpenQ(1, 0.020); sendQtoGripper(gripperInitOpenQ); linearPath.clear(); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_READY_REACHED); target_type = NONE; last_state = MOVING_TO_READY_POS; rw::common::Log::log().info() << "In State: " << "GET_TARGETS" << std::endl; control_state = GET_TARGETS; } break; case GET_TARGETS: if(currentOrder.isOrderSorted()) { rw::common::Log::log().info() << "Order is sorted!"<< std::endl; PLCserial.write("f2"); conveyorStoppedTimeSaved = false; conveyorHasStopped = false; set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_SORTING); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_ORDER_COMPLETE); if(!orderQueue.empty()) orderQueue.pop(); visionQueue.clear(); workingOnOrder = false; LINFO << "Order " << currentOrder.get_orderID() <<" sorted successfully"; endBatchTime = ros::Time::now().toSec(); batchTime = endBatchTime - startBatchTime; break; } else if(conveyorHasStopped && visionQueue.empty()) { PLCserial.write("f2"); set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_SORTING); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_OUT_OF_BRICKS); LINFO << "Out of bricks for Order " << currentOrder.get_orderID(); conveyorStoppedTimeSaved = false; conveyorHasStopped = false; } if(!visionQueue.empty()) { std::cout << "Queue size is : " << visionQueue.size() << std::endl; nextBrick = visionQueue.front(); std::cout << "target is " << nextBrick.getBrickTransform() << std::endl; std::cout << "timestamp: " << nextBrick.getTimestamp() << std::endl; std::cout << "color: " << nextBrick.getColor() << std::endl; std::cout << "speed: " << nextBrick.getSpeed() << std::endl; std::cout << "Queue size after dequeue is : " << visionQueue.size() << std::endl; last_state = GET_TARGETS; rw::common::Log::log().info() << "In State: " << "WAITING_FOR_BRICK" << std::endl; control_state = WAITING_FOR_BRICK; } break; case MOVING_TO_BRICK_READY: // If brick ready position target is reached? if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_BRICK_READY_REACHED) { linearPath.clear(); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_BRICK_READY_REACHED); double currentTime; if(conveyorHasStopped) { currentTime = conveyorStoppedTime; } else { currentTime = ros::Time::now().toSec(); } double timeDiff = currentTime - nextBrick.getTimestamp(); for(int i=0; i<conveyorToBrick.R().m().size1(); i++) for(int j=0; j<conveyorToBrick.R().m().size2(); j++) if(fabs(0-conveyorToBrick.R()(i,j)) < 0.00001) conveyorToBrick.R()(i,j) = 0.0; Rotation3D<> rotX14(1.0, 0.0, 0.0, 0.0, cos(14*Deg2Rad), -sin(14*Deg2Rad), 0.0, sin(14*Deg2Rad), cos(14*Deg2Rad)); Rotation3D<> rotXneg14(1.0, 0.0, 0.0, 0.0, cos(-14*Deg2Rad), -sin(-14*Deg2Rad), 0.0, sin(-14*Deg2Rad), cos(-14*Deg2Rad)); Vector3D<> brickPos = conveyorToBrick.P(); Rotation3D<> brickRot(conveyorToBrick.R()); /*if(brickPos(1) > 18.5) brickRot = rotX14*brickRot; else if(brickPos(1) < 8.5) brickRot = rotXneg14*brickRot;*/ double brickSpeed = nextBrick.getSpeed(); brickPos(0) = brickPos(0) + ((brickSpeed) * timeDiff); brickPos(2) = fabs(brickPos(2)) + 0.005; brickPos(0) += BRICK_READY_X_OFFSET; // do not actually move the offset! Transform3D<> brickTransform(brickPos, brickRot);//conveyorToBrick.R()); rw::common::Log::log().info() << "brickTransform: \n" << brickTransform << std::endl; Frame::Ptr objectFrame = _rwWorkCell->findFrame("Lego1x6"); MovableFrame::Ptr objectMovFrame = objectFrame.cast<MovableFrame>(); objectMovFrame->setTransform(brickTransform, _state); Transform3D<> baseToObject = _device->baseTframe(_rwWorkCell->findFrame("Lego1x6"), _state); Q crapQ(7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); Q brickTargetQ = moveInvKin(baseToObject, 0.036); //moveToTarget(baseToObject, 0.05, false);//TODO -----------------HACK------------------ //moveToTarget(baseToObject, -0.03, false); if(brickTargetQ != crapQ) { moveToTarget(brickTargetQ); } else { linearPath.clear(); rw::common::Log::log().info() << "Brick no solution for InvKin!" << std::endl; visionQueue.pop(); control_state = GET_TARGETS; break; } if (!MANUAL_MODE) { // If the targets queue is not empty, Issue move command for moving robot to next brick position target target_type = BRICK; moveRobot(); last_state = MOVING_TO_BRICK_READY; rw::common::Log::log().info() << "In State: " << "MOVING_TO_BRICK" << std::endl; control_state = MOVING_TO_BRICK; } else if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) { // If the targets queue is not empty, Issue move command for moving robot to next brick position target target_type = BRICK; moveRobot(); last_state = MOVING_TO_BRICK_READY; rw::common::Log::log().info() << "In State: " << "MOVING_TO_BRICK" << std::endl; control_state = MOVING_TO_BRICK; } } break; case WAITING_FOR_BRICK: { double currentTime; if(conveyorHasStopped) { currentTime = conveyorStoppedTime; } else { currentTime = ros::Time::now().toSec(); } Frame::Ptr camFrame = _rwWorkCell->findFrame("webCamFrame"); Transform3D<> conveyorToCam = camFrame->getTransform(_state); conveyorToBrick = conveyorToCam*nextBrick.getBrickTransform(); double timeDiff = currentTime - nextBrick.getTimestamp(); RPY<> brickRot(conveyorToBrick.R()); Vector3D<> brickPos = conveyorToBrick.P(); double brickSpeed = nextBrick.getSpeed(); brickPos(0) = brickPos(0) + ((brickSpeed) * timeDiff); brickPos(2) = 0.010; if (brickPos(0) >= (END_GRASP_AREA - BRICK_READY_X_OFFSET) || conveyorHasStopped) { //stop conveyor belt PLCserial.write("s"); if(!conveyorStoppedTimeSaved) { conveyorStoppedTime = ros::Time::now().toSec(); conveyorStoppedTimeSaved = true; } conveyorHasStopped = true; Frame::Ptr camFrame = _rwWorkCell->findFrame("webCamFrame"); Transform3D<> conveyorToCam = camFrame->getTransform(_state); conveyorToBrick = conveyorToCam*nextBrick.getBrickTransform(); double currentTime=0.0; if(conveyorHasStopped) { currentTime = conveyorStoppedTime; } double timeDiff = currentTime - nextBrick.getTimestamp(); //RPY<> brickReadyRot(conveyorToBrick.R()); for(int i=0; i<conveyorToBrick.R().m().size1(); i++) for(int j=0; j<conveyorToBrick.R().m().size2(); j++) if(fabs(0-conveyorToBrick.R()(i,j)) < 0.00001) conveyorToBrick.R()(i,j) = 0.0; Vector3D<> brickReadyPos = conveyorToBrick.P(); brickReadyPos(0) += BRICK_READY_X_OFFSET; double brickSpeed = nextBrick.getSpeed(); brickReadyPos(0) = brickReadyPos(0) + ((brickSpeed)*timeDiff); brickReadyPos(2) = 0.030; rw::common::Log::log().info() << "brickReady X: \n" << brickReadyPos(0) << std::endl; // Are we past the grasp area? if(brickReadyPos(0) >= (END_GRASP_AREA + BRICK_READY_X_OFFSET) + 0.05) { rw::common::Log::log().info() << "Break!!!!" << std::endl; visionQueue.pop(); control_state = GET_TARGETS; break; } Transform3D<> brickReadyTransform(brickReadyPos, conveyorToBrick.R()); Frame::Ptr objectFrame = _rwWorkCell->findFrame("Lego1x6"); MovableFrame::Ptr objectMovFrame = objectFrame.cast<MovableFrame>(); rw::common::Log::log().info() << "brickReadyTransform: \n" << brickReadyTransform << std::endl; objectMovFrame->setTransform(brickReadyTransform, _state); Transform3D<> baseToObject = _device->baseTframe(_rwWorkCell->findFrame("Lego1x6"), _state); Q crapQ(7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); brickReadyTargetQ = moveInvKin(baseToObject, -0.050); if(brickReadyTargetQ != crapQ) { moveToTarget(brickReadyTargetQ); } else { rw::common::Log::log().info() << "BrickReady no solution for InvKin!" << std::endl; linearPath.clear(); visionQueue.pop(); control_state = GET_TARGETS; break; } if (!MANUAL_MODE) { target_type = BRICKREADY; moveRobot(); last_state = WAITING_FOR_BRICK; rw::common::Log::log().info() << "In State: " << "MOVING_TO_BRICK_READY" << std::endl; control_state = MOVING_TO_BRICK_READY; } else if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) { target_type = BRICKREADY; moveRobot(); last_state = WAITING_FOR_BRICK; rw::common::Log::log().info() << "In State: " << "MOVING_TO_BRICK_READY" << std::endl; control_state = MOVING_TO_BRICK_READY; } } } break; case MOVING_TO_BRICK: { // If brick position target is reached? if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_BRICK_REACHED) { linearPath.clear(); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_BRICK_REACHED); Q gripperCloseQ(1, 0.007); // Q gripperCloseQ(1, 0.0); without extension sendQtoGripper(gripperCloseQ); closeGripper(); _graspTimer->start(GRASP_TIME); last_state = MOVING_TO_BRICK; rw::common::Log::log().info() << "In State: " << "GRASPING" << std::endl; control_state = GRASPING; } } break; case GRASPING: if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_GRASPING_DONE) { nrOfGrasps++; // Issue move command for moving robot to box position target and switch state to MOVING_TO_BOX if(graspSuccessful()) { rw::common::Log::log().info() << "Grasp successful.." << std::endl; string colorOfBrick = nextBrick.getColor(); if((colorOfBrick == "Blue") && (currentOrder.get_blueBricks() > 0)) currentOrder.decrementBrick(colorOfBrick); else if ((colorOfBrick == "Red") && (currentOrder.get_redBricks() > 0)) currentOrder.decrementBrick(colorOfBrick); else if ((colorOfBrick == "Yellow") && (currentOrder.get_yellowBricks() > 0)) currentOrder.decrementBrick(colorOfBrick); else { rw::common::Log::log().info() << "Spare brick grasped!" << std::endl; set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_SPAREBRICK_GRASPED); } } else { rw::common::Log::log().info() << "Grasp unsuccessful!" << std::endl; } if(linearPath.empty()) { moveToTarget(brickReadyTargetQ); Q safeQJointPos, spareBoxJointPos, boxJointPos; safeQJointPos = Q(7, 0.971, -0.988, -0.243, -0.923, -0.049, -1.515, -0.754); moveToTarget(safeQJointPos); if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_SPAREBRICK_GRASPED) { spareBoxJointPos = Q(7, 95.0*Deg2Rad, -20.0*Deg2Rad, 0.0*Deg2Rad, -100.0*Deg2Rad, 0.0*Deg2Rad, -60.0*Deg2Rad, 8.0*Deg2Rad); moveToTarget(spareBoxJointPos); } else { boxJointPos = Q(7, 95.0*Deg2Rad, -90.0*Deg2Rad, 0.0*Deg2Rad, -10.0*Deg2Rad, 0.0*Deg2Rad, -80.0*Deg2Rad, 0.0*Deg2Rad); moveToTarget(boxJointPos); } } if (!MANUAL_MODE) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_GRASPING_DONE); if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_SPAREBRICK_GRASPED) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_SPAREBRICK_GRASPED); target_type = SPAREBOX; } else { target_type = BOX; } moveRobot(); last_state = GRASPING; rw::common::Log::log().info() << "In State: " << "MOVING_TO_BOX" << std::endl; control_state = MOVING_TO_BOX; } else if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_GRASPING_DONE); if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_SPAREBRICK_GRASPED) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_SPAREBRICK_GRASPED); target_type = SPAREBOX; } else { target_type = BOX; } moveRobot(); last_state = GRASPING; rw::common::Log::log().info() << "In State: " << "MOVING_TO_BOX" << std::endl; control_state = MOVING_TO_BOX; } } break; case MOVING_TO_BOX: // If Box position is reached? if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_BOX_REACHED || get_rsd_info(INFO_TYPE_STATUS) & STATUS_SPAREBOX_REACHED) { linearPath.clear(); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_BOX_REACHED); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_SPAREBOX_REACHED); Q gripperInitOpenQ(1, 0.020); sendQtoGripper(gripperInitOpenQ); //openGripper();//TODO wont open enougth _graspTimer->start(GRASP_TIME); target_type = NONE; last_state = MOVING_TO_BOX; rw::common::Log::log().info() << "In State: " << "UNGRASPING" << std::endl; control_state = UNGRASPING; } break; case UNGRASPING: if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_UNGRASPING_DONE) { visionQueue.pop(); if (linearPath.empty()) { // Issue move command for moving robot to ready position and switch state to MOVING_TO_READY_POS Q readyJointPos = Q(7, 0.972, -0.988, -0.242, -0.923, -0.049, -1.515, -0.751); moveToTarget(readyJointPos); } if (!MANUAL_MODE) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_UNGRASPING_DONE); target_type = READY; moveRobot(); last_state = UNGRASPING; control_state = MOVING_TO_READY_POS; } else if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) { set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_UNGRASPING_DONE); target_type = READY; moveRobot(); last_state = UNGRASPING; control_state = MOVING_TO_READY_POS; } } break; case RESET: // Clear any previous planned path, before reset if (!(get_rsd_info(INFO_TYPE_STATUS) & (STATUS_INITIAL_REACHED))) { if(linearPath.empty()) { //linearPath.clear(); Q intermediateJointPos = Q(7, 1.708, -0.988, -0.242, -0.923, 0.287, -1.235, -0.093); moveToTarget(intermediateJointPos); // Then do a move in joint space for the last distance Q initJointPos = Q(7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); moveToTarget(initJointPos); } if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) { target_type = INITIAL; moveRobot(); control_state = MOVING_TO_RESET; } } break; case MOVING_TO_RESET: if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_INITIAL_REACHED) { linearPath.clear(); set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_INITIAL_REACHED); target_type = NONE; last_state = MOVING_TO_RESET; rw::common::Log::log().info() << "In State: " << "ABORT" << std::endl; control_state = ABORT; } break; case ABORT: // ABORTING! break; default: break; } break; case ORDER_SORTED: set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_ORDERSORTED); break; case LOADING: set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_ORDERSORTED); set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_LOADING); break; case OUT_OF_BRICKS: set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_OUTOFBRICKS); break; case ERROR: set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_ERROR); break; } }