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;
}
Example #2
0
	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;
	}
Example #3
0
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);
	}
}
Example #4
0
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;
}
Example #5
0
	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;
	}
Example #6
0
        /// 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));
            }

        }
Example #7
0
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;
	}

}