bool Config::parseLine(const std::string& section, const std::string& line, std::ifstream& file, int& lineNb) { if(isKey(line, lineNb)) { //key = value extractLine(section, line, file, lineNb); } else if(isArray(line)) { //array extractArray(section, line, file, lineNb); } else { //else crap return false; } return true; }
void NCDParser::publishLaserMessage(const std::vector<std::string>& tokens, const std::string& laserFrame, const ros::Publisher& publisher) { ROS_DEBUG("Laser message"); sensor_msgs::LaserScan scan; double time = extractValue(tokens[3], "time="); scan.header.stamp = ros::Time(time); scan.header.frame_id = laserFrame; scan.angle_min = extractValue(tokens[3], "minAngle=") * DEG_TO_RAD; scan.angle_max = extractValue(tokens[3], "maxAngle=") * DEG_TO_RAD; scan.angle_increment = extractValue(tokens[3], "angRes=") * DEG_TO_RAD; scan.range_min = RANGE_MIN; scan.range_max = RANGE_MAX; scan.ranges = extractArray(tokens[3], "Range=[181]"); scan.intensities = extractArray(tokens[3], "Reflectance=[181]"); publisher.publish(scan); }
bool JsonParser::extractValue(JsonNode &node) { if (!extractWhitespace()) return false; switch (input[pos]) { case '\"': return extractString(node); case 'n' : return extractNull(node); case 't' : return extractTrue(node); case 'f' : return extractFalse(node); case '{' : return extractStruct(node); case '[' : return extractArray(node); case '-' : return extractFloat(node); default: { if (input[pos] >= '0' && input[pos] <= '9') return extractFloat(node); return error("Value expected!"); } } }
void NCDParser::publishTfMessages(const std::vector<std::string>& tokens) { ROS_DEBUG("Tf message"); // extract time double time = extractValue(tokens[3], "time="); // extract x, y, theta std::vector<float> xytheta = extractArray(tokens[3], "Pose=[3x1]"); double x = xytheta[0]; double y = xytheta[1]; double z = 0.0; double theta = xytheta[2]; // extract Pitch double pitch = extractValue(tokens[3], "Pitch="); // extract Roll double roll = extractValue(tokens[3], "Roll="); btQuaternion rotation; rotation.setRPY (roll, pitch, theta); worldToOdom_.setRotation (rotation); btVector3 origin; origin.setValue (x, y, z); worldToOdom_.setOrigin (origin); tf::StampedTransform worldToOdomStamped(worldToOdom_, ros::Time(time), worldFrame_, odomFrame_); tfBroadcaster_.sendTransform(worldToOdomStamped); tf::StampedTransform odomToLeftLaserStamped(odomToLeftLaser_, ros::Time(time), odomFrame_, leftLaserFrame_); tfBroadcaster_.sendTransform(odomToLeftLaserStamped); tf::StampedTransform odomToRightLaserStamped(odomToRightLaser_, ros::Time(time), odomFrame_, rightLaserFrame_); tfBroadcaster_.sendTransform(odomToRightLaserStamped); }