static void concatenateLongReads(Node * node, Node * candidate,
Graph * graph)
{
PassageMarkerI marker, tmpMarker;
// Passage marker management in node:
for (marker = getMarker(node); marker != NULL_IDX;
marker = getNextInNode(marker)) {
if (!goesToNode(marker, candidate))
incrementFinishOffset(marker,
getNodeLength(candidate));
}
// Swapping new born passageMarkers from candidate to node
for (marker = getMarker(candidate); marker != NULL_IDX;
marker = tmpMarker) {
tmpMarker = getNextInNode(marker);
if (!comesFromNode(marker, node)) {
extractPassageMarker(marker);
incrementStartOffset(marker,
getNodeLength(node));
transposePassageMarker(marker, node);
incrementFinishOffset(getTwinMarker(marker),
getNodeLength(node));
} else {
reconnectPassageMarker(marker, node, &tmpMarker);
}
}
}
static void computePartialReadToNodeMapping(IDnum nodeID,
ReadOccurence ** readNodes,
IDnum * readNodeCounts,
boolean * readMarker,
ReadSet * reads)
{
ShortReadMarker *shortMarker;
IDnum index, readIndex;
ReadOccurence *readArray, *readOccurence;
Node *node = getNodeInGraph(graph, nodeID);
ShortReadMarker *nodeArray = getNodeReads(node, graph);
IDnum nodeReadCount = getNodeReadCount(node, graph);
PassageMarkerI marker;
for (index = 0; index < nodeReadCount; index++) {
shortMarker = getShortReadMarkerAtIndex(nodeArray, index);
readIndex = getShortReadMarkerID(shortMarker);
readArray = readNodes[readIndex];
readOccurence = &readArray[readNodeCounts[readIndex]];
readOccurence->nodeID = nodeID;
readOccurence->position =
getShortReadMarkerPosition(shortMarker);
readOccurence->offset =
getShortReadMarkerOffset(shortMarker);
readNodeCounts[readIndex]++;
}
for (marker = getMarker(node); marker != NULL_IDX;
marker = getNextInNode(marker)) {
readIndex = getPassageMarkerSequenceID(marker);
if (readIndex <= 0 || reads->categories[readIndex - 1] == REFERENCE)
continue;
if (!readMarker[readIndex]) {
readArray = readNodes[readIndex];
readOccurence =
&readArray[readNodeCounts[readIndex]];
readOccurence->nodeID = nodeID;
readOccurence->position = getStartOffset(marker);
readOccurence->offset =
getPassageMarkerStart(marker);
readNodeCounts[readIndex]++;
readMarker[readIndex] = true;
} else {
readArray = readNodes[readIndex];
readOccurence =
&readArray[readNodeCounts[readIndex] - 1];
readOccurence->position = -1;
readOccurence->offset = -1;
}
}
for (marker = getMarker(node); marker != NULL_IDX;
marker = getNextInNode(marker)) {
readIndex = getPassageMarkerSequenceID(marker);
if (readIndex > 0)
readMarker[readIndex] = false;
}
}
static IDnum *computeReadToNodeCounts()
{
IDnum readIndex, nodeIndex;
IDnum maxNodeIndex = 2 * nodeCount(graph) + 1;
IDnum maxReadIndex = sequenceCount(graph) + 1;
IDnum *readNodeCounts = callocOrExit(maxReadIndex, IDnum);
boolean *readMarker = callocOrExit(maxReadIndex, boolean);
ShortReadMarker *nodeArray, *shortMarker;
PassageMarkerI marker;
Node *node;
IDnum nodeReadCount;
//puts("Computing read to node mapping array sizes");
for (nodeIndex = 0; nodeIndex < maxNodeIndex; nodeIndex++) {
node = getNodeInGraph(graph, nodeIndex - nodeCount(graph));
if (node == NULL)
continue;
// Short reads
if (readStartsAreActivated(graph)) {
nodeArray = getNodeReads(node, graph);
nodeReadCount = getNodeReadCount(node, graph);
for (readIndex = 0; readIndex < nodeReadCount; readIndex++) {
shortMarker =
getShortReadMarkerAtIndex(nodeArray,
readIndex);
readNodeCounts[getShortReadMarkerID
(shortMarker)]++;
}
}
// Long reads
for (marker = getMarker(node); marker != NULL_IDX;
marker = getNextInNode(marker)) {
readIndex = getPassageMarkerSequenceID(marker);
if (readIndex < 0)
continue;
if (readMarker[readIndex])
continue;
readNodeCounts[readIndex]++;
readMarker[readIndex] = true;
}
// Clean up marker array
for (marker = getMarker(node); marker != NULL_IDX;
marker = getNextInNode(marker)) {
readIndex = getPassageMarkerSequenceID(marker);
if (readIndex > 0)
readMarker[readIndex] = false;
}
}
free(readMarker);
return readNodeCounts;
}
static void projectFromNode(IDnum nodeID,
ReadOccurence ** readNodes,
IDnum * readNodeCounts,
IDnum * readPairs, Category * cats,
boolean * dubious, Coordinate * lengths)
{
IDnum index;
ShortReadMarker *nodeArray, *shortMarker;
PassageMarker *marker;
Node *node;
IDnum nodeReadCount;
node = getNodeInGraph(graph, nodeID);
if (node == NULL || !getUniqueness(node))
return;
nodeArray = getNodeReads(node, graph);
nodeReadCount = getNodeReadCount(node, graph);
for (index = 0; index < nodeReadCount; index++) {
shortMarker = getShortReadMarkerAtIndex(nodeArray, index);
if (dubious[getShortReadMarkerID(shortMarker) - 1])
continue;
projectFromShortRead(node, shortMarker, readPairs, cats,
readNodes, readNodeCounts, lengths);
}
for (marker = getMarker(node); marker != NULL;
marker = getNextInNode(marker)) {
if (getPassageMarkerSequenceID(marker) > 0)
projectFromLongRead(node, marker, readPairs, cats,
readNodes, readNodeCounts,
lengths);
}
}
void Composition::jumpToMarkerEndFrame(int index)
{
Marker *marker = getMarker(index);
if(marker) {
jumpToMarkerEndFrame(marker);
}
}
void Composition::jumpToMarkerEndFrame(const string& name)
{
Marker *marker = getMarker(name);
if(marker) {
jumpToMarkerEndFrame(marker);
}
}
void Composition::setActiveMarker(int index, float speed)
{
Marker *marker = getMarker(index);
if(marker) {
setActiveMarker(marker, speed);
}
}
void Composition::setActiveMarker(const string& name, float speed)
{
Marker *marker = getMarker(name);
if(marker) {
setActiveMarker(marker, speed);
}
}
// DEBUG
void checkNode(Node* node) {
PassageMarkerI marker1 = getMarker(node);
if (marker1 == NULL_IDX)
return;
PassageMarkerI marker2 = getNextInNode(marker1);
if (marker2 == NULL_IDX)
return;
if (getStartOffset(marker1) == getStartOffset(marker2))
abort();
if (getFinishOffset(marker1) == getFinishOffset(marker2))
abort();
printf(">>>> Node %li\n", (long) getNodeID(node));
printf("Marker1: %li - %li > %li (%li) \n", (long) getStartOffset(marker1), (long) getPassageMarkerLength(marker1), (long) (getNodeLength(node) - getFinishOffset(marker1)), (long) getFinishOffset(marker1));
printf("%s\n", readPassageMarker(marker1));
printf("Marker2: %li - %li > %li (%li) \n", (long) getStartOffset(marker2), (long) getPassageMarkerLength(marker2), (long) (getNodeLength(node) - getFinishOffset(marker2)), (long) getFinishOffset(marker2));
printf("%s\n", readPassageMarker(marker2));
if (getStartOffset(marker1) < getNodeLength(node) - getFinishOffset(marker2)
&& getStartOffset(marker2) < getNodeLength(node) - getFinishOffset(marker1)) {
//abort();
;
}
}
static void adjustLongReads(Node * target, Coordinate nodeLength)
{
PassageMarkerI marker;
for (marker = getMarker(target); marker != NULL_IDX;
marker = getNextInNode(marker))
incrementFinishOffset(marker, nodeLength);
}
int Guitar::getNote() const
{
cv::Mat camToInstrument(4,4,CV_32FC1, (void*)getMarker().getTransformation());
cv::Mat camToCenter(4,4,CV_32FC1, (void*)m_centralpoint.getTransformation());
cv::Mat centralToInstrument = camToCenter.inv() * camToInstrument;
return round(((int)Marker::getRotationAngleZ((float*)centralToInstrument.data))/45);
}
void SeriesList::drawInFrame(Frame& innerFrame, double minX, double maxX, double minY, double maxY)
{
double multX = innerFrame.getWidth()/(maxX-minX);
double multY = innerFrame.getHeight()/(maxY-minY);
// Draw lines
for(int i=0;i<getNumSeries();i++)
{
innerFrame.push_state();
StrokeStyle s = getStyle(i);
Marker m = getMarker(i);
if(m.getColor().isClear() && s.getColor().isClear())
{
innerFrame << Comment("Plot contained data with clear stroke and marker. Skipping.");
continue;
}
vector< pair<double,double> >& vec = getPointList(i);
Path curve(vec,innerFrame.lx(), innerFrame.ly());
// What I'd give for a line of haskell...
// map (\(x,y) -> (multX*(x-minX), multY*(y-minY))) vector
map_object map_instance(multX,minX,multY,minY);
innerFrame.setMarker(m);
innerFrame.setLineStyle(s);
if(s.getColor().isClear())
{
// crop
auto_ptr< Path > cropX = Splitter::cropToBox(minX,maxX,minY,maxY,curve);
// Fit it to the box.
std::for_each(cropX->begin(), cropX->end(), map_instance);
// Draw the line
innerFrame.line(*cropX);
}
else
{
// interpolate
auto_ptr< std::list<Path> > interpX = Splitter::interpToBox(minX,maxX,minY,maxY,curve);
for(std::list<Path>::iterator i=interpX->begin();i!=interpX->end();i++)
{
// Fit it to the box
std::for_each(i->begin(), i->end(), map_instance);
// Draw the line
innerFrame.line(*i);
}
}
innerFrame.pop_state();
}
}
static void admitGroupies(Node * source, Node * bypass)
{
PassageMarkerI marker, tmpMarker;
for (marker = getMarker(source); marker != NULL_IDX;
marker = tmpMarker) {
tmpMarker = getNextInNode(marker);
extractPassageMarker(marker);
transposePassageMarker(marker, bypass);
incrementFinishOffset(getTwinMarker(marker),
getNodeLength(bypass));
}
}
void GameState::onUpdate(float dt) {
if(m_renderTextures[0].getSize() != Root().window->getSize()) {
resize();
}
// m_zoom = 6;
if(m_player) {
float targetZoom = 6;// + m_player->physicsBody()->getLinearVelocity().length();
float zoomSpeed = 2;
m_zoom = m_zoom * (1 - dt * zoomSpeed) + targetZoom * (dt * zoomSpeed);
glm::vec2 target = m_player->position() - glm::vec2(0, 0.3);
glm::vec2 d(0.5, 0.5);
glm::vec2 diff = target - m_center;
diff.x = diff.x < -d.x ? -d.x : (diff.x > d.x ? d.x : diff.x);
diff.y = diff.y < -d.y ? -d.y : (diff.y > d.y ? d.y : diff.y);
auto new_center = target - diff;
zoomSpeed = 8;
m_center = m_center * (1 - dt * zoomSpeed) + new_center * (dt * zoomSpeed);
if(m_helpProgress < 1.f) {
if(m_currentHelp == "") {
// check marker distance
auto trigger = getMarker(Marker::HELP_TRIGGER);
if(trigger) {
float distance = glm::length(trigger->position() - m_player->position());
if(distance < 2.f) { // trigger distance
m_currentHelp = m_levelHelp[m_currentLevelName];
}
}
} else {
m_helpProgress = fmin(1.f, m_helpProgress + dt / 5.f);
}
}
}
if(m_message != "") {
m_messageTime += dt;
if(m_messageTime > 5) {
m_message = "";
}
}
}
static void trimLongReadTips()
{
IDnum index;
Node *node;
PassageMarkerI marker, next;
velvetLog("Trimming read tips\n");
for (index = 1; index <= nodeCount(graph); index++) {
node = getNodeInGraph(graph, index);
if (getUniqueness(node))
continue;
for (marker = getMarker(node); marker != NULL_IDX;
marker = next) {
next = getNextInNode(marker);
if (!isInitial(marker) && !isTerminal(marker))
continue;
if (isTerminal(marker))
marker = getTwinMarker(marker);
while (!getUniqueness(getNode(marker))) {
if (next != NULL_IDX
&& (marker == next
|| marker == getTwinMarker(next)))
next = getNextInNode(next);
if (getNextInSequence(marker) != NULL_IDX) {
marker = getNextInSequence(marker);
destroyPassageMarker
(getPreviousInSequence
(marker));
} else {
destroyPassageMarker(marker);
break;
}
}
}
}
}
void GameState::loadLevel(int num) {
if(Root().game_state.m_player) Root().game_state.m_player->m_walkSound.pause();
if(num < 0 || num >= m_levels.size()) {
Root().menu_state.setGameOver(num > 0);
Root().states.pop();
return;
}
m_currentLevel = num;
m_currentLevelName = m_levels[m_currentLevel].first;
m_helpProgress = 0.f;
m_currentHelp = "";
std::string filename = m_currentLevelName + ".dat";
loadFromFile("levels/" + filename);
// spawn something
auto spawn = getMarker(Marker::SPAWN);
m_player = nullptr;
auto pos = glm::vec2(0, 0);
if(spawn) {
pos = spawn->position();
} else {
std::cout << "Warning: level " << filename << " does not contain any spawn marker. Spawning at (0, 0)." << std::endl;
}
if(m_currentLevelName == "spawn") {
spawnEgg(pos);
} else {
spawnPlayer(pos);
}
m_levelFade = 1.f;
tween::TweenerParam p2(1000, tween::SINE, tween::EASE_IN_OUT);
p2.addProperty(&m_levelFade, 0.f);
m_tweener.addTween(p2);
if(Root().debug && m_debugDrawEnabled) {
message(m_currentLevelName);
}
}
static void updateMembers(Node * bypass, Node * nextNode)
{
PassageMarkerI marker, next, tmp;
Coordinate nextLength = getNodeLength(nextNode);
// Update marker + arc info
for (marker = getMarker(bypass); marker != NULL_IDX; marker = tmp) {
tmp = getNextInNode(marker);
if (!isTerminal(marker)
&& getNode(getNextInSequence(marker)) == nextNode) {
// Marker steps right into target
next = getNextInSequence(marker);
disconnectNextPassageMarker(marker, graph);
destroyPassageMarker(next);
} else if (getUniqueness(nextNode)
&& goesToNode(marker, nextNode)) {
// Marker goes indirectly to target
while (getNode(getNextInSequence(marker)) !=
nextNode) {
next = getNextInSequence(marker);
disconnectNextPassageMarker(marker, graph);
destroyPassageMarker(next);
}
next = getNextInSequence(marker);
disconnectNextPassageMarker(marker, graph);
destroyPassageMarker(next);
} else if (!isTerminal(marker)
&& getFinishOffset(marker) == 0) {
// Marker goes somewhere else than to target
next = getNextInSequence(marker);
incrementFinishOffset(marker, nextLength);
} else {
// Marker goes nowhere
incrementFinishOffset(marker, nextLength);
}
}
}
int main() {
int funcIndex, status;
params *arguments = malloc(sizeof(params));
bool newLineNeeded = { TRUE };
getStartupText();
while (TRUE) {
getMarker(&newLineNeeded);
newLineNeeded.x = TRUE;
if ((funcIndex = getFunc(getInput(SPACE_CHAR))) == INVALID_FUNCTION)
continue;
if ((status = getArgs(&(cmd[funcIndex].info), getInput(NEWLINE),
arguments)) != OK) {
continue;
}
if (cmd[funcIndex].ptr == &empty) { /* Doesn't create a new line after an empty command. */
newLineNeeded.x = FALSE;
continue;
}
(cmd[funcIndex].ptr)(arguments);
}/* End while */
return 0;
}
int findPointCorrespondances(int *listSize, double *list, double **imgPts) {
int error_code;
if (VERBOSE)
printf("-------\n");
if (VERBOSE)
printf("number of segments: %d\n", *listSize);
if (*listSize >= 28) {
quadrilateral qlList[*listSize / 4];
quadrilateralSet qlSet[QLSET_NB_QLS];
markerQr marker;
getQlList(*listSize, list, qlList);
getQlSetArr(*listSize / 4, qlList, qlSet);
error_code = getIncompleteQlSetArr(*listSize / 4, qlList, qlSet);
if (error_code == MRKR_INCOMPLETE_FOUND || error_code == MRKR_COMPLETE_FOUND)
{
getMarker(qlSet, &marker);
getMarkerVertices(marker, imgPts);
}
} else {
for (int i = 0; i < MRKR_NP; i++) {
imgPts[i][0] = 0;
imgPts[i][1] = 0;
}
error_code = MRKR_NOT_ENOUGH_SEGMENTS;
printf("MSG: MRKR_NOT_ENOUGH_SEGMENTS\n");
}
return error_code;
}
bool Cortex::runSM(Cortex::RunState start, Cortex::RunState stop)
{
if(!executingRun)
{
currentRunState = start;
number_trails = 0;
marker_number = 0;
executingRun = true;
}
if(executingRun && currentRunState == stop)
{
executingRun = false;
return true;
}
switch(currentRunState)
{
case START:
ROS_INFO("**********START**********");
torso->sendGoal(0.20);
head->sendGoal(0,0.45,0,0);
switchControllers(arm_controllers_default, arm_controllers_JTcart); // Switch to default arm controllers
currentRunState = TUCK;
nextRunState = MOVE_TO_START_LOCATION;
break;
// -------------------------------------------------------------------------------------- //
case TUCK:
ROS_INFO("**********TUCKING ARM**********");
armActionGoal.tuck_left = true;
armActionGoal.tuck_right = true;
tuck_ac_->sendGoal(armActionGoal);
currentRunState = WAIT_FOR_TUCK;
break;
case UNTUCK:
ROS_INFO("**********UNTUCKING ARM**********");
armActionGoal.tuck_left = false;
armActionGoal.tuck_right = true;
tuck_ac_->sendGoal(armActionGoal);
currentRunState = WAIT_FOR_TUCK;
break;
case WAIT_FOR_TUCK:
if( (ros::Time::now() - storedTime) > ros::Duration(0.25)) // Check every 0.25 seconds
{
//ROS_INFO("**********WAIT_FOR_TUCK**********");
ac_state = new actionlib::SimpleClientGoalState(tuck_ac_->getState());
if(ac_state->isDone())
{
if(ac_state->state_ == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("Base action finished: %s",ac_state->toString().c_str());
}
else
{
ROS_INFO("Base action failed (?): %s",ac_state->toString().c_str());
}
currentRunState = nextRunState;
}
delete ac_state;
storedTime = ros::Time::now();
}
break;
// -------------------------------------------------------------------------------------- //
case MOVE_TO_START_LOCATION:
ROS_INFO("**********MOVE_TO_START_LOCATION**********");
if(getLocationPose("start",&locationPose))
base->sendBaseGoal_MapFrame(locationPose);
currentRunState = WAIT_FOR_BASE_MOTION;
nextRunState = PREPARE_ARMS_1;
break;
case PREPARE_ARMS_1:
ROS_INFO("**********PREPARE_ARMS_1**********");
currentRunState = UNTUCK;
nextRunState = PREPARE_ARMS_2;
break;
case PREPARE_ARMS_2:
ROS_INFO("**********PREPARE_ARMS_2**********");
armsJoint->sendGoal(l_object_tuck,ArmsJoint::LEFT);
ros::Duration(1.0).sleep();
currentRunState = MOVE_TO_PICKING_LOCATION;
break;
// -------------------------------------------------------------------------------------- //
case MOVE_TO_PICKING_LOCATION:
ROS_INFO("**********MOVE_TO_PICKING_LOCATION**********");
if(getLocationPose("front_counter",&locationPose))
base->sendBaseGoal_MapFrame(locationPose);
currentRunState = WAIT_FOR_BASE_MOTION;
nextRunState = START_PICKING;
break;
case WAIT_FOR_BASE_MOTION:
if( (ros::Time::now() - storedTime) > ros::Duration(0.25)) // Check every 0.25 seconds
{
//ROS_INFO("**********WAIT_FOR_BASE_MOTION**********");
if(base->motionComplete())
{
currentRunState = nextRunState;
}
storedTime = ros::Time::now();
}
break;
// -------------------------------------------------------------------------------------- //
case START_PICKING:
ROS_INFO("**********START_PICKING**********");
currentRunState = GET_OBJECT_LOCATION;
break;
case GET_OBJECT_LOCATION:
ROS_INFO("**********GET_OBJECT_LOCATION**********");
if( getMarker(marker_number) ) // Marker was detected
{
std::cout<<marker_msg.response.marker_position;
currentRunState = MOVE_TO_OBJECT;
}
else
{
currentRunState = GET_OBJECT_LOCATION;
}
break;
case MOVE_TO_OBJECT:
ROS_INFO("**********MOVE_TO_OBJECT**********");
// Adjust base location
//ros::Duration(1.0).sleep();
currentRunState = PICK_OBJECT;
break;
// -------------------------------------------------------------------------------------- //
case PICK_OBJECT:
ROS_INFO("**********PICK_OBJECT**********"); // TODO make this a separate function / grasp class
// TODO remove any blocking functions, sleep statements
// Untuck arm using joint positions
armsJoint->sendGoal(l_untuck_joints,ArmsJoint::LEFT);
while(!armsJoint->motionComplete())
{
std::cout<<".";
ros::Duration(0.5).sleep();
}
std::cout<<"\n";
// Switch to Cartesian
switchControllers(arm_controllers_JTcart, arm_controllers_default);
ros::Duration(0.1).sleep();
// Adjust arm position depending on marker location
ROS_INFO("Moving to position");
if(getArmPose("l_start_picking",&armPose))
{
armPose.position.y = marker_msg.response.marker_position.pose.position.y; // This assumes the PR2 is perfectly parallel to marker
// armPose.position.z = marker_msg.response.marker_position.pose.position.z;
std::cout<<armPose;
arms->moveToPose(ArmsCartesian::LEFT,armPose,true);
}
// Open grippers
grippers->open(Gripper::LEFT);
ros::Duration(0.5).sleep();
distance=0.22;
//Move forward
ROS_INFO("Moving forward");
arms->moveInDirection(ArmsCartesian::LEFT, armPose, ArmsCartesian::X, distance, x_step, dt);
ros::Duration(2.0).sleep();
grippers->closeGently(Gripper::LEFT);
ros::Duration(2.0).sleep();
//Move Backward
ROS_INFO("Moving Back");
arms->moveInDirection(ArmsCartesian::LEFT, armPose, ArmsCartesian::X, -distance, x_step, dt);
ros::Duration(2.0).sleep();
currentRunState = TUCK_OBJECT;
break;
case TUCK_OBJECT:
ROS_INFO("**********TUCK_OBJECT**********");
switchControllers(arm_controllers_default, arm_controllers_JTcart);
armsJoint->sendGoal(l_object_tuck,ArmsJoint::LEFT);
currentRunState = START_PLACING;
break;
// -------------------------------------------------------------------------------------- //
case START_PLACING:
ROS_INFO("**********START_PLACING**********");
currentRunState = MOVE_TO_DROP_LOCATION;
break;
case MOVE_TO_DROP_LOCATION:
ROS_INFO("**********MOVE_TO_DROP_LOCATION**********");
if(getLocationPose("front_table",&locationPose))
base->sendBaseGoal_MapFrame(locationPose);
currentRunState = WAIT_FOR_BASE_MOTION;
nextRunState = PLACE_OBJECT;
break;
case PLACE_OBJECT:
ROS_INFO("**********PLACE_OBJECT**********");
// Untuck arm using joint positions
switchControllers(arm_controllers_default, arm_controllers_JTcart);
armsJoint->sendGoal(l_untuck_placing_joints,ArmsJoint::LEFT);
while(!armsJoint->motionComplete())
{
std::cout<<".";
ros::Duration(0.5).sleep();
}
std::cout<<"\n";
// Switch to Cartesian
switchControllers(arm_controllers_JTcart, arm_controllers_default);
ros::Duration(0.1).sleep();
ROS_INFO("Moving to position");
if(getArmPose("l_start_placing",&armPose))
{
std::cout<<armPose;
arms->moveToPose(ArmsCartesian::LEFT,armPose,true);
}
ros::Duration(1.0).sleep();
distance = 0.2;
//Move forward
ROS_INFO("Moving forward");
arms->moveInDirection(ArmsCartesian::LEFT, armPose, ArmsCartesian::X, distance, x_step, dt);
ros::Duration(2.0).sleep();
grippers->open(Gripper::LEFT);
ros::Duration(2.0).sleep();
//Move Backward
ROS_INFO("Moving Back");
std::cout<<armPose;
arms->moveInDirection(ArmsCartesian::LEFT, armPose, ArmsCartesian::X, -distance, x_step, dt);
ros::Duration(2.0).sleep();
grippers->close(Gripper::LEFT);
switchControllers(arm_controllers_default, arm_controllers_JTcart);
currentRunState = STOP_PLACING;
break;
case STOP_PLACING:
ROS_INFO("**********STOP_PLACING**********");
if(marker_number<2)
{
currentRunState = PREPARE_ARMS_2;
number_trails++;
marker_number++;
ROS_INFO("Number of trails: %d", number_trails);
}
else
{
currentRunState = TUCK;
nextRunState = MOVE_BACK;
}
break;
// -------------------------------------------------------------------------------------- //
case MOVE_BACK:
ROS_INFO("**********MOVE_BACK**********");
if(getLocationPose("start",&locationPose))
base->sendBaseGoal_MapFrame(locationPose);
currentRunState = WAIT_FOR_BASE_MOTION;
nextRunState = DONE;
case DONE:
ROS_INFO("**********DONE**********");
default:
ROS_WARN("runSM: invalid state");
break;
}
return false;
}
static boolean uniqueNodesConnect(Node * startingNode)
{
Node *destination = NULL;
PassageMarkerI startMarker, currentMarker;
RBConnection *newList;
RBConnection *list = NULL;
boolean multipleHits = false;
if (arcCount(startingNode) == 0)
return false;
if (getMarker(startingNode) == NULL_IDX)
return false;
dbgCounter++;
// Checking for multiple destinations
for (startMarker = getMarker(startingNode); startMarker != NULL_IDX;
startMarker = getNextInNode(startMarker)) {
if (getFinishOffset(startMarker) >
2 * getWordLength(graph))
continue;
for (currentMarker = getNextInSequence(startMarker);
currentMarker != NULL_IDX;
currentMarker = getNextInSequence(currentMarker)) {
if (!getUniqueness(getNode(currentMarker))) {
continue;
} else if (getNodeStatus(getNode(currentMarker))) {
if (getStartOffset(currentMarker) >
2 * getWordLength(graph))
break;
for (newList = list; newList != NULL;
newList = newList->next) {
if (newList->node ==
getNode(currentMarker)) {
newList->multiplicity++;
break;
}
}
if (newList == NULL)
abort();
break;
} else {
if (getStartOffset(currentMarker) >
2 * getWordLength(graph))
break;
setSingleNodeStatus(getNode(currentMarker),
true);
newList = allocateRBConnection();
newList->node = getNode(currentMarker);
newList->multiplicity = 1;
newList->marker = startMarker;
newList->next = list;
list = newList;
break;
}
}
}
while (list != NULL) {
newList = list;
list = newList->next;
setSingleNodeStatus(newList->node, false);
if (newList->multiplicity >= MULTIPLICITY_CUTOFF) {
if (destination == NULL) {
destination = newList->node;
path = newList->marker;
} else if (destination != newList->node)
multipleHits = true;
}
deallocateRBConnection(newList);
}
if (multipleHits) {
multCounter++;
setUniqueness(startingNode, false);
return false;
}
if (destination == NULL || destination == startingNode
|| destination == getTwinNode(startingNode)) {
nullCounter++;
return false;
}
// Check for reciprocity
for (startMarker = getMarker(getTwinNode(destination));
startMarker != NULL_IDX;
startMarker = getNextInNode(startMarker)) {
if (getFinishOffset(startMarker) >
2 * getWordLength(graph))
continue;
for (currentMarker = getNextInSequence(startMarker);
currentMarker != NULL_IDX;
currentMarker = getNextInSequence(currentMarker)) {
if (!getUniqueness(getNode(currentMarker))) {
continue;
} else if (getNodeStatus(getNode(currentMarker))) {
if (getStartOffset(currentMarker) >
2 * getWordLength(graph))
break;
for (newList = list; newList != NULL;
newList = newList->next) {
if (newList->node ==
getNode(currentMarker)) {
newList->multiplicity++;
break;
}
}
if (newList == NULL)
abort();
break;
} else {
if (getStartOffset(currentMarker) >
2 * getWordLength(graph))
break;
setSingleNodeStatus(getNode(currentMarker),
true);
newList = allocateRBConnection();
newList->node = getNode(currentMarker);
newList->multiplicity = 1;
newList->next = list;
list = newList;
break;
}
}
}
while (list != NULL) {
newList = list;
list = newList->next;
setSingleNodeStatus(newList->node, false);
if (newList->multiplicity >= MULTIPLICITY_CUTOFF
&& newList->node != getTwinNode(startingNode))
multipleHits = true;
deallocateRBConnection(newList);
}
if (multipleHits) {
multCounter++;
setUniqueness(destination, false);
return false;
}
// Aligning long reads to each other:
// TODO
// Merge pairwise alignments and produce consensus
// TODO
return true;
}
bool SpherePickingRobotNavigator::performSphereSegmentation()
{
// ===================================== preparing result objects =====================================
arm_navigation_msgs::CollisionObject obj;
int bestClusterIndex = -1;
sensor_msgs::PointCloud sphereCluster;
// ===================================== calling segmentation =====================================
sphere_segmentation_.fetchParameters(SEGMENTATION_NAMESPACE);
bool success = sphere_segmentation_.segment(segmented_clusters_,obj,bestClusterIndex);
// ===================================== checking results ========================================
if(!success)
{
ROS_ERROR_STREAM(NODE_NAME<<": Sphere segmentation did not succeed");
return false;
}
// ===================================== storing results =====================================
// assigning id first
obj.id = makeCollisionObjectNameFromModelId(0);
// retrieving segmented sphere cluster
sphere_segmentation_.getSphereCluster(sphereCluster);
// storing best cluster
segmented_clusters_.clear();
segmented_clusters_.push_back(sphereCluster);
// storing pose
geometry_msgs::PoseStamped pose;
pose.header.frame_id = obj.header.frame_id;
pose.pose = obj.poses[0];
recognized_obj_pose_map_[std::string(obj.id)] = pose;
// storing recognized object as model for planning
household_objects_database_msgs::DatabaseModelPoseList models;
household_objects_database_msgs::DatabaseModelPose model;
model.model_id = 0;
model.pose = pose;
model.confidence = 1.0f;
model.detector_name = "sphere_segmentation";
models.model_list.push_back(model);
recognized_models_.clear();
recognized_models_.push_back(models);
// ===================================== updating local planning scene =====================================
addDetectedObjectToLocalPlanningScene(obj);
// ===================================== updating markers =====================================
// update markers
visualization_msgs::Marker &segMarker = getMarker(MARKER_SEGMENTED_OBJECT);
visualization_msgs::Marker &attachedMarker = getMarker(MARKER_ATTACHED_OBJECT);
// segmented object
collisionObjToMarker(obj,segMarker);
segMarker.action = visualization_msgs::Marker::ADD;
addMarker(MARKER_SEGMENTED_OBJECT,segMarker);
// attached object, hide for now until gripper is closed
collisionObjToMarker(obj,attachedMarker);
attachedMarker.action = visualization_msgs::Marker::DELETE;
attachedMarker.header.frame_id = gripper_link_name_;
addMarker(MARKER_ATTACHED_OBJECT,attachedMarker);
// ===================================== printing completion info message =====================================
arm_navigation_msgs::Shape &shape = obj.shapes[0];
ROS_INFO_STREAM("\n"<<NODE_NAME<<": Sphere Segmentation completed:\n");
ROS_INFO_STREAM("\tFrame id: "<<obj.header.frame_id<<"\n");
ROS_INFO_STREAM("\tRadius: "<<shape.dimensions[0]<<"\n");
ROS_INFO_STREAM("\tx: "<<pose.pose.position.x<<", y: "<<pose.pose.position.y
<<", z: "<<pose.pose.position.z<<"\n");
return true;
}
Marker* Composition::getMarker(const string& name)
{
return getMarker(getMarkerIndex(name));
}
Marker MarkerHandler::processMarker(string markerText, const int& where) {
StringUtil::trim(markerText);
if(markerText.find("#pragma ")!=0)
{
throw string("Invalid Marker, all markers should start with the #pragma pre-processor directive");
}
StringUtil::replaceFirst(markerText, "#pragma ", "");
vector<string> fragments = StringUtil::splitAndReturn<vector<string> >(markerText, " ");
string name = fragments.at(0);
if(name=="")
{
throw string("NOT_MARKER");
}
if(validMarkers.size()==0)
{
initMarkers();
}
Marker targetMarker = getMarker(name, where);
if(targetMarker.name=="")
{
string err = "Could not find the "+Marker::getTypeName(where)+" type marker with name " + name;
throw err;
}
targetMarker = getMarker(name);
if(targetMarker.name=="")
{
string err = "Could not find a marker with name " + name;
throw err;
}
if(targetMarker.reqAttrSize>0 && (int)fragments.size()==1)
{
string err = "No attributes specified for the marker " + targetMarker.name;
throw err;
}
else
{
map<string, string> tvalues;
for (int i = 1; i < (int)fragments.size(); ++i) {
if(fragments.at(i)!="")
{
vector<string> attr = StringUtil::splitAndReturn<vector<string> >(fragments.at(i), "=");
string attname = attr.at(0);
string value = attr.at(1);
StringUtil::trim(attname);
StringUtil::trim(value);
if(value.at(0)!='"' && value.at(value.length()-1)!='"')
{
string err = "Attribute value for "+attname+" should be within double quotes (\"\") for the marker " + targetMarker.name;
throw err;
}
if(value!="\"\"")
{
value = value.substr(1, value.length()-2);
}
else
{
value = "";
}
tvalues[attname] = value;
}
}
map<string, bool>::iterator it = targetMarker.attributes.begin();
for(;it!=targetMarker.attributes.end();++it) {
if(tvalues.find(it->first)==tvalues.end())
{
if(it->second) {
string err = "No value specified for mandatory attribute "+it->first+" for the marker " + targetMarker.name;
throw err;
}
cout << "Ignoring attribute " + it->first + " for marker " + targetMarker.name << endl;
tvalues.erase(it->first);
}
else
{
vector<string> vss = targetMarker.valueSet[it->first];
string value = tvalues[it->first];
if(value=="")
{
string err = "Attribute value for "+it->first+" cannot be blank for the marker " + targetMarker.name;
throw err;
}
if(vss.size()>0)
{
bool valid = false;
for (int var = 0; var < (int)vss.size(); ++var) {
if(vss.at(var)==value)
{
valid = true;
break;
}
}
if(!valid)
{
string err = "Attribute "+it->first+" cannot have a value of "+value+" for the marker " + targetMarker.name;
throw err;
}
}
}
}
targetMarker.attributeValues = tvalues;
}
return targetMarker;
}
// Replaces two consecutive nodes into a single equivalent node
// The extra memory is freed
void concatenateStringOfNodes(Node * nodeA, Graph * graph)
{
Node *twinA = getTwinNode(nodeA);
Node * nodeB = nodeA;
Node * twinB;
Node *currentNode, *nextNode;
Coordinate totalLength = 0;
PassageMarkerI marker, tmpMarker;
Arc *arc;
Category cat;
while (simpleArcCount(nodeB) == 1
&&
simpleArcCount(getTwinNode
(getDestination(getArc(nodeB)))) ==
1
&& getDestination(getArc(nodeB)) != getTwinNode(nodeB)
&& getDestination(getArc(nodeB)) != nodeA) {
totalLength += getNodeLength(nodeB);
nodeB = getDestination(getArc(nodeB));
}
twinB = getTwinNode(nodeB);
totalLength += getNodeLength(nodeB);
reallocateNodeDescriptor(nodeA, totalLength);
currentNode = nodeA;
while (currentNode != nodeB) {
currentNode = getDestination(getArc(currentNode));
// Passage marker management in node A:
for (marker = getMarker(nodeA); marker != NULL_IDX;
marker = getNextInNode(marker))
if (getNode(getNextInSequence(marker)) != currentNode)
incrementFinishOffset(marker,
getNodeLength(currentNode));
// Swapping new born passageMarkers from B to A
for (marker = getMarker(currentNode); marker != NULL_IDX; marker = tmpMarker) {
tmpMarker = getNextInNode(marker);
if (isInitial(marker)
|| getNode(getPreviousInSequence(marker)) != nodeA) {
extractPassageMarker(marker);
transposePassageMarker(marker, nodeA);
incrementFinishOffset(getTwinMarker(marker),
getNodeLength(nodeA));
} else
disconnectNextPassageMarker(getPreviousInSequence
(marker), graph);
}
// Read starts
concatenateReadStarts(nodeA, currentNode, graph);
// Gaps
appendNodeGaps(nodeA, currentNode, graph);
// Update uniqueness:
setUniqueness(nodeA, getUniqueness(nodeA) || getUniqueness(currentNode));
// Update virtual coverage
for (cat = 0; cat < CATEGORIES; cat++)
incrementVirtualCoverage(nodeA, cat,
getVirtualCoverage(currentNode, cat));
// Update original virtual coverage
for (cat = 0; cat < CATEGORIES; cat++)
incrementOriginalVirtualCoverage(nodeA, cat,
getOriginalVirtualCoverage
(currentNode, cat));
// Descriptor management (node)
directlyAppendDescriptors(nodeA, currentNode, totalLength);
}
// Correct arcs
for (arc = getArc(nodeB); arc != NULL; arc = getNextArc(arc)) {
if (getDestination(arc) != twinB)
createAnalogousArc(nodeA, getDestination(arc),
arc, graph);
else
createAnalogousArc(nodeA, twinA, arc, graph);
}
// Freeing gobbled nodes
currentNode = getTwinNode(nodeB);
while (currentNode != getTwinNode(nodeA)) {
arc = getArc(currentNode);
nextNode = getDestination(arc);
destroyNode(currentNode, graph);
currentNode = nextNode;
}
}
bool Composition::isMarkerActive(const string& name)
{
Marker *marker = getMarker(name);
return marker?isMarkerActive(marker):false;
}
bool Composition::isMarkerActive(int index)
{
Marker *marker = getMarker(index);
return marker?isMarkerActive(marker):false;
}
// Replaces two consecutive nodes into a single equivalent node
// The extra memory is freed
void concatenateNodes(Node * nodeA, Node * nodeB, Graph * graph)
{
PassageMarkerI marker, tmpMarker;
Node *twinA = getTwinNode(nodeA);
Node *twinB = getTwinNode(nodeB);
Arc *arc;
Category cat;
// Arc management:
// Freeing useless arcs
while (getArc(nodeA) != NULL)
destroyArc(getArc(nodeA), graph);
// Correct arcs
for (arc = getArc(nodeB); arc != NULL; arc = getNextArc(arc)) {
if (getDestination(arc) != twinB)
createAnalogousArc(nodeA, getDestination(arc),
arc, graph);
else
createAnalogousArc(nodeA, twinA, arc, graph);
}
// Passage marker management in node A:
for (marker = getMarker(nodeA); marker != NULL_IDX;
marker = getNextInNode(marker))
if (isTerminal(marker))
incrementFinishOffset(marker,
getNodeLength(nodeB));
// Swapping new born passageMarkers from B to A
for (marker = getMarker(nodeB); marker != NULL_IDX; marker = tmpMarker) {
tmpMarker = getNextInNode(marker);
if (isInitial(marker)
|| getNode(getPreviousInSequence(marker)) != nodeA) {
extractPassageMarker(marker);
transposePassageMarker(marker, nodeA);
incrementFinishOffset(getTwinMarker(marker),
getNodeLength(nodeA));
} else
disconnectNextPassageMarker(getPreviousInSequence
(marker), graph);
}
// Read starts
concatenateReadStarts(nodeA, nodeB, graph);
// Gaps
appendNodeGaps(nodeA, nodeB, graph);
// Descriptor management (node)
appendDescriptors(nodeA, nodeB);
// Update uniqueness:
setUniqueness(nodeA, getUniqueness(nodeA) || getUniqueness(nodeB));
// Update virtual coverage
for (cat = 0; cat < CATEGORIES; cat++)
incrementVirtualCoverage(nodeA, cat,
getVirtualCoverage(nodeB, cat));
// Update original virtual coverage
for (cat = 0; cat < CATEGORIES; cat++)
incrementOriginalVirtualCoverage(nodeA, cat,
getOriginalVirtualCoverage
(nodeB, cat));
// Freeing gobbled node
destroyNode(nodeB, graph);
}
