BlobFollower::BlobFollower(int argc, char *argv[]) {
ros::init(argc, argv, "blob_follower"); //name of node
ros::NodeHandle handle; //the handle
std::string blob_sub_topic;
if (!handle.getParam("/topic_list/vision_topics/pixel_detect/published/blob", blob_sub_topic)){
ROS_ERROR("/topic_list/vision_topics/pixel_detect/published/blob is not defined in node %s!", ros::this_node::getName().c_str());
std::exit(1);
}
std::string twist_pub_topic;
if (!handle.getParam("/topic_list/controller_topics/motor2/subscribed/twist_topic", twist_pub_topic)){
ROS_ERROR("/topic_list/controller_topics/motor2/subscribed/twist_topic is not defined!");
std::exit(1);
}
pub_motor = handle.advertise<geometry_msgs::Twist>(twist_pub_topic, 1000);
sub_rgbd = handle.subscribe(blob_sub_topic, 1000, &BlobFollower::rgbdCallback, this);
ROSUtil::getParam(handle, "/blob_follower/desired_depth", desiredDepth);
ROSUtil::getParam(handle, "/blob_follower/width_tolerance", widthTolerance);
ROSUtil::getParam(handle, "/blob_follower/depth_tolerance", depthTolerance);
ROSUtil::getParam(handle, "/blob_follower/linear_speed", linearSpeed);
ROSUtil::getParam(handle, "/blob_follower/angular_speed", angularSpeed);
runNode();
}
SegmentStitching::SegmentStitching(int argc, char *argv[]) {
ros::init(argc, argv, "segment_stitching");
ros::NodeHandle handle;
std::string segmentTopic;
ROSUtil::getParam(handle, "/topic_list/mapping_topics/segment_storage/bag_segment_topic",
segmentTopic);
std::string segmentFile;
ROSUtil::getParam(handle, "/segment_stitching/input_file",
segmentFile);
ROSUtil::getParam(handle, "/segment_stitching/ransac_threshold", ransacThreshold);
segcloud_pub = handle.advertise<pcl::PointCloud<pcl::PointXYZ> >("/segment_stitching/segcloud", 1);
linemarker_pub = handle.advertise<visualization_msgs::Marker>("/segment_stitching/linemarkers", 1);
markerArray_pub = handle.advertise<visualization_msgs::MarkerArray>("/segment_stitching/markerarray", 1);
std::string lineTopic;
ROSUtil::getParam(handle, "/topic_list/mapping_topics/segment_stitching/published/line_topic",
lineTopic);
line_pub = handle.advertise<mapping_msgs::SegmentLineVector>(lineTopic, 1);
std::string objectTopic;
ROSUtil::getParam(handle, "/topic_list/mapping_topics/segment_stitching/published/object_topic",
objectTopic);
object_pub = handle.advertise<mapping_msgs::SegmentObjectVector>(objectTopic, 1);
// Initialise the locations of the IR sensors relative to the centre of the robot.
populateSensorPositions(handle);
// open the bag to read it
segmentBag.open(segmentFile, rosbag::bagmode::Read);
// define the topics to read
std::vector<std::string> topics;
topics.push_back(segmentTopic);
// define a view onto the bag file - only interested in one topic
rosbag::View view(segmentBag, rosbag::TopicQuery(topics));
ROS_INFO("Reading segments from %s", segmentFile.c_str());
// Iterate over messages in the bag
for (rosbag::View::iterator it = view.begin(); it != view.end(); it++){
// extract the messageInstance that the iterator points to
rosbag::MessageInstance mi = *it;
// Extract the segment from the bag
mapping_msgs::MapSegment segment = *(mi.instantiate<mapping_msgs::MapSegment>());
// Put it into the vector of segments
mapSegments.push_back(segment);
}
ROS_INFO("Number of segments: %d", (int)mapSegments.size());
runNode();
}
///////////////////////////////////////////////////////////////
// rerun is to do a rerun
// Takes the elements of the queue, if there is no more
// work it breaks, if it is a copy event is calculates the
// degree puts it in the node and makes sure the node has
// a child, and if it is a run event it runs the wake up
// code
/////////////////////////////////////////////////////////////
cluster* rerun(Queue *q,tree_t* tree, int doSynch)
{
cluster* root;
node *nd = NULL;
int curheight = -1;
deprintf(" \n Rerunning \n");
currentTree = tree;
currentQueue = q;
morework=1;
//debug=1;
while (!isEmpty(q)) {
// Loop
// Free the qnode
drun(assert(!isEmpty(q)));
nd = removeQueue(q);
if(!(nd->deleted)) {
//If it is the end of a round then check if all is done, otherwise continue, resetting
//the morework flag
if(nd->height != curheight)
{
curheight = nd->height;
deprintf("Height is %d\n",curheight);
if(morework == 0) break;
morework = 0;
}
deprintf("Running %d\n",nd->nId);
runNode(nd,q);
drun(assert(verifyVertex(nd)));
}
}//while
// Empty the queue
emptyQueue(q);
if(PUSHDOWN)
{
deprintf("push down\n");
pushDownList(&oldRootList);
}
if(doSynch) {
root = syncList(&newRootList);
}
return root;
}
MotorController2::MotorController2(int argc, char *argv[]){
ros::init(argc, argv, "motor_controller2");
ros::NodeHandle n;
v = 0.0;
w = 0.0;
std::string pwm_pub_topic;
ROSUtil::getParam(n, "/topic_list/robot_topics/subscribed/pwm_topic", pwm_pub_topic);
std::string encoder_sub_topic;
ROSUtil::getParam(n, "/topic_list/robot_topics/published/encoder_topic", encoder_sub_topic);
std::string twist_sub_topic;
ROSUtil::getParam(n, "/topic_list/controller_topics/motor2/subscribed/twist_topic", twist_sub_topic);
chatter_pub = n.advertise<ras_arduino_msgs::PWM>(pwm_pub_topic, 1000);
sub_feedback = n.subscribe(encoder_sub_topic, 1000,&MotorController2::feedbackCallback, this);
sub_setpoint = n.subscribe(twist_sub_topic, 1000,&MotorController2::setpointCallback, this);
ROSUtil::getParam(n, "/controller2/Gp_L", Gp_L);
ROSUtil::getParam(n, "/controller2/Gp_R", Gp_R);
ROSUtil::getParam(n, "/controller2/Gi_L", Gi_L);
ROSUtil::getParam(n, "/controller2/Gi_R", Gi_R);
ROSUtil::getParam(n, "/controller2/Gd_L", Gd_L);
ROSUtil::getParam(n, "/controller2/Gd_R", Gd_R);
ROSUtil::getParam(n, "/controller2/Gc_L", Gc_L);
ROSUtil::getParam(n, "/controller2/Gc_R", Gc_R);
/*
ROSUtil::getParam(n, "/controller_turn/Gp_L", Gp_L);
ROSUtil::getParam(n, "/controller_turn/Gp_R", Gp_R);
ROSUtil::getParam(n, "/controller_turn/Gi_L", Gi_L);
ROSUtil::getParam(n, "/controller_turn/Gi_R", Gi_R);
ROSUtil::getParam(n, "/controller_turn/Gd_R", Gd_R);
ROSUtil::getParam(n, "/controller_turn/Gc_L", Gc_L);
ROSUtil::getParam(n, "/controller_turn/Gc_R", Gc_R);*/
ROSUtil::getParam(n, "/controller_turn/control_freq", control_frequency);
ROSUtil::getParam(n, "/controller_turn/control_time", control_time);
ROSUtil::getParam(n, "/controller2/control_freq", control_frequency);
ROSUtil::getParam(n, "/controller2/control_time", control_time);
ROSUtil::getParam(n, "/robot_info/ticks_per_rev", ticks_per_rev);
ROSUtil::getParam(n, "/robot_info/wheel_baseline", b);
ROSUtil::getParam(n, "/robot_info/wheel_radius", r);
ROSUtil::getParam(n, "/controller2/satMin", satMIN);
ROSUtil::getParam(n, "/controller2/satMax", satMAX);
runNode();
}
bool ZLGtkSelectionDialog::run() {
while (gtk_dialog_run(myDialog) == GTK_RESPONSE_ACCEPT) {
if (myNodeSelected || handler().isOpenHandler()) {
GtkTreeSelection *selection = gtk_tree_view_get_selection(myView);
GtkTreeModel *dummy;
GtkTreeIter iter;
if (gtk_tree_selection_get_selected(selection, &dummy, &iter)) {
int index;
gtk_tree_model_get(GTK_TREE_MODEL(myStore), &iter, 2, &index, -1);
const std::vector<ZLTreeNodePtr> &nodes = handler().subnodes();
if ((index >= 0) && (index < (int)nodes.size())) {
runNode(nodes[index]);
}
}
myNodeSelected = false;
} else {
runState(gtk_entry_get_text(myStateLine));
}
if (myExitFlag) {
return true;
}
}
return false;
}
void HashLife::run()
{
QTime timer;
timer.start();
m_running = true;
m_writeLock->lock();
m_readLock->lock();
while (m_increment + 2 > m_depth)
expand();
Node *e = emptyNode(m_depth - 2);
// Test boundary to be empty, or we have to expand() to guarantee the result fits in the boundary
// This requires m_depth to be at least Block::DEPTH + 2
if ((m_root->ul->ul != e || m_root->ul->ur != e || m_root->ul->dl != e)
|| (m_root->ur->ul != e || m_root->ur->ur != e || m_root->ur->dr != e)
|| (m_root->dl->ul != e || m_root->dl->dl != e || m_root->dl->dr != e)
|| (m_root->dr->ur != e || m_root->dr->dl != e || m_root->dr->dr != e))
expand();
// To make the depth of RESULT equal to m_depth, we first expand the universe
// This is nearly identical to code in expand() except we don't need to touch m_x and m_y
e = emptyNode(m_depth - 1);
Node *nul = m_nodeHash->get(e, e, e, m_root->ul);
Node *nur = m_nodeHash->get(e, e, m_root->ur, e);
Node *ndl = m_nodeHash->get(e, m_root->dl, e, e);
Node *ndr = m_nodeHash->get(m_root->dr, e, e, e);
Node *nroot = m_nodeHash->get(nul, nur, ndl, ndr);
m_readLock->unlock();
Node *new_root = runNode(nroot, m_depth + 1);
m_readLock->lock();
m_root = new_root;
m_readLock->unlock();
m_writeLock->unlock();
m_generation += BigInteger::exp2(m_increment);
m_running = false;
emit gridChanged();
qDebug() << timer.elapsed();
}
void ZLQtSelectionDialog::runNodeSlot() {
QListViewItem *item = myListView->currentItem();
if (item != 0) {
runNode(((ZLQtSelectionDialogItem*)item)->node());
}
}
BlobDetectorNode::BlobDetectorNode(int argc, char* argv[]) {
ros::NodeHandle handle = nodeSetup(argc, argv);
runNode(handle);
}
MotorController::MotorController(int argc, char *argv[]){
ros::NodeHandle handle = nodeSetup(argc, argv);
runNode(handle);
}
Node *HashLife::runNode(Node *node, size_t depth)
{
if (node->result)
return node->result;
if (depth == Block::DEPTH + 1)
{
uchar nul, nur, ndl, ndr;
Block::runStep(AlgorithmManager::rule(), nul, nur, ndl, ndr, reinterpret_cast<Block *>(node->ul), reinterpret_cast<Block *>(node->ur), reinterpret_cast<Block *>(node->dl), reinterpret_cast<Block *>(node->dr));
return node->result = reinterpret_cast<Node *>(m_blockHash->get(nul, nur, ndl, ndr));
}
else
{
// HashLife runStep routine
// 0 0 0 0 0 0 0 0
// 0 a a b b c c 0
// 0 a A B B C c 0
// 0 d D E E F f 0
// 0 d D E E F f 0
// 0 g G H H I i 0
// 0 g g h h i i 0
// 0 0 0 0 0 0 0 0
// 1. Calculate 9 sub-nodes
Node *a = runNode(node->ul, depth - 1);
Node *b = runNode(m_nodeHash->get(node->ul->ur, node->ur->ul, node->ul->dr, node->ur->dl), depth - 1);
Node *c = runNode(node->ur, depth - 1);
Node *d = runNode(m_nodeHash->get(node->ul->dl, node->ul->dr, node->dl->ul, node->dl->ur), depth - 1);
Node *e = runNode(m_nodeHash->get(node->ul->dr, node->ur->dl, node->dl->ur, node->dr->ul), depth - 1);
Node *f = runNode(m_nodeHash->get(node->ur->dl, node->ur->dr, node->dr->ul, node->dr->ur), depth - 1);
Node *g = runNode(node->dl, depth - 1);
Node *h = runNode(m_nodeHash->get(node->dl->ur, node->dr->ul, node->dl->dr, node->dr->dl), depth - 1);
Node *i = runNode(node->dr, depth - 1);
if (m_increment + 2 < depth) // no need to do more increment
{
if (depth == Block::DEPTH + 2) // 9 sub-nodes are actually blocks
{
Block *A = reinterpret_cast<Block *>(a);
Block *B = reinterpret_cast<Block *>(b);
Block *C = reinterpret_cast<Block *>(c);
Block *D = reinterpret_cast<Block *>(d);
Block *E = reinterpret_cast<Block *>(e);
Block *F = reinterpret_cast<Block *>(f);
Block *G = reinterpret_cast<Block *>(g);
Block *H = reinterpret_cast<Block *>(h);
Block *I = reinterpret_cast<Block *>(i);
Node *nul = reinterpret_cast<Node *>(m_blockHash->get(A->dr, B->dl, D->ur, E->ul));
Node *nur = reinterpret_cast<Node *>(m_blockHash->get(B->dr, C->dl, E->ur, F->ul));
Node *ndl = reinterpret_cast<Node *>(m_blockHash->get(D->dr, E->dl, G->ur, H->ul));
Node *ndr = reinterpret_cast<Node *>(m_blockHash->get(E->dr, F->dl, H->ur, I->ul));
return node->result = m_nodeHash->get(nul, nur, ndl, ndr);
}
Node *nul = m_nodeHash->get(a->dr, b->dl, d->ur, e->ul);
Node *nur = m_nodeHash->get(b->dr, c->dl, e->ur, f->ul);
Node *ndl = m_nodeHash->get(d->dr, e->dl, g->ur, h->ul);
Node *ndr = m_nodeHash->get(e->dr, f->dl, h->ur, i->ul);
return node->result = m_nodeHash->get(nul, nur, ndl, ndr);
}
// else use the full increment power
// 2. Calculate final RESULT
Node *nul = runNode(m_nodeHash->get(a, b, d, e), depth - 1);
Node *nur = runNode(m_nodeHash->get(b, c, e, f), depth - 1);
Node *ndl = runNode(m_nodeHash->get(d, e, g, h), depth - 1);
Node *ndr = runNode(m_nodeHash->get(e, f, h, i), depth - 1);
return node->result = m_nodeHash->get(nul, nur, ndl, ndr);
}
}
void QOpenFileDialog::accept() {
runNode(((QOpenFileDialogItem*)myListView->currentItem())->node());
}
void runNode(void *data) {
MoveNode *currNode = (MoveNode *)data;
MoveNode *currChild = currNode->child;
if (currNode->nodeId == NULL) {
return;
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
Sensor *sensor = &pairMap[currNode->nPairId].sensors[i];
// createSensor(sensor);
/*if (sensor->type == SHAFT_ENCODER) {
sensor->enc = encoderInit(sensor->port, sensor->port + 1, false);
}*/
startSensor(sensor);
sensor = NULL;
}
signed char *saveState = NULL;
printDebug("Started node.");
// printf("Started node %d.\n\r", currNode->nodeId);
// printf("Node's child: %d\n\r", currChild->nodeId);
int nextPoint = 0;
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
while (nextPoint < currNode->numPoints) {
// printf("DEBUG: %d\n\r", nextPoint);
while (joystickGetDigital(1, 5, JOY_UP)) { // pause
if (saveState == NULL) {
saveState = malloc(pairMap[currNode->nPairId].numPorts * sizeof(*(saveState)));
if (saveState == NULL) {
return; // break completely
}
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
saveState[i] = motorGet(pairMap[currNode->nPairId].motorPorts[i]);
motorStop(pairMap[currNode->nPairId].motorPorts[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
pauseTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
printDebug("Paused!");
}
if (joystickGetDigital(1, 8, JOY_DOWN)) { // stop it entirely
printDebug("Stopping!");
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
return;
}
delay(20);
}
if (outOfMemory) {
return;
}
if (saveState != NULL) {
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
motorSet(pairMap[currNode->nPairId].motorPorts[i], saveState[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
}
if (reachedPoint(currNode, currNode->points[nextPoint].endSensorVal, currNode->points[nextPoint - 1].endSensorVal)) {
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
}
if (currChild != NULL) {
// printf("DEBUG: %d %d %d\n\r", currChild->nodeId, nextPoint, currChild->startPoint);
if (nextPoint + 1 >= currChild->startPoint && needToStart(currNode, currChild)) {
void *param = (void *)currChild;
taskCreate(runNode, TASK_DEFAULT_STACK_SIZE / 2, param, TASK_PRIORITY_DEFAULT);
currChild = currChild->sibling;
}
}
delay(5);
}
printDebug("Finished node.");
// printf("Finished node %d.", currNode->nodeId);
if (findParent(currNode)->nodeId == rootNode->nodeId) {
while (inMotion()) {
delay(20);
}
if (currNode->sibling != NULL) {
delay(currNode->sibling->startVal[0]);
runNode(currNode->sibling);
} else {
printDebug("Done.");
delay(500);
}
}
}
