void LLHUDNameTag::addLabel(const std::string& label_utf8)
{
LLWString wstr = utf8string_to_wstring(label_utf8);
if (!wstr.empty())
{
LLWString seps(utf8str_to_wstring("\r\n"));
LLWString empty;
typedef boost::tokenizer<boost::char_separator<llwchar>, LLWString::const_iterator, LLWString > tokenizer;
boost::char_separator<llwchar> sep(seps.c_str(), empty.c_str(), boost::keep_empty_tokens);
tokenizer tokens(wstr, sep);
tokenizer::iterator iter = tokens.begin();
while (iter != tokens.end())
{
U32 line_length = 0;
do
{
S32 segment_length = mFontp->maxDrawableChars(iter->substr(line_length).c_str(),
HUD_TEXT_MAX_WIDTH, wstr.length(), LLFontGL::WORD_BOUNDARY_IF_POSSIBLE);
LLHUDTextSegment segment(iter->substr(line_length, segment_length), LLFontGL::NORMAL, mColor, mFontp);
mLabelSegments.push_back(segment);
line_length += segment_length;
}
while (line_length != iter->size());
++iter;
}
}
}
bool Include::AddFileList(const std::string &name, bool ignoreOk, bool MakeFile)
{
Eval e(name, false);
std::string iname = e.Evaluate();
bool rv = true;
CmdFiles cmdFiles;
std::string seps(" ");
seps += CmdFiles::PATH_SEP;
std::string includeDirs;
Variable *id = VariableContainer::Instance()->Lookup(".INCLUDE_DIRS");
if (id)
{
includeDirs = id->GetValue();
if (id->GetFlavor() == Variable::f_recursive)
{
Eval r(includeDirs, false);
includeDirs = r.Evaluate();
}
}
while (iname.size())
{
std::string current = Eval::ExtractFirst(iname, seps);
cmdFiles.AddFromPath(current, includeDirs);
}
for (CmdFiles::FileNameIterator it = cmdFiles.FileNameBegin(); rv && it != cmdFiles.FileNameEnd(); ++it)
{
Variable *v = VariableContainer::Instance()->Lookup("MAKEFILE_LIST");
if (!v)
{
v = new Variable(std::string("MAKEFILE_LIST"), *(*it), Variable::f_simple, Variable::o_file);
*VariableContainer::Instance() += v;
}
else
{
v->SetValue(v->GetValue() +" " + *(*it));
}
files.push_back(*(*it));
rv &= Parse(*(*it), ignoreOk | MakeFile, MakeFile);
}
return rv;
}
void LLHUDText::addLine(const std::string &text_utf8,
const LLColor4& color,
const LLFontGL::StyleFlags style,
const LLFontGL* font)
{
LLWString wline = utf8str_to_wstring(text_utf8);
if (!wline.empty())
{
// use default font for segment if custom font not specified
if (!font)
{
font = mFontp;
}
typedef boost::tokenizer<boost::char_separator<llwchar>, LLWString::const_iterator, LLWString > tokenizer;
LLWString seps(utf8str_to_wstring("\r\n"));
boost::char_separator<llwchar> sep(seps.c_str());
tokenizer tokens(wline, sep);
tokenizer::iterator iter = tokens.begin();
while (iter != tokens.end())
{
U32 line_length = 0;
do
{
F32 max_pixels = HUD_TEXT_MAX_WIDTH_NO_BUBBLE;
S32 segment_length = font->maxDrawableChars(iter->substr(line_length).c_str(), max_pixels, wline.length(), LLFontGL::WORD_BOUNDARY_IF_POSSIBLE);
LLHUDTextSegment segment(iter->substr(line_length, segment_length), style, color, font);
mTextSegments.push_back(segment);
line_length += segment_length;
}
while (line_length != iter->size());
++iter;
}
}
}
void ArduinoOnboard::arduinoDataCallback()
{
std::string msg;
// These are super expensive on the jetson
// m_nhPvt.getParam("srvBatteryCrit", srvBatteryCrit);
// m_nhPvt.getParam("srvBatteryLow", srvBatteryLow);
// m_nhPvt.getParam("camBatteryCrit", camBatteryCrit);
// m_nhPvt.getParam("camBatteryLow", camBatteryLow);
while(findMessage(msg))
{
//allocate new wheelSpeeds message
autorally_msgs::wheelSpeedsPtr wheelSpeeds(new autorally_msgs::wheelSpeeds);
wheelSpeeds->header.stamp = ros::Time::now();
boost::char_separator<char> seps(":,\n");
tokenizer tok(msg, seps);
tokenizer::iterator it=tok.begin();
//allocate new servo message for RC servoCommand
autorally_msgs::servoMSGPtr servos(new autorally_msgs::servoMSG);
servos->header.frame_id = "RC";
servos->backBrake = -5.0;
servos->frontBrake = -5.0;
double lf, rf, lb, rb;
bool wheels = false;
//bool servo = false;
//bool camera = false;
bool rc = false;
while(it!=tok.end())
{
if(*it == "wheels")
{
if(++it == tok.end()) break;
lf = atof(it->c_str());
if(++it == tok.end()) break;
rf = atof(it->c_str());
if(++it == tok.end()) break;
lb = atof(it->c_str());
if(++it == tok.end()) break;
rb = atof(it->c_str());
wheels = true;
//} else if(*it == "servo")
//{
// if(++it == tok.end()) break;
// m_port.diag("Servo Battery Voltage", *it);
// servo = true;
//} else if(*it == "camera")
//{
// if(++it == tok.end()) break;
// m_port.diag("Camera Battery Voltage", *it);
// camera = true;
} else if(*it == "rc")
{
if(++it == tok.end()) break;
servos->throttle = atof(it->c_str());
if(++it == tok.end()) break;
servos->steering = atof(it->c_str());
rc = true;
} else
{
NODELET_ERROR("ArduinoOnboard: Bad token %s", it->c_str());
m_port.diag_warn("ArduinoOnboard got a bad token");
}
if(it!=tok.end())
{
++it;
}
}
if(!(wheels && rc))
{
NODELET_ERROR("ArduinoOnboard: Incomplete packet %d %d", wheels, rc);
m_port.diag_warn("ArduinoOnboard: Incomplete packet");
}
if(isnan(lf)) lf = 0.0;
if(isnan(lb)) lb = 0.0;
if(isnan(rf)) rf = 0.0;
if(isnan(rb)) rb = 0.0;
wheelSpeeds->lfSpeed = (lf)*m_wheelDiameter*PI;
wheelSpeeds->lbSpeed = (lb)*m_wheelDiameter*PI;
wheelSpeeds->rfSpeed = (rf)*m_wheelDiameter*PI;
wheelSpeeds->rbSpeed = (rb)*m_wheelDiameter*PI;
//check if one of the front rotation sensors is disabled
if(m_lfEnabled && !m_rfEnabled)
{
wheelSpeeds->rfSpeed = wheelSpeeds->lfSpeed;
} else if(!m_lfEnabled && m_rfEnabled)
{
wheelSpeeds->lfSpeed = wheelSpeeds->rfSpeed;
}
//check if one of the back rotation sensors is disabled
if(m_lbEnabled && !m_rbEnabled)
{
wheelSpeeds->rbSpeed = wheelSpeeds->lbSpeed;
} else if(!m_lbEnabled && m_rbEnabled)
{
wheelSpeeds->lbSpeed = wheelSpeeds->rbSpeed;
}
//Calculate servo commands
//Raw data from arduino is in units of 500 ns.
servos->throttle /= 2.0;
servos->steering /= 2.0;
std::map<std::string, ServoSettings>::const_iterator mapIt;
if( (mapIt = m_servoSettings.find("steering")) != m_servoSettings.end())
{
if(servos->steering > mapIt->second.center)
{
servos->steering = (servos->steering - mapIt->second.center) / (mapIt->second.max-mapIt->second.center);
if (servos->steering > 1.0)
servos->steering = 1.0;
} else if(servos->steering < mapIt->second.center)
{
servos->steering = (servos->steering-mapIt->second.center)/(mapIt->second.center-mapIt->second.min);
if (servos->steering < -1.0)
servos->steering = -1.0;
} else
{
servos->steering = 0.0;
}
if(mapIt->second.reverse)
{
servos->steering = -servos->steering;
}
}
if( (mapIt = m_servoSettings.find("throttle")) != m_servoSettings.end())
{
if(servos->throttle > mapIt->second.center)
{
servos->throttle = (servos->throttle - mapIt->second.center) / (mapIt->second.max-mapIt->second.center);
if (servos->throttle > 1.0)
servos->throttle = 1.0;
} else if(servos->throttle < mapIt->second.center)
{
servos->throttle = (servos->throttle-mapIt->second.center)/(mapIt->second.center-mapIt->second.min);
if (servos->throttle < -1.0)
servos->throttle = -1.0;
} else
{
servos->throttle = 0.0;
}
if(mapIt->second.reverse)
{
servos->throttle = -servos->throttle;
}
}
servos->header.stamp = ros::Time::now();
if (m_lastTime + ros::Duration(2.0) > ros::Time::now()) {
m_lastTime = ros::Time::now();
//set FPS
int newFPS = 0;
m_nhPvt.getParam("triggerFPS", newFPS);
if(newFPS != m_triggerFPS)
{
m_triggerFPS = newFPS;
m_port.lock();
m_port.writePort("#fps:" + std::to_string(m_triggerFPS) + "\r\n");
m_port.unlock();
}
}
//publish data
m_wheelSpeedPub.publish(wheelSpeeds);
m_servoPub.publish(servos);
m_port.tick("arduinoData");
m_port.diag("Triggering FPS", std::to_string(m_triggerFPS));
}
}
