void go(int direction) {
ros::Rate loop_timer(1/sample_dt); //create a ros object from the ros “Rate” class; set 100Hz rate
// start with all zeros in the command message; should be the case by default, but just to be safe..
twist_cmd.linear.x = 0.0;
twist_cmd.linear.y = 0.0;
twist_cmd.linear.z = 0.0;
twist_cmd.angular.x = 0.0;
twist_cmd.angular.y = 0.0;
twist_cmd.angular.z = 0.0;
switch (direction) {
case NONE:
break;
case FORWARD:
twist_cmd.linear.x = speed;
break;
case BACKWARD:
twist_cmd.linear.x = -1.0 * speed;
break;
default:
break;
}
for (int i=0;i<10;i++) {
(*twist_commander).publish(twist_cmd);
loop_timer.sleep();
ros::spinOnce();
}
}
void spin(int direction) {
ros::Rate loop_timer(1/sample_dt); //create a ros object from the ros “Rate” class; set 100Hz rate
twist_cmd.linear.x = 0.0;
twist_cmd.linear.y = 0.0;
twist_cmd.linear.z = 0.0;
twist_cmd.angular.x = 0.0;
twist_cmd.angular.y = 0.0;
twist_cmd.angular.z = 0.0;
switch (direction) {
case NONE:
break;
case LEFT:
twist_cmd.angular.z = yaw_rate;
break;
case RIGHT:
twist_cmd.angular.z = -1.0 * yaw_rate;
break;
default:
break;
}
for (int i=0;i<10;i++) {
(*twist_commander).publish(twist_cmd);
loop_timer.sleep();
ros::spinOnce();
}
}
void do_halt() {
ros::Rate loop_timer(1/g_sample_dt);
g_twist_cmd.angular.z= 0.0;
g_twist_cmd.linear.x=0.0;
for (int i=0;i<10;i++) {
g_twist_commander.publish(g_twist_cmd);
loop_timer.sleep();
}
}
// a few action functions:
//a function to reorient by a specified angle (in radians), then halt
void do_spin(double spin_ang) {
ros::Rate loop_timer(1/g_sample_dt);
double timer=0.0;
double final_time = fabs(spin_ang)/g_spin_speed;
g_twist_cmd.angular.z= sgn(spin_ang)*g_spin_speed;
while(timer<final_time) {
g_twist_commander.publish(g_twist_cmd);
timer+=g_sample_dt;
loop_timer.sleep();
}
do_halt();
}
int main(int argc, char** argv) {
ros::init(argc, argv, "object_recognition_kitchen"); //node name
ros::NodeHandle nh;
ros::Subscriber object_sub = nh.subscribe("/recognized_object_array", 1, &objectCallback);
ros::Subscriber plane_sub = nh.subscribe("/table_array", 1, &planeCallback);
ros::Duration loop_timer(2.5);
while (ros::ok()) {
ros::spinOnce();
loop_timer.sleep();
}
return 0;
}
//node to send Twist commands to the Simple 2-Dimensional Robot Simulator via cmd_vel
int main(int argc, char **argv) {
ros::init(argc, argv, "commander");
ros::NodeHandle n; // two lines to create a publisher object that can talk to ROS
ros::Publisher twist_commander = n.advertise<geometry_msgs::Twist>("/robot0/cmd_vel", 1);
ros::Subscriber alarm_subscriber = n.subscribe("lidar_alarm",1,alarmCallback);
//some "magic numbers"
double sample_dt = 0.01; //specify a sample period of 10ms
double speed = 0.5; // 1m/s speed command
double yaw_rate = 0.5; //0.5 rad/sec yaw rate command
double time_3_sec = 3.0; // should move 3 meters or 1.5 rad in 3 seconds
geometry_msgs::Twist twist_cmd; //this is the message type required to send twist commands to STDR
// start with all zeros in the command message; should be the case by default, but just to be safe..
twist_cmd.linear.x=0.0;
twist_cmd.linear.y=0.0;
twist_cmd.linear.z=0.0;
twist_cmd.angular.x=0.0;
twist_cmd.angular.y=0.0;
twist_cmd.angular.z=0.0;
ros::Rate loop_timer(1/sample_dt); //create a ros object from the ros “Rate” class; set 100Hz rate
double timer=0.0;
//start sending some zero-velocity commands, just to warm up communications with STDR
for (int i=0;i<10;i++) {
twist_commander.publish(twist_cmd);
ros::spinOnce();
loop_timer.sleep();
}
while(ros::ok()) { // do forever
twist_cmd.angular.z=0.0; // do not spin
twist_cmd.linear.x=speed; //command to move forward
while(!g_lidar_alarm) { // keep moving forward until get an alarm signal
twist_commander.publish(twist_cmd);
timer+=sample_dt;
ros::spinOnce();
loop_timer.sleep();
}
//here if got an alarm; turn CCW until alarm clears
twist_cmd.linear.x=0.0; //stop moving forward
twist_cmd.angular.z=yaw_rate; //and start spinning in place
timer=0.0; //reset the timer
while(g_lidar_alarm) {
twist_commander.publish(twist_cmd);
timer+=sample_dt;
ros::spinOnce();
loop_timer.sleep();
}
}
//done commanding the robot; node runs to completion
}
//a function to move forward by a specified distance (in meters), then halt
void do_move(double distance) { // always assumes robot is already oriented properly
// but allow for negative distance to mean move backwards
ros::Rate loop_timer(1/g_sample_dt);
double timer=0.0;
double final_time = fabs(distance)/g_move_speed;
g_twist_cmd.angular.z = 0.0; //stop spinning
g_twist_cmd.linear.x = sgn(distance)*g_move_speed;
while(timer<final_time) {
g_twist_commander.publish(g_twist_cmd);
timer+=g_sample_dt;
loop_timer.sleep();
}
do_halt();
}
void stop() {
ros::Rate loop_timer(1/sample_dt); //create a ros object from the ros “Rate” class; set 100Hz rate
twist_cmd.linear.x = 0.0;
twist_cmd.linear.y = 0.0;
twist_cmd.linear.z = 0.0;
twist_cmd.angular.x = 0.0;
twist_cmd.angular.y = 0.0;
twist_cmd.angular.z = 0.0;
for (int i=0;i<10;i++) {
(*twist_commander).publish(twist_cmd);
loop_timer.sleep();
ros::spinOnce();
}
}
void turn(double rad) {
double time = abs(rad)/yaw_rate;
double timer = 0.0;
ros::Rate loop_timer(1/sample_dt); //create a ros object from the ros “Rate” class; set 100Hz rate
// start with all zeros in the command message; should be the case by default, but just to be safe..
twist_cmd.linear.x = 0.0;
twist_cmd.linear.y = 0.0;
twist_cmd.linear.z = 0.0;
twist_cmd.angular.x = 0.0;
twist_cmd.angular.y = 0.0;
twist_cmd.angular.z = 0.0;
if (rad == 0) {
} else if (rad > 0.0) {
twist_cmd.angular.z = yaw_rate;
ROS_INFO("Turn left");
} else if (rad < 0.0){
twist_cmd.angular.z = -1.0 * yaw_rate;
ROS_INFO("Turn right");
}
while(timer<time) {
(*twist_commander).publish(twist_cmd);
timer+=sample_dt;
loop_timer.sleep();
ros::spinOnce();
}
twist_cmd.angular.z=0.0;
for (int i=0;i<10;i++) {
(*twist_commander).publish(twist_cmd);
loop_timer.sleep();
ros::spinOnce();
}
}
int long_basic_op(char *opType, unsigned long reps){
int i;
#ifdef ONEVOL
long a, b, c;
volatile long d;
#undef ONEVOL
#else
#ifdef ALLVOL
volatile long a, b, c, d;
#undef ALLVOL
#else
long a, b, c, d;
#endif
#endif
char choice = (char)opType[0];
srand((unsigned int)time(NULL));
a = rand();
b = rand();
c = rand();
d = rand();
struct timespec start, end;
/* warm-up loop with nop */
warmup_loop(reps);
/* measure loop on its own with nop */
loop_timer_nop(reps);
/* measure loop on its own */
loop_timer(reps);
switch(choice){
case '+': {
/* warm-up loop */
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<100; i++) c = a+b;
/* main loops */
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++) c = a+d;
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Addition - 1 op HR");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a+d;
b = d+c;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Addition - 2 ops");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a+d;
b = d+c;
a = b+d;
c = d+a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Addition - 4 ops");
reps = reps * 4 / 5;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a+d;
b = d+c;
a = b+d;
c = d+a;
b = c+d;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Addition - 5 ops");
reps = reps * 5 / 8;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a+d;
b = d+c;
a = b+d;
c = d+a;
b = c+d;
c = d+b;
a = d+c;
b = a+d;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Addition - 8 ops");
reps = reps * 4 / 5;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a+d;
b = d+c;
a = b+d;
c = d+a;
b = c+d;
c = d+b;
a = d+c;
b = a+d;
c = b+d;
b = c+d;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Addition - 10 ops");
break;
}
case '-': {
/* warm-up loop */
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<100; i++) c = a-b;
/* main loop */
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++) c = a-b;
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Subtraction - 1 op");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a-b;
a = b-c;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Subtraction - 2 ops");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a-b;
a = b-c;
b = a-c;
c = c-a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Subtraction - 4 ops");
reps = reps * 4 / 5;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a-b;
a = b-c;
b = a-c;
c = c-a;
b = b-a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Subtraction - 5 ops");
reps = reps * 5 / 8;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a-b;
a = b-c;
b = a-c;
c = c-a;
b = b-a;
c = c-b;
a = a-c;
b = b-c;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Integer Subtraction - 8 ops");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a-b;
a = b-c;
b = a-c;
c = c-a;
b = b-a;
c = c-b;
a = a-c;
b = b-c;
c = b-a;
b = c-a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Subtraction - 10 ops");
break;
}
case '*': {
/* warm-up loop */
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<100; i++) c = a*b;
/* main loop */
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++) c = a*b;
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Multiplication - 1 op");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a*b;
a = b*c;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Multiplication - 2 ops");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a*b;
a = b*c;
b = a*c;
c = c*a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Multiplication - 4 ops");
reps = reps * 4 / 5;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a*b;
a = b*c;
b = a*c;
c = c*a;
b = b*a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Multiplication - 5 ops");
reps = reps * 5 / 8;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a*b;
a = b*c;
b = a*c;
c = c*a;
b = b*a;
c = c*b;
a = a*c;
b = b*c;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Multiplication - 8 ops");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
c = a*b;
a = b*c;
b = a*c;
c = c*a;
b = b*a;
c = c*b;
a = a*c;
b = b*c;
c = b*a;
b = c*a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Multiplication - 10 ops");
break;
}
case '/': {
/* need to avoid re-use of variables to make sure */
/* we do not get divison by 0. */
volatile int r1,r2,r3,r4,r5,r6,r7,r8,r9,r10;
/* warm-up loop */
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<100; i++) r1 = a/b;
/* main loop */
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++) r1 = a/b;
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Division - 1 op");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
r1 = a/b;
r2 = b/c;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Division - 2 ops");
reps = reps / 2;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
r1 = a/b;
r2 = b/c;
r3 = a/c;
r4 = b/a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Division - 4 ops");
reps = reps * 4 / 5;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
r1 = a/b;
r2 = b/c;
r3 = a/c;
r4 = b/a;
r5 = c/a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Division - 5 ops");
reps = reps * 5 / 8;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
r1 = a/b;
r2 = b/c;
r3 = a/c;
r4 = b/a;
r5 = c/a;
r6 = c/b;
r7 = a/a;
r8 = b/b;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Division - 8 ops");
reps = reps * 4 / 5;
clock_gettime(CLOCK, &start);
# pragma omp parallel private(i) firstprivate(a,b,c,d)
for(i=0; i<reps; i++){
r1 = a/b;
r2 = b/c;
r3 = a/c;
r4 = b/a;
r5 = c/a;
r6 = c/b;
r7 = a/a;
r8 = b/b;
r9 = c/c;
r10 = r10/a;
}
clock_gettime(CLOCK, &end);
elapsed_time_hr(start, end, "Long Division - 10 ops");
break;
}
default: printf("Only possible operation choices are '+', '-', '*' and '/'.\n");
}
return 0;
}
//node to send Twist commands to the Simple 2-Dimensional Robot Simulator via cmd_vel
int main(int argc, char **argv) {
ros::init(argc, argv, "stdr_commander");
ros::NodeHandle n; // two lines to create a publisher object that can talk to ROS
ros::Publisher twist_commander = n.advertise<geometry_msgs::Twist>("/robot0/cmd_vel", 1);
//some "magic numbers"
double sample_dt = 0.01; //specify a sample period of 10ms
double speed = 1.0; // 1m/s speed command
double yaw_rate = 0.5; //0.5 rad/sec yaw rate command
double time_3_sec = 3.0; // should move 3 meters or 1.5 rad in 3 seconds
geometry_msgs::Twist twist_cmd; //this is the message type required to send twist commands to STDR
// start with all zeros in the command message; should be the case by default, but just to be safe..
twist_cmd.linear.x = 0.0;
twist_cmd.linear.y = 0.0;
twist_cmd.linear.z = 0.0;
twist_cmd.angular.x = 0.0;
twist_cmd.angular.y = 0.0;
twist_cmd.angular.z = 0.0;
ros::Rate loop_timer(1/sample_dt); //create a ros object from the ros “Rate” class; set 100Hz rate
double timer= 0.0;
//start sending some zero-velocity commands, just to warm up communications with STDR
for (int i = 0; i < 10; i++) {
twist_commander.publish(twist_cmd);
loop_timer.sleep();
}
//Go straight
twist_cmd.linear.x=speed; //command to move forward
while(timer < time_3_sec) {
twist_commander.publish(twist_cmd);
timer+= sample_dt;
loop_timer.sleep();
}
//Turn left
twist_cmd.linear.x = 0.0; //stop moving forward
twist_cmd.angular.z = yaw_rate; //and start spinning in place
timer= 0.0; //reset the timer
while(timer < time_3_sec) {
twist_commander.publish(twist_cmd);
timer+= sample_dt;
loop_timer.sleep();
}
//Got straight
twist_cmd.angular.z = 0.0; //and stop spinning in place
twist_cmd.linear.x = speed; //and move forward again
timer = 0.0; //reset the timer
while(timer < time_3_sec) {
twist_commander.publish(twist_cmd);
timer+= sample_dt;
loop_timer.sleep();
}
//Turn right
twist_cmd.linear.x = 0.0; //stop moving forward
twist_cmd.angular.z = -yaw_rate; //and start spinning in place
timer= 0.0; //reset the timer
while(timer < time_3_sec) {
twist_commander.publish(twist_cmd);
timer+= sample_dt;
loop_timer.sleep();
}
//Got straight
twist_cmd.angular.z = 0.0; //and stop spinning in place
twist_cmd.linear.x = speed; //and move forward again
timer = 0.0; //reset the timer
while(timer < time_3_sec) {
twist_commander.publish(twist_cmd);
timer+= sample_dt;
loop_timer.sleep();
}
//Turn left
twist_cmd.linear.x = 0.0; //stop moving forward
twist_cmd.angular.z = yaw_rate; //and start spinning in place
timer= 0.0; //reset the timer
while(timer < time_3_sec) {
twist_commander.publish(twist_cmd);
timer+= sample_dt;
loop_timer.sleep();
}
//Got straight
twist_cmd.angular.z = 0.0; //and stop spinning in place
twist_cmd.linear.x = speed; //and move forward again
timer = 0.0; //reset the timer
while(timer < 10.0) {
twist_commander.publish(twist_cmd);
timer+= sample_dt;
loop_timer.sleep();
}
//halt the motion
twist_cmd.angular.z = 0.0;
twist_cmd.linear.x = 0.0;
for (int i = 0;i < 10; i++) {
twist_commander.publish(twist_cmd);
loop_timer.sleep();
}
//done commanding the robot; node runs to completion
}
//node to send Twist commands to the Simple 2-Dimensional Robot Simulator via cmd_vel
int main(int argc, char **argv) {
ros::init(argc, argv, "feedback_solver");
ros::NodeHandle n; // two lines to create a publisher object that can talk to ROS
ros::Subscriber alarm_subscriber = n.subscribe("lidar_alarm",1,alarmCallback);
ros::Subscriber direction_subscriber = n.subscribe("opt_direction", 1, directionCallback);
ros::Subscriber odem_subscriber = n.subscribe("odom", 1, odemCallback);
RobotCommander robot(&n);
ros::Rate loop_timer(1/adjust_time); //create a ros object from the ros “Rate” class; set 100Hz rate
//start sending some zero-velocity commands, just to warm up communications with STDR
robot.move(NONE, 3.0);
while(ros::ok()) { // do forever
if (g_lidar_alarm) {
ROS_INFO("Back from wall");
if (opt_dir > M_PI/2 || opt_dir < -1*M_PI/2) {
opt_dir = opt_dir - M_PI;
robot.turn(opt_dir);
robot.move(-0.2);
} else {
robot.turn(opt_dir);
robot.move(0.2);
}
g_lidar_alarm = false;
//is_move = true;
move_cnt = 0;
ROS_INFO("Best direction: %f degree", (opt_dir/M_PI)*180);
ROS_INFO("Current orientation: %f degree", (orientation/M_PI)*180);
}
else {
/*
* alternative way to turn, worse than current
if (opt_dir > (3.0 / 180.0) * M_PI) {
if (is_move) {
robot.stop();
is_move = false;
ROS_INFO("Turn left");
}
robot.spin(LEFT);
} else if (opt_dir < (-3.0 / 180.0) * M_PI) {
if (is_move) {
robot.stop();
is_move = false;
ROS_INFO("Turn right");
}
robot.spin(RIGHT);
} else {
if (is_move == false) {
robot.go(FORWARD);
is_move = true;
ROS_INFO("Go!");
}
}*/
if (move_cnt < 2) {
robot.turn(opt_dir);
move_cnt = 5;
ROS_INFO("Best direction: %f degree", (opt_dir/M_PI)*180);
ROS_INFO("Current orientation: %f degree", (orientation/M_PI)*180);
} else {
robot.move(0.1);
move_cnt--;
}
}
//loop_timer.sleep();
ros::spinOnce();
}
//done commanding the robot; node runs to completion
}
// node to send Twist commands to the Simple 2-Dimensional Robot Simulator via cmd_vel
int main(int argc, char **argv) {
ros::init(argc, argv, "stdr_commander");
// create a publisher for Twist commands to the STDR robot
ros::NodeHandle n;
ros::Publisher twist_commander = n.advertise<geometry_msgs::Twist>("/robot0/cmd_vel", 1);
// some "magic numbers" for Twist commands
double sample_dt = 0.01; // sample period of 10ms
double speed = 1.0; // 1m/s speed command
double yaw_rate = 0.5; // 0.5 rad/sec yaw rate command
// this is the message type required to send twist commands to STDR
geometry_msgs::Twist twist_cmd;
// zero the Twist command message
twist_cmd.linear.x = 0.0;
twist_cmd.linear.y = 0.0;
twist_cmd.linear.z = 0.0;
twist_cmd.angular.x = 0.0;
twist_cmd.angular.y = 0.0;
twist_cmd.angular.z = 0.0;
// loop variables for Twist publishing messages
// rate is 100HZ with 0.01 sample_dt
ros::Rate loop_timer(1 / sample_dt); //sets execution step rate
double segment_timer = 0.0; // current execution time of segment loop
double total_segment_time = 0.0; // total execution time of segment loop
// send zero-velocity commands to warm up communications with STDR
for (int i = 0; i < 10; i++) {
twist_commander.publish(twist_cmd);
loop_timer.sleep();
}
// execute segment 1
// move forward 3 meters
segment_timer = 0.0; // reset timer
total_segment_time = 3.0; // set segment time
twist_cmd.linear.x = speed; // set translation component; linear.x is forward
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute turn 1
// spin in place 0.5 radians counter-clockwise
segment_timer = 0.0; //reset the timer
total_segment_time = 1.0; // set segment time
twist_cmd.linear.x = 0.0; // remove translation component
twist_cmd.angular.z = yaw_rate; // set rotation component; angular.z is turning axis
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 2
// move forward 5 meters
segment_timer = 0.0; //reset the timer
total_segment_time = 5.0; // set the segment run time
twist_cmd.angular.z = 0.0; // remove rotation component
twist_cmd.linear.x = speed; // set translation component
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 3
// turn XXX radian counter-clockwise
segment_timer = 0.0;
total_segment_time = 2.75;
twist_cmd.linear.x = 0.0;
twist_cmd.angular.z = yaw_rate;
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 4
// move forward 7 meters
segment_timer = 0.0;
total_segment_time = 7.0;
twist_cmd.angular.z = 0.0;
twist_cmd.linear.x = speed;
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 5
// turn XXX radians counter-clockwise
segment_timer = 0.0;
total_segment_time = 2.5;
twist_cmd.linear.x = 0.0;
twist_cmd.angular.z = yaw_rate;
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 6
// move forward 3 meters
segment_timer = 0.0;
total_segment_time = 3.0;
twist_cmd.angular.z = 0.0;
twist_cmd.linear.x = speed;
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 7
//turn XXX radians clockwise
segment_timer = 0.0;
total_segment_time = 3.25;
twist_cmd.linear.x = 0.0;
twist_cmd.angular.z = -yaw_rate;
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 8
// move forward 3 meters
segment_timer = 0.0;
total_segment_time = 2.85;
twist_cmd.angular.z = 0.0;
twist_cmd.linear.x = speed;
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 9
// turn XXX radians counter-clockwise
segment_timer = 0.0;
total_segment_time = 3.15;
twist_cmd.linear.x = 0.0;
twist_cmd.angular.z = yaw_rate;
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
// execute segment 10
// move forward XXX meters to final location, top-left corner
segment_timer = 0.0;
total_segment_time = 2.0;
twist_cmd.angular.z = 0.0;
twist_cmd.linear.x = speed;
while(segment_timer < total_segment_time) {
twist_commander.publish(twist_cmd);
segment_timer += sample_dt;
loop_timer.sleep();
}
//halt the motion
segment_timer = 0.0;
total_segment_time = 0.0;
twist_cmd.angular.z = 0.0;
twist_cmd.linear.x = 0.0;
for (int i = 0; i < 10; i++) {
twist_commander.publish(twist_cmd);
loop_timer.sleep();
}
//done commanding the robot; node runs to completion
}
