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