void GazeboRosSkidSteerDrive::cmdVelCallback(
const geometry_msgs::Twist::ConstPtr& cmd_msg) {
boost::mutex::scoped_lock scoped_lock(lock);
x_ = cmd_msg->linear.x;
rot_ = cmd_msg->angular.z;
}
int swd::database::save_parameter(const int& request_id, const std::string& path,
const std::string& value, const int& total_whitelist_rules,
const int& critical_impact, const int& threat) {
ensure_connection();
boost::unique_lock<boost::mutex> scoped_lock(dbi_mutex_);
char *path_esc = strdup(path.c_str());
dbi_conn_quote_string(conn_, &path_esc);
char *value_esc = strdup(value.c_str());
dbi_conn_quote_string(conn_, &value_esc);
dbi_result res = dbi_conn_queryf(conn_, "INSERT INTO parameters "
"(request_id, path, value, total_whitelist_rules, critical_impact, threat) "
"VALUES (%i, %s, %s, %i, %i, %i)", request_id, path_esc, value_esc,
total_whitelist_rules, critical_impact, threat);
free(path_esc);
free(value_esc);
if (!res) {
throw swd::exceptions::database_exception("Can't execute parameter query");
}
int id = dbi_conn_sequence_last(conn_, "parameters_id_seq");
dbi_result_free(res);
return id;
}
int swd::database::save_hash(const int& request_id, const std::string& algorithm,
const std::string& digest) {
ensure_connection();
boost::unique_lock<boost::mutex> scoped_lock(dbi_mutex_);
char *algorithm_esc = strdup(algorithm.c_str());
dbi_conn_quote_string(conn_, &algorithm_esc);
char *digest_esc = strdup(digest.c_str());
dbi_conn_quote_string(conn_, &digest_esc);
dbi_result res = dbi_conn_queryf(conn_, "INSERT INTO hashes (request_id, "
"algorithm, digest) VALUES (%i, %s, %s)", request_id, algorithm_esc, digest_esc);
free(algorithm_esc);
free(digest_esc);
if (!res) {
throw swd::exceptions::database_exception("Can't execute hash query");
}
int id = dbi_conn_sequence_last(conn_, "hashes_id_seq");
dbi_result_free(res);
return id;
}
JNIEnv *ThingsManagerJVM::getEnv()
{
std::unique_lock<std::mutex> scoped_lock(m_currentThreadMutex);
if (NULL == m_jvm)
{
LOGE("Failed to get JVM");
return NULL;
}
JNIEnv *env = NULL;
jint ret = m_jvm->GetEnv((void **)&env, JNI_CURRENT_VERSION);
switch (ret)
{
case JNI_OK:
return env;
case JNI_EDETACHED:
if (0 > m_jvm->AttachCurrentThread(&env, NULL))
{
LOGE("Failed to attach current thread to env");
return NULL;
}
return env;
case JNI_EVERSION:
LOGE("JNI version not supported");
default:
LOGE("Failed to get the environment");
return NULL;
}
}
void CInfoConsole::AddLineHelper (int priority, const char *text)
{
if (priority > verboseLevel)
return;
PUSH_CODE_MODE;
ENTER_MIXED;
boost::recursive_mutex::scoped_lock scoped_lock(infoConsoleMutex);
char text2[50];
if(strlen(text)>42){
memcpy(text2,text,42);
text2[42]=0;
} else
strcpy(text2,text);
(*filelog) << text2 << "\n";
filelog->flush();
InfoLine l;
if((int)data.size()>(numLines-1)){
data[1].time+=data[0].time;
data.pop_front();
}
data.push_back(l);
data.back().text=text2;
data.back().time=lifetime-lastTime;
lastTime=lifetime;
if(strlen(text)>42){
AddLine("%s",&text[42]);
}
POP_CODE_MODE;
}
void EventManager::freePool(BaseEventPool* pool)
{
boost::recursive_mutex::scoped_lock scoped_lock(mEventPoolMutex);
pool->clear();
mPoolQueue.push_back(pool);
}
// Update the controller
void YoubotArmController::UpdateChild()
{
boost::mutex::scoped_lock scoped_lock(lock);
if(cmd_mode_ == CMD_POSITION)
{
arm_joints_[0]->SetAngle(0, math::Angle(cmd_.q1));
arm_joints_[1]->SetAngle(0, math::Angle(cmd_.q2));
arm_joints_[2]->SetAngle(0, math::Angle(cmd_.q3));
arm_joints_[3]->SetAngle(0, math::Angle(cmd_.q4));
arm_joints_[4]->SetAngle(0, math::Angle(cmd_.q5));
}
else if(cmd_mode_ == CMD_VELOCITY)
{
arm_joints_[0]->SetVelocity(0, cmd_.q1);
arm_joints_[1]->SetVelocity(0, cmd_.q2);
arm_joints_[2]->SetVelocity(0, cmd_.q3);
arm_joints_[3]->SetVelocity(0, cmd_.q4);
arm_joints_[4]->SetVelocity(0, cmd_.q5);
}
else if(cmd_mode_ == CMD_TORQUE)
{
arm_joints_[0]->SetForce(0, cmd_.q1);
arm_joints_[1]->SetForce(0, cmd_.q2);
arm_joints_[2]->SetForce(0, cmd_.q3);
arm_joints_[3]->SetForce(0, cmd_.q4);
arm_joints_[4]->SetForce(0, cmd_.q5);
}
}
bool
GazeboRosIMU::SetAccelBiasCallback(thinc_sim_gazebo_plugins::SetBias::Request &req, thinc_sim_gazebo_plugins::SetBias::Response &res)
{
boost::mutex::scoped_lock scoped_lock(lock);
accelModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z));
return true;
}
void WorkerQueue::push(ServerWorker* worker){
//mutex lock (scoped)
boost::mutex::scoped_lock scoped_lock(mutex);
worker->add();
this->workers_list.push_back(worker);
}
Config* Config::Instance(){
boost::mutex::scoped_lock scoped_lock(instanceMutex);
if(theSingleInstance == NULL)
theSingleInstance = new Config();
return theSingleInstance;
}
void BCLog::Logger::LogPrintStr(const std::string &str)
{
std::string strTimestamped = LogTimestampStr(str);
if (m_print_to_console) {
// print to console
fwrite(strTimestamped.data(), 1, strTimestamped.size(), stdout);
fflush(stdout);
}
if (m_print_to_file) {
std::lock_guard<std::mutex> scoped_lock(m_file_mutex);
// buffer if we haven't opened the log yet
if (m_fileout == nullptr) {
m_msgs_before_open.push_back(strTimestamped);
}
else
{
// reopen the log file, if requested
if (m_reopen_file) {
m_reopen_file = false;
m_fileout = fsbridge::freopen(m_file_path, "a", m_fileout);
if (!m_fileout) {
return;
}
setbuf(m_fileout, nullptr); // unbuffered
}
FileWriteStr(strTimestamped, m_fileout);
}
}
}
int StatsTable::FindCounter(const std::string& name)
{
// Note: the API returns counters numbered from 1..N, although
// internally, the array is 0..N-1. This is so that we can return
// zero as "not found".
if(!impl_)
{
return 0;
}
// Create a scope for our auto-lock.
{
AutoLock scoped_lock(counters_lock_);
// Attempt to find the counter.
CountersMap::const_iterator iter;
iter = counters_.find(name);
if(iter != counters_.end())
{
return iter->second;
}
}
// Counter does not exist, so add it.
return AddCounter(name);
}
bool OpenDebugLog()
{
boost::call_once(&DebugPrintInit, debugPrintInitFlag);
boost::mutex::scoped_lock scoped_lock(*mutexDebugLog);
assert(fileout == nullptr);
assert(vMsgsBeforeOpenLog);
fs::path pathDebug = GetDebugLogPath();
fileout = fsbridge::fopen(pathDebug, "a");
if (!fileout) {
return false;
}
setbuf(fileout, nullptr); // unbuffered
// dump buffered messages from before we opened the log
while (!vMsgsBeforeOpenLog->empty()) {
FileWriteStr(vMsgsBeforeOpenLog->front(), fileout);
vMsgsBeforeOpenLog->pop_front();
}
delete vMsgsBeforeOpenLog;
vMsgsBeforeOpenLog = nullptr;
return true;
}
char *Trigger::getTriggers() {
std::stringstream ss;
{
boost::recursive_mutex::scoped_lock scoped_lock(trigger_list_mutex);
std::list<Trigger*>::iterator iter = all_triggers.begin();
while (iter != all_triggers.end()) {
Trigger *t = *iter++;
ss << t->getName() << " (" << t->refs;
if (t->_internals->holders.size()) {
ss << ":";
std::list<Action*>::iterator h_iter = t->_internals->holders.begin();
while (h_iter != t->_internals->holders.end()) {
ss << *(*h_iter++) << " ";
}
}
ss << ")\n";
}
}
if (!ss.str().length()) return 0;
size_t len = ss.str().length();
char *res = new char[len+1];
strncpy(res, ss.str().c_str(), len);
res[len] = 0;
return res;
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboPanel::Update()
{
boost::mutex::scoped_lock scoped_lock(lock);
if ( terminated_ )
{
return;
}
common::Time current_time = world_->GetSimTime();
// check score
double angle = joint_->GetAngle(0).Radian();
if ( fmod(current_time.Double(), 1) == 0 ) {
ROS_INFO_STREAM("Current time is " << current_time.Double() << ", Current angle is = " << angle);
}
// void Entity::GetNearestEntityBelow(double &_distBelow, std::string &_entityName)
//
std::stringstream ss;
std_msgs::String msg_time;
ss << 20*60 - current_time.Double();
msg_time.data = ss.str();
pub_time_.publish(msg_time);
if ( fabs(angle) > 3.14)
{
std_msgs::String msg_score, msg_time;
msg_score.data = "Mission Completed";
ROS_INFO_STREAM("Remaining time is " << msg_time.data << "[sec], Score is " << msg_score.data);
pub_score_.publish(msg_score);
terminated_ = true;
}
}
ServerWorker* WorkerQueue::pop(){
//mutex lock (scoped)
boost::mutex::scoped_lock scoped_lock(mutex);
ServerWorker* worker = this->workers_list.front();
this->workers_list.erase(this->workers_list.end()-1);
return worker;
}
void GazeboRosCarDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg ) {
boost::mutex::scoped_lock scoped_lock ( lock );
base_speed_cmd_ = cmd_msg->linear.x;
base_omega_cmd_ = cmd_msg->angular.z;
}
int CNet::SendData(unsigned char *data, int length,int connection)
{
if(playbackDemo && !serverNet)
return 1;
if(length<=0)
logOutput.Print("Errenous send length in SendData %i",length);
Connection* c=&connections[connection];
if(c->active) {
if(c->localConnection) {
boost::mutex::scoped_lock scoped_lock(netMutex);
Connection* lc=c->localConnection;
if(lc->readyLength+length>=NETWORK_BUFFER_SIZE) {
logOutput.Print("Overflow when sending to local connection %i %i %i %i %i",imServer,connection,lc->readyLength,length,NETWORK_BUFFER_SIZE);
return 0;
}
memcpy(&lc->readyData[lc->readyLength],data,length);
lc->readyLength+=length;
} else {
if(c->outgoingLength+length>=NETWORK_BUFFER_SIZE) {
logOutput.Print("Overflow when sending to remote connection %i %i %i %i %i",imServer,connection,c->outgoingLength,length,NETWORK_BUFFER_SIZE);
return 0;
}
memcpy(&c->outgoingData[c->outgoingLength],data,length);
c->outgoingLength+=length;
}
}
return 1;
}
// ============================================================================
// AcquisitionTask::set_state
// ============================================================================
void AcquisitionTask::set_state(Tango::DevState state)
{
{
yat::MutexLock scoped_lock(m_status_lock);
m_state = state;
}
}
void swd::log::open_file(std::string file) {
/* Use mutex to avoid race conditions. */
boost::unique_lock<boost::mutex> scoped_lock(mutex_);
/* Set the file. */
file_ = file;
}
//this is called all the way from the receiver threads, we should process this swiftly
//and keep in mind that it is called crazily and concurently from lots of threads
//we MUST copy the data too
//also, this function can be called by threads owning clients_mutex !!!
void Broadcaster::QueueMessage(int type, int uid, unsigned int streamid, unsigned int len, const char *data) {
if (!m_is_running) {
return;
}
queue_entry_t msg = {type, uid, streamid, len, ""};
memcpy(msg.data, data, len);
{
MutexLocker scoped_lock(m_queue_mutex);
if (m_msg_queue.empty()) {
m_packet_drop_counter = 0;
m_is_dropping_packets = (++m_packet_good_counter > 3) ? false : m_is_dropping_packets;
} else if (type == RoRnet::MSG2_STREAM_DATA_DISCARDABLE) {
auto search = std::find_if(m_msg_queue.begin(), m_msg_queue.end(), [&](const queue_entry_t& m)
{ return m.type == RoRnet::MSG2_STREAM_DATA_DISCARDABLE && m.uid == uid && m.streamid == streamid; });
if (search != m_msg_queue.end()) {
// Found outdated discardable streamdata -> replace it
(*search) = msg;
m_packet_good_counter = 0;
m_is_dropping_packets = (++m_packet_drop_counter > 3) ? true : m_is_dropping_packets;
Messaging::StatsAddOutgoingDrop(sizeof(RoRnet::Header) + msg.datalen); // Statistics
return;
}
}
m_msg_queue.push_back(msg);
}
m_queue_cond.signal();
}
swd::blacklist_filters swd::database::get_blacklist_filters() {
swd::log::i()->send(swd::notice, "Get blacklist filters from db");
ensure_connection();
boost::unique_lock<boost::mutex> scoped_lock(dbi_mutex_);
dbi_result res = dbi_conn_query(conn_, "SELECT id, impact, rule FROM blacklist_filters");
if (!res) {
throw swd::exceptions::database_exception("Can't execute blacklist_filters query");
}
swd::blacklist_filters filters;
while (dbi_result_next_row(res)) {
swd::blacklist_filter_ptr filter(new swd::blacklist_filter());
filter->set_id(dbi_result_get_uint(res, "id"));
filter->set_impact(dbi_result_get_uint(res, "impact"));
filter->set_regex(dbi_result_get_string(res, "rule"));
filters.push_back(filter);
}
dbi_result_free(res);
return filters;
}
////////////////////////////////////////////////////////////////////////////////
// returns true always, imu is always calibrated in sim
bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
boost::mutex::scoped_lock scoped_lock(lock);
rateModel.reset();
return true;
}
bool swd::database::is_flooding(const std::string& client_ip,
const int& profile_id) {
ensure_connection();
boost::unique_lock<boost::mutex> scoped_lock(dbi_mutex_);
char *client_ip_esc = strdup(client_ip.c_str());
dbi_conn_quote_string(conn_, &client_ip_esc);
dbi_result res = dbi_conn_queryf(conn_, "SELECT is_flooding(%i, %s) AS result",
profile_id, client_ip_esc);
free(client_ip_esc);
if (!res) {
throw swd::exceptions::database_exception("Can't execute request count query");
}
bool flooding = false;
if (dbi_result_get_numrows(res) == 1) {
if (!dbi_result_next_row(res)) {
throw swd::exceptions::database_exception("No flooding?");
}
flooding = (dbi_result_get_uint(res, "result") == 1);
}
dbi_result_free(res);
return flooding;
}
sp<IAppOpsService> AppOpsManager::getService()
{
std::lock_guard<Mutex> scoped_lock(mLock);
int64_t startTime = 0;
sp<IAppOpsService> service = mService;
while (service == NULL || !IInterface::asBinder(service)->isBinderAlive()) {
sp<IBinder> binder = defaultServiceManager()->checkService(_appops);
if (binder == NULL) {
// Wait for the app ops service to come back...
if (startTime == 0) {
startTime = uptimeMillis();
ALOGI("Waiting for app ops service");
} else if ((uptimeMillis()-startTime) > 10000) {
ALOGW("Waiting too long for app ops service, giving up");
service = NULL;
break;
}
sleep(1);
} else {
service = interface_cast<IAppOpsService>(binder);
mService = service;
}
}
return service;
}
void CInfoConsole::Draw()
{
boost::recursive_mutex::scoped_lock scoped_lock(infoConsoleMutex);
glPushMatrix();
glDisable(GL_TEXTURE_2D);
glColor4f(0,0,0.5f,0.5f);
if(!data.empty()){
glBegin(GL_TRIANGLE_STRIP);
glVertex3f(xpos,ypos,0);
glVertex3f(xpos+width,ypos,0);
glVertex3f(xpos,ypos-height,0);
glVertex3f(xpos+width,ypos-height,0);
glEnd();
}
glTranslatef(xpos+0.01,ypos-0.026f,0);
glScalef(0.015f,0.02f,0.02f);
glColor4f(1,1,1,1);
glEnable(GL_TEXTURE_2D);
std::deque<InfoLine>::iterator ili;
for(ili=data.begin();ili!=data.end();ili++){
font->glPrint("%s",ili->text.c_str());
glTranslatef(0,-1.2f,0);
}
glPopMatrix();
}
void CGameServer::UpdateLoop(void)
{
while(gameLoading){ //avoid timing out while loading (esp if calculating path data)
SDL_Delay(100);
serverNet->Update();
}
SDL_Delay(100); //we might crash if game hasnt finished initializing within this time
/*
* Need a better solution than this for starvation.
* Decreasing thread priority (making it more important)
* requires root privileges on POSIX systems
*/
//SetThreadPriority(thisThread,THREAD_PRIORITY_ABOVE_NORMAL); //we want the server to continue running smoothly even if the game client is struggling
while(!quitServer)
{
{
boost::mutex::scoped_lock scoped_lock(gameServerMutex);
if(!Update()){
info->AddLine("Game server experienced an error in update");
globalQuit=true;
}
}
SDL_Delay(10);
}
terminate = true;
}
void CInfoConsole::NotifyLogMsg(CLogSubsystem& subsystem, const char* text)
{
if (!font) return;
PUSH_CODE_MODE;
ENTER_MIXED;
boost::recursive_mutex::scoped_lock scoped_lock(infoConsoleMutex);
RawLine rl(text, &subsystem, rawId);
rawId++;
rawData.push_back(rl);
if (rawData.size() > maxRawLines) {
rawData.pop_front();
}
if (newLines < maxRawLines) {
newLines++;
}
float maxWidth = 25.0f;
int pos=0, line_start=0;
while (text[pos]) {
// iterate through text until maxWidth width is reached
char temp[120];
float w = 0.0f;
for (;text[pos] && pos-line_start < sizeof(temp) - 1 && w <= maxWidth;pos++) {
w += font->CalcCharWidth(text[pos]);
temp[pos-line_start] = text[pos];
}
temp[pos-line_start] = 0;
// if needed, find a breaking position
if (w > maxWidth) {
int break_pos = pos-line_start;
while (break_pos >= 0 && temp[break_pos] != ' ')
break_pos --;
if (break_pos <= 0) break_pos = pos-line_start;
line_start += break_pos + (temp[break_pos] == ' ' ? 1 : 0);
pos = line_start;
temp[break_pos] = 0;
} else {
line_start = pos;
}
// add the line to the console
InfoLine l;
if((int)data.size()>(numLines-1)){
data[1].time+=data[0].time;
data.pop_front();
}
data.push_back(l);
data.back().text=temp;
data.back().time=lifetime-lastTime;
lastTime=lifetime;
}
POP_CODE_MODE;
}
void GazeboRosVideo::processImage(const sensor_msgs::ImageConstPtr &msg)
{
// Get a reference to the image from the image message pointer
boost::mutex::scoped_lock scoped_lock(m_image_);
// We get image with alpha channel as it allows memcpy onto ogre texture
image_ = cv_bridge::toCvCopy(msg, "bgra8");
new_image_available_ = true;
}
CInfoConsole& CInfoConsole::operator<< (float f)
{
boost::recursive_mutex::scoped_lock scoped_lock(infoConsoleMutex);
char t[50];
sprintf(t,"%f ",f);
tempstring+=t;
return *this;
}
