void append_number(int x)
{
while (lock_stream.test_and_set()) {
}
stream << "thread #" << x << '\n';
lock_stream.clear();
}
int main()
#endif
{
gViewerThread = nullptr;
gRunThread.clear();
lua_State* L = luaL_newstate();
luaL_openlibs(L);
IupOpen(nullptr, nullptr);
IupControlsOpen();
iuplua_open(L);
iupcontrolslua_open(L);
TER::LoadFunctions(L);
ZON::LoadFunctions(L);
MOD::LoadFunctions(L);
WLD::LoadFunctions(L);
Viewer::LoadFunctions(L);
Util::LoadFunctions(L);
if (luaL_loadfile(L, "gui/main.lua") != 0)
{
ShowError("Could not load GUI script:\n%s\n", lua_tostring(L, -1));
}
else if (lua_pcall(L, 0, 0, 0) != 0)
{
ShowError("Runtime error:\n%s\n", lua_tostring(L, -1));
}
lua_close(L);
return 0;
}
void g(int n)
{
using namespace std::chrono_literals;
std::this_thread::sleep_for(2s);
std::cout << "Thread " << n << " is going to clear the flag." << std::endl;
lock.clear(); // 解锁
}
/**
* \param engine The script engine to control.
* \param prefix The prefix to select behaviors from.
* \param min The minimum time in milliseconds between two random poses.
* \param max The maximum time in milliseconds between two random poses.
*/
PoseChanger(ScriptEngine & engine, std::string prefix, unsigned int min = 2000, unsigned int max = 8000) :
prefix(prefix),
min(min),
max(max),
engine_(engine),
timer_(engine_.ios())
{
started_.clear();
}
/**
* \param error The error that occured, if any.
*/
void handleTimeout_(boost::system::error_code const & error) {
if (!error) {
std::cout << "Executing random behavior." << std::endl;
if (!engine_.behavior.queued()) engine_.behavior.enqueueRandom(prefix);
asyncWaitRandom_();
} else {
started_.clear();
}
}
void step()
{
while(lock_vis.test_and_set()){}
for(auto i = elements->begin(); i != elements->end(); ++i)
{
if(i->step())
{
i = elements->erase(i);
--i;
}
}
lock_vis.clear();
}
void robotPoseCallback(const wrecs_msgs::sf_state_est::ConstPtr& msg)
{
if (!lock.test_and_set()) {
for (int i = 0; i < 30; i++) {
stored_msg.Joints[i] = msg->joints[i];
}
for (int i = 0; i < 3; i++) {
stored_msg.ComEst[i] = msg->com_est[i];
}
for (int i = 0; i < 2; i++) {
stored_msg.CopEst[i] = msg->cop_est[i];
}
stored_msg.FootBF0.X = msg->foot_b_F[0].x;
stored_msg.FootBF0.Y = msg->foot_b_F[0].y;
stored_msg.FootBF0.Z = msg->foot_b_F[0].z;
stored_msg.FootBF1.X = msg->foot_b_F[1].x;
stored_msg.FootBF1.Y = msg->foot_b_F[1].y;
stored_msg.FootBF1.Z = msg->foot_b_F[1].z;
stored_msg.SdfMidFoot0.Position.X = msg->sdf_mid_foot[0].position.x;
stored_msg.SdfMidFoot0.Position.Y = msg->sdf_mid_foot[0].position.y;
stored_msg.SdfMidFoot0.Position.Z = msg->sdf_mid_foot[0].position.z;
stored_msg.SdfMidFoot0.Orientation.W = msg->sdf_mid_foot[0].orientation.w;
stored_msg.SdfMidFoot0.Orientation.X = msg->sdf_mid_foot[0].orientation.x;
stored_msg.SdfMidFoot0.Orientation.Y = msg->sdf_mid_foot[0].orientation.y;
stored_msg.SdfMidFoot0.Orientation.Z = msg->sdf_mid_foot[0].orientation.z;
stored_msg.SdfMidFoot1.Position.X = msg->sdf_mid_foot[1].position.x;
stored_msg.SdfMidFoot1.Position.Y = msg->sdf_mid_foot[1].position.y;
stored_msg.SdfMidFoot1.Position.Z = msg->sdf_mid_foot[1].position.z;
stored_msg.SdfMidFoot1.Orientation.W = msg->sdf_mid_foot[1].orientation.w;
stored_msg.SdfMidFoot1.Orientation.X = msg->sdf_mid_foot[1].orientation.x;
stored_msg.SdfMidFoot1.Orientation.Y = msg->sdf_mid_foot[1].orientation.y;
stored_msg.SdfMidFoot1.Orientation.Z = msg->sdf_mid_foot[1].orientation.z;
newMessageArrived = true;
lock.clear();
}
}
/// <summary>
/// Updates the cover art.
/// </summary>
void Update() {
// Set while the updater thread is running.
static std::atomic_flag closed = ATOMIC_FLAG_INIT;
if (!closed.test_and_set()) {
std::thread([] () {
TextFunctions::_Update();
SendMessage(gLSModule.GetMessageWindow(), WindowMessages::WM_TEXTUPDATENOTIFY, 0, 0);
for (auto &coverArt : gCoverArt) {
coverArt.second.Update();
}
closed.clear();
}).detach();
}
}
// CODETAG_IOR_SIGNALS
//++
// Details: The SIGINT signal is sent to a process by its controlling terminal
// when a
// user wishes to interrupt the process. This is typically initiated by
// pressing
// Control-C, but on some systems, the "delete" character or "break"
// key can be
// used.
// Be aware this function may be called on another thread besides the
// main thread.
// Type: Function.
// Args: vSigno - (R) Signal number.
// Return: None.
// Throws: None.
//--
void sigint_handler(int vSigno) {
#ifdef _WIN32 // Restore handler as it is not persistent on Windows
signal(SIGINT, sigint_handler);
#endif
static std::atomic_flag g_interrupt_sent = ATOMIC_FLAG_INIT;
CMIDriverMgr &rDriverMgr = CMIDriverMgr::Instance();
lldb::SBDebugger *pDebugger = rDriverMgr.DriverGetTheDebugger();
if (pDebugger != nullptr) {
if (!g_interrupt_sent.test_and_set()) {
pDebugger->DispatchInputInterrupt();
g_interrupt_sent.clear();
}
}
// Send signal to driver so that it can take suitable action
rDriverMgr.DeliverSignal(vSigno);
}
void visLoop()
{
running = true;
while(running)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
while(lock_vis.test_and_set()){}
for(auto i = elements->begin(); i != elements->end(); ++i)
{
i->draw();
}
lock_vis.clear();
glfwSwapBuffers(window);
glfwPollEvents();
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *isNew = (real_T *)ssGetOutputPortRealSignal(S, 0);
SL_ROS_SUB_MSG *msg = (SL_ROS_SUB_MSG *)ssGetOutputPortSignal(S, 1);
int_T* busInfo = (int_T *)ssGetUserData(S);
//mexPrintf("Acquiring lock...");
while (lock.test_and_set());
//mexPrintf("Acquired lock. Outputing message...");
isNew[0] = (int)newMessageArrived;
//memcpy(msg, &stored_msg, sizeof(SL_ROS_SUB_MSG));
if (newMessageArrived) {
*msg = stored_msg;
/*Copy temporary structure into output bus*/
/*(void)memcpy(msg + busInfo[0], stored_msg.Joints, busInfo[1]);
(void)memcpy(msg + busInfo[2], stored_msg.ComEst, busInfo[3]);
(void)memcpy(msg + busInfo[4], stored_msg.CopEst, busInfo[5]);
(void)memcpy(msg + busInfo[6], &stored_msg.SdfMidFoot1.Position.X, busInfo[7]);
(void)memcpy(msg + busInfo[8], &stored_msg.SdfMidFoot1.Position.Y, busInfo[9]);
(void)memcpy(msg + busInfo[10], &stored_msg.SdfMidFoot1.Position.Z, busInfo[11]);
(void)memcpy(msg + busInfo[12], &stored_msg.SdfMidFoot1.Orientation.X, busInfo[13]);
(void)memcpy(msg + busInfo[14], &stored_msg.SdfMidFoot1.Orientation.Y, busInfo[15]);
(void)memcpy(msg + busInfo[16], &stored_msg.SdfMidFoot1.Orientation.Z, busInfo[17]);
(void)memcpy(msg + busInfo[18], &stored_msg.SdfMidFoot1.Orientation.W, busInfo[19]);
(void)memcpy(msg + busInfo[20], &stored_msg.FootBF1.X, busInfo[21]);
(void)memcpy(msg + busInfo[22], &stored_msg.FootBF1.Y, busInfo[23]);
(void)memcpy(msg + busInfo[24], &stored_msg.FootBF1.Z, busInfo[25]);
(void)memcpy(msg + busInfo[26], &stored_msg.FootBF0.X, busInfo[27]);
(void)memcpy(msg + busInfo[28], &stored_msg.FootBF0.Y, busInfo[29]);
(void)memcpy(msg + busInfo[30], &stored_msg.FootBF0.Z, busInfo[31]);
(void)memcpy(msg + busInfo[32], &stored_msg.SdfMidFoot0.Position.X, busInfo[33]);
(void)memcpy(msg + busInfo[34], &stored_msg.SdfMidFoot0.Position.Y, busInfo[35]);
(void)memcpy(msg + busInfo[36], &stored_msg.SdfMidFoot0.Position.Z, busInfo[37]);
(void)memcpy(msg + busInfo[38], &stored_msg.SdfMidFoot0.Orientation.X, busInfo[39]);
(void)memcpy(msg + busInfo[40], &stored_msg.SdfMidFoot0.Orientation.Y, busInfo[41]);
(void)memcpy(msg + busInfo[42], &stored_msg.SdfMidFoot0.Orientation.Z, busInfo[43]);
(void)memcpy(msg + busInfo[44], &stored_msg.SdfMidFoot0.Orientation.W, busInfo[45]);*/
}
newMessageArrived = false;
lock.clear();
}
void DataChunStorage::writeCache(int cacheNumber, int numberOfEntries)
{
// Lock until all data are written
while (storeChunkLock.test_and_set()) {ERROR("CACHE IS FULL!!!");}
// Go through cache and identify all data chunks. Write them with the
// correct size.
for(int i = 0; i < numberOfEntries; i++)
{
int writeSize;
switch((int)dataCache[cacheNumber][i].typeNumberId)
{
case CHUNK_TYPE_ID_MALLOC:
writeSize = sizeof(DataChunkMalloc);
break;
case CHUNK_TYPE_ID_FREE:
writeSize = sizeof(DataChunkFree);
break;
case CHUNK_TYPE_ID_CALLOC:
writeSize = sizeof(DataChunkCalloc);
break;
case CHUNK_TYPE_ID_REALLOC:
writeSize = sizeof(DataChunkRealloc);
break;
case CHUNK_TYPE_ID_MEMALIGN:
writeSize = sizeof(DataChunkMemalign);
break;
default:
ERROR("Unknown chunk type");
writeSize = 0;
break;
}
int written = write(logFileFd, (char*)&dataCache[cacheNumber][i], writeSize);
(void)written;
}
// Unlock writeing
storeChunkLock.clear();
}
void reset() {
flag.clear();
}
spinlock()
{
m_lock.clear();
}
void unlock() {
m_lock.clear(std::memory_order_release);
}
spin_lock(void) { lc_.clear(std::memory_order_relaxed); }
/*
* Run an external program to make a report and tell the user where the report
* file is.
*
* Use of char*'s is deliberate: only async-safe calls allowed past this point!
*/
void
ArchLogPostMortem(const char* reason, const char* message /* = nullptr */)
{
static std::atomic_flag busy = ATOMIC_FLAG_INIT;
// Disallow recursion and allow only one thread at a time.
while (busy.test_and_set(std::memory_order_acquire)) {
// Spin!
}
const char* progname = ArchGetProgramNameForErrors();
// If we can attach a debugger then just exit here.
if (ArchDebuggerAttach()) {
ARCH_DEBUGGER_TRAP;
_exit(0);
}
/* Could use tmpnam but we're trying to be minimalist here. */
char logfile[1024];
if (_GetStackTraceName(logfile, sizeof(logfile)) == -1) {
// Cannot create the logfile.
static const char msg[] = "Cannot create a log file\n";
write(2, msg, sizeof(msg) - 1);
busy.clear(std::memory_order_release);
return;
}
// Write reason for stack trace to logfile.
if (FILE* stackFd = ArchOpenFile(logfile, "a")) {
if (reason) {
fprintf(stackFd, "This stack trace was requested because: %s\n",
reason);
}
if (message) {
fprintf(stackFd, "%s\n", message);
}
fclose(stackFd);
}
/* get hostname for printing out in the error message only */
char hostname[MAXHOSTNAMELEN];
if (gethostname(hostname,MAXHOSTNAMELEN) != 0) {
/* error getting hostname; don't try to print it */
hostname[0] = '\0';
}
fprintf(stderr, "\n");
fprintf(stderr,
"------------------------ '%s' is dying ------------------------\n",
progname);
// print out any registered program info
{
std::lock_guard<std::mutex> lock(_progInfoForErrorsMutex);
if (_progInfoForErrors) {
fprintf(stderr, "%s", _progInfoForErrors);
}
}
if (reason) {
fprintf(stderr, "This stack trace was requested because: %s\n", reason);
}
if (message) {
fprintf(stderr, "%s\n", message);
}
fprintf(stderr, "The stack can be found in %s:%s\n", hostname, logfile);
int loggedStack = _LogStackTraceForPid(logfile);
fprintf(stderr, "done.\n");
// Additionally, print the first few lines of extra log information since
// developers don't always think to look for it in the stack trace file.
_EmitAnyExtraLogInfo(stderr, 3 /* max */);
fprintf(stderr,
"------------------------------------------------------------------\n");
if (loggedStack) {
_EmitAnyExtraLogInfo(logfile);
_FinishLoggingFatalStackTrace(progname, logfile, NULL /*session log*/,
true /* crashing hard? */);
}
busy.clear(std::memory_order_release);
}
ByteCodeCacheEntry() : m_compiled(false), m_shader_bytecode({})
{
m_initialized.clear();
}
void unlock(void)
{
lc_.clear(std::memory_order_release);
}
spinlock_mutex()
: flag()
{
flag.clear();
}
void unlock()
{
flag.clear(std::memory_order_release);
}
inline void staticmutex::unlock() {
m_flag.clear(std::memory_order_release);
}
inline void staticmutex::init() {
m_flag.clear();
}
void unlock(){
flag.clear();
}
void g(int n)
{
cout<<"thread "<< n << "is going to start"<<endl;
lock.clear();
cout<<"thread "<< n << "starts working"<<endl;
}
/* Conversion to system time */
static
time_point to_system_time(const time_point& t) {
static std::atomic<int64_t> ns_offset(0);
static std::atomic<int64_t> offset_timestamp(0);
static std::atomic_flag lock = ATOMIC_FLAG_INIT;
static const duration OFFSET_TIMEOUT (15 * (int64_t)1E9, time_unit::NSEC);
static const duration CLOSE_DISTANCE (15 * (int64_t)1E3, time_unit::NSEC);
static const uint64_t MAX_UPDATE_TRIES (100);
time_point current_tsc_time = tsc_clock::now();
time_unit tsc_unit = current_tsc_time.time_since_epoch().unit();
int64_t offset_ts = offset_timestamp.load(std::memory_order_acquire);
if (offset_ts == 0 ||
current_tsc_time.time_since_epoch() - duration(offset_ts, tsc_unit) > OFFSET_TIMEOUT
)
{
if (!lock.test_and_set(std::memory_order_acquire)) {
time_point cycles_start, cycles_end;
time_point current_system_time;
bool close_pair_found = false;
for (uint64_t update_try = 0; update_try < MAX_UPDATE_TRIES; ++update_try) {
cycles_start = tsc_clock::now();
current_system_time = system_clock::now();
cycles_end = tsc_clock::now();
if (cycles_end - cycles_start < CLOSE_DISTANCE) {
close_pair_found = true;
break;
}
}
if (close_pair_found) {
time_point cycles_middle = cycles_start + (cycles_end - cycles_start) / 2;
int64_t new_offset =
duration::convert_to(
time_unit::NSEC,
current_system_time.time_since_epoch() - cycles_middle.time_since_epoch()
)
.count();
ns_offset.store(new_offset, std::memory_order_release);
offset_timestamp.store(cycles_middle.time_since_epoch().count(), std::memory_order_release);
}
lock.clear(std::memory_order_release);
}
}
return
time_point(
duration::convert_to(
time_unit::NSEC,
t.time_since_epoch() + duration(ns_offset.load(std::memory_order_acquire), time_unit::NSEC)
),
clock_type::SYSTEM
);
}
/// Stop executing random behaviors.
void cancel() {
timer_.cancel();
started_.clear();
}
void foo()
{
working.clear();
}
void unlock() {
flag.clear(std::memory_order_seq_cst);
std::atomic_thread_fence(std::memory_order_seq_cst);
}
void unlock() noexcept
{
lock_flag.clear(std::memory_order_release);
}
