void
gomp_mutex_lock_slow (gomp_mutex_t *mutex, int oldval)
{
/* First loop spins a while. */
while (oldval == 1)
{
if (do_spin (mutex, 1))
{
/* Spin timeout, nothing changed. Set waiting flag. */
oldval = __atomic_exchange_n (mutex, -1, MEMMODEL_ACQUIRE);
if (oldval == 0)
return;
futex_wait (mutex, -1);
break;
}
else
{
/* Something changed. If now unlocked, we're good to go. */
oldval = 0;
if (__atomic_compare_exchange_n (mutex, &oldval, 1, false,
MEMMODEL_ACQUIRE, MEMMODEL_RELAXED))
return;
}
}
/* Second loop waits until mutex is unlocked. We always exit this
loop with wait flag set, so next unlock will awaken a thread. */
while ((oldval = __atomic_exchange_n (mutex, -1, MEMMODEL_ACQUIRE)))
do_wait (mutex, -1);
}
void idle(void) {
if(reality_mode) {
static long oldtime = 0;
long newtime = get_time();
long dt = newtime - oldtime;
oldtime = newtime;
elevation += (double)dt*MULTIPLIER*ELEV_RATE*
(int)((double)(mousey - ae_window_width()/2)/5);
while(elevation > M_PI_2) {
elevation -= M_PI;
azimuth -= 2*M_PI;
}
while(elevation < -M_PI_2) {
elevation += M_PI;
azimuth += 2*M_PI;
}
azimuth += (double)dt*MULTIPLIER*AZI_RATE*
(int)((double)(mousex - ae_window_width()/2)/5);
while(azimuth > M_PI) azimuth -= 2.0*M_PI;
while(azimuth < -M_PI) azimuth += 2.0*M_PI;
if(action & ROLL) {
roll += (double)dt*MULTIPLIER*ROLL_RATE*
(int)((double)(mousex - ae_window_width()/2)/5);
while (roll > 2.0*M_PI) roll = roll - 4.0*M_PI;
if (compass_win_exists) {
glutSetWindow(compass_win);
glutPostRedisplay();
glutSetWindow(disp_win);
}
}
glutSetWindow(ae_win);
glutPostRedisplay();
glutSetWindow(disp_win);
recalc_view_and_up_from_AER();
really_look_at_it();
} else {
if (action & ROLL) {
roll_clockwise();
if (compass_win_exists) {
glutSetWindow(compass_win);
glutPostRedisplay();
glutSetWindow(disp_win);
}
}
}
if (action & ZOOM_IN)
zoom();
else if (action & ZOOM_OUT)
zoom();
if (action & SPIN)
do_spin();
glutSetWindow(disp_win);
glutPostRedisplay();
}
static void calculate_mem(void) {
FILE *f, *p;
char buf[150];
char p1[50];
int n;
if(file_exists("/proc/meminfo")) {
f=fopen("/proc/meminfo", "r");
while(fgets(buf, sizeof(buf) - 1, f)) {
trim(buf);
if(split_array(buf," ",&chargv) > 0) {
if(chargv[0]!=NULL && chargv[1]!=NULL) {
if(!strncmp(chargv[0],"MemTotal:",9)) FOUNDMEM=atoi(chargv[1]);
if(!strncmp(chargv[0],"MemFree:",8)) MEMFREE=atoi(chargv[1]);
if(!strncmp(chargv[0],"Cached:",7)) MEMCACHE=atoi(chargv[1]);
}
}
}
fclose(f);
}
if(FOUNDMEM==0) {
fprintf_stdout("**** MEMORY COUNTING FAILED! ****\n");
exit(1);
}
if(file_exists("/proc/kcore")) {
memset(buf,0x0,sizeof(buf));
memset(p1,0x0,sizeof(p1));
p=popen("du /proc/kcore","r");
fgets(buf, sizeof(buf) - 1, p);
if(buf[0]!='\0') {
splitc(p1,buf,' ');
n=atoi(p1);
if(n!=0) fprintf_stdout("* Physical memory size: %d kB\n",n);
if(n!=0 && n < 184324) {
fprintf_stdout("**** NOT ENOUGH MEMORY (%d kB) ****\n",n);
exit(1);
}
}
pclose(p);
}
TOTALMEM=MEMFREE + MEMCACHE;
fprintf_stdout("* Total memory found: %d kB\n",FOUNDMEM);
do_spin("* Initializing..");
MAXSIZE=TOTALMEM - MINLEFT;
RAMSIZE=TOTALMEM / 5;
if(TOTALMEM > MINLEFT) {
if(RAMSIZE < 0 ) RAMSIZE=65536;
RAMSIZE=RAMSIZE * 4;
}
}
void
gomp_sem_wait_slow (gomp_sem_t *sem, int count)
{
/* First loop spins a while. */
while (count == 0)
if (do_spin (sem, 0)
/* Spin timeout, nothing changed. Set waiting flag. */
&& __atomic_compare_exchange_n (sem, &count, SEM_WAIT, false,
MEMMODEL_ACQUIRE, MEMMODEL_RELAXED))
{
futex_wait (sem, SEM_WAIT);
count = *sem;
break;
}
/* Something changed. If it wasn't the wait flag, we're good to go. */
else if (__builtin_expect (((count = *sem) & SEM_WAIT) == 0 && count != 0,
1))
{
if (__atomic_compare_exchange_n (sem, &count, count - SEM_INC, false,
MEMMODEL_ACQUIRE, MEMMODEL_RELAXED))
return;
}
/* Second loop waits until semaphore is posted. We always exit this
loop with wait flag set, so next post will awaken a thread. */
while (1)
{
unsigned int wake = count & ~SEM_WAIT;
int newval = SEM_WAIT;
if (wake != 0)
newval |= wake - SEM_INC;
if (__atomic_compare_exchange_n (sem, &count, newval, false,
MEMMODEL_ACQUIRE, MEMMODEL_RELAXED))
{
if (wake != 0)
{
/* If we can wake more threads, do so now. */
if (wake > SEM_INC)
gomp_sem_post_slow (sem);
break;
}
do_wait (sem, SEM_WAIT);
count = *sem;
}
}
}
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long. Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <example_action_server::demoAction> customizes the simple action server to use our own "action" message
// defined in our package, "example_action_server", in the subdirectory "action", called "demo.action"
// The name "demo" is prepended to other message types created automatically during compilation.
// e.g., "demoAction" is auto-generated from (our) base name "demo" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<example_action_server::demoAction>::GoalConstPtr& goal) {
ROS_INFO("callback activated");
double yaw_desired, yaw_current, travel_distance, spin_angle;
nav_msgs::Path paths;
geometry_msgs::Pose pose_desired;
paths = goal->input;
int npts = paths.poses.size();
ROS_INFO("received path request with %d poses",npts);
int move = 0;
for (int i=0;i<npts;i++) { //visit each subgoal
// odd notation: drill down, access vector element, drill some more to get pose
pose_desired = paths.poses[i].pose; //get first pose from vector of poses
get_yaw_and_dist(g_current_pose, pose_desired,travel_distance, yaw_desired);
ROS_INFO("pose %d: desired yaw = %f; desired (x,y) = (%f,%f)",i,yaw_desired,
pose_desired.position.x,pose_desired.position.y);
ROS_INFO("current (x,y) = (%f, %f)",g_current_pose.position.x,g_current_pose.position.y);
ROS_INFO("travel distance = %f",travel_distance);
ROS_INFO("pose %d: desired yaw = %f",i,yaw_desired);
yaw_current = convertPlanarQuat2Phi(g_current_pose.orientation); //our current yaw--should use a sensor
spin_angle = yaw_desired - yaw_current; // spin this much
//spin_angle = min_spin(spin_angle);// but what if this angle is > pi? then go the other way
do_spin(spin_angle); // carry out this incremental action
// we will just assume that this action was successful--really should have sensor feedback here
g_current_pose.orientation = pose_desired.orientation; // assumes got to desired orientation precisely
move = pose_desired.position.x;
ROS_INFO("Move: %d", move);
do_move(move);
if(as_.isPreemptRequested()){
do_halt();
ROS_WARN("Srv: goal canceled.");
result_.output = -1;
as_.setAborted(result_);
return;
}
}
result_.output = 0;
as_.setSucceeded(result_);
ROS_INFO("Move finished.");
}
void idle(void) {
long newtime = get_time();
long dt = newtime - oldtime;
oldtime = newtime;
if(fsim_mode) {
elevation += ELEV_RATE*dt*trunc(5*(mousey - disp_window_height()));
azimuth += AZI_RATE*dt*trunc(5*(mousex - disp_window_width()));
if(action & ROLL)
roll += ROLL_RATE*dt*trunc(5*(mousex - disp_window_width()));
} else {
if (action & ROLL)
roll_clockwise();
if (action & ZOOM_IN)
zoom();
else if (action & ZOOM_OUT)
zoom();
}
if (action & SPIN)
do_spin();
glutPostRedisplay();
}
void QuatTrackBall::rotate(const Vec2f& new_v)
{
calcRotation(new_v);
do_spin();
}
static void
do_wait (int *addr, int val)
{
if (do_spin (addr, val))
futex_wait (addr, val);
}
