void processSerialCallback(const msgs::serial::ConstPtr& rx_msg) { boost::char_separator<char> sep("$*,"); tokenizer::iterator tok_iter; tokenizer tokens(rx_msg->data, sep); if(tokens.begin() != tokens.end()) { tok_iter = tokens.begin(); // get NMEA identifier if((*tok_iter).compare("VNQMR") == 0) { processIMU(tokens,rx_msg->data); } else { ROS_INFO("Ignoring Unknown NMEA identifier %s",(*tok_iter).c_str()); } } }
bool imuIdentifierThread::processWayPoint() { processIMU(); if (wayPoints[currentWaypoint].name == "START " || wayPoints[currentWaypoint].name == "END " || wayPoints[currentWaypoint].name == "MIDDLE " ) { if (posCtrlFlag) { printMessage(1,"Putting head in home position..\n"); goHome(); posCtrlFlag = false; } if (yarp::os::Time::now() - timeNow > CTRL_PERIOD) { posCtrlFlag = true; return false; } } else { Vector jls = wayPoints[currentWaypoint].jntlims; Vector vls = wayPoints[currentWaypoint].vels; bool flag = false; iencsH->getEncoders(encsH->data()); yarp::sig::Vector head = *encsH; // ivelH -> velocityMove(vls.data()); VectorOf<int> jointsToSet; jointsToSet.push_back(0); jointsToSet.push_back(1); jointsToSet.push_back(2); ivelH -> velocityMove(jointsToSet.size(), jointsToSet.getFirst(), vls.data()); for (int i = 0; i < 3; i++) { if (vls(i) > 0.0) { if (jls(i) - head(i) > 0.0) { flag = true; } } else if (vls(i) < 0.0) { if (jls(i) - head(i) < 0.0) { flag = true; } } } return flag; } return true; }