/* Turn()
* client: client to connect to robot
* deg: amount of degrees robot should turn
*/
float Turn(playerc_client_t *client, float deg)
{
printf("Enter Turn\n");
float error_a = error_ta(position2d, deg);
printf("error_a = %f\n", error_a);
while(fabs(error_a) > 0.1)
{
// Find margin of error between current and target angles
error_a = error_ta(position2d, deg);
// Set angle velocity based on error
va = PID_A(error_a);
printf("error_a = %f angle = %f\n", error_a, va);
playerc_position2d_set_cmd_vel(position2d, 0.0, 0.0, va, 1.0);
// Test collision with each bumper
if(bumper->bumpers[0]!=0 || bumper->bumpers[1]!=0)
{
playerc_position2d_set_cmd_vel(position2d, 0.0, 0.0, 0.0, 0.0);
break;
}
playerc_client_read(client);
printf("Turning : x = %f y = %f a = %f\n", position2d->px, position2d->py, position2d->pa);
}
printf("Leave Turn\n");
}
/* Move()
* client: client to connect to robot
* distance: x-coordinate that robot should aim for;
* angle: angle that robot should stay at;
*/
float Move(playerc_client_t *client, float distance, float angle)
{
printf("Enter Move\n");
error_x = error_tx(position2d, distance);
while(error_x > 0.1)
{
error_x = error_tx(position2d, distance);
vx = PID(error_x);
error_a = error_ta(position2d, angle);
va = PID_A(error_a);
playerc_position2d_set_cmd_vel(position2d, vx, 0, va, 1.0);
if(bumper->bumpers[0]!=0 || bumper->bumpers[1]!=0)
{
playerc_position2d_set_cmd_vel(position2d, 0.0, 0.0, 0.0, 0.0);
break;
}
playerc_client_read(client);
printf("Moving : x = %f y = %f a = %f\n", position2d->px, position2d->py, position2d->pa);
}
printf("Leave Move\n");
}
int main(int argc, const char **argv)
{
double dist, speed;
playerc_client_t *client;
playerc_position2d_t *position2d;
// Create a client object and connect to the server; the server must
// be running on "localhost" at port 6665
client = playerc_client_create(NULL, "gort", 9876);
if (playerc_client_connect(client) != 0)
{
fprintf(stderr, "error: %s\n", playerc_error_str());
return -1;
}
// Create a position2d proxy (device id "position2d:0") and susbscribe
// in read/write mode
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE) != 0)
{
fprintf(stderr, "error: %s\n", playerc_error_str());
return -1;
}
// Enable the robots motors
playerc_position2d_enable(position2d, 1);
// Start the robot moving
dist = 0;
speed = 1;
playerc_position2d_set_cmd_vel(position2d, speed, 0, 0, 1);
while( dist <= 3.048 ) {
// Read data from the server and display current robot position
playerc_client_read(client);
printf("position : %f %f %f\n", position2d->px, position2d->py, position2d->pa);
dist = position2d->px;
if(dist > 2.6 && speed > .01) {
playerc_position2d_set_cmd_vel(position2d, (speed /= 2), 0, 0, 1);
}
}
//stop the robot
playerc_position2d_set_cmd_vel(position2d, 0, 0, 0, 1);
// Shutdown and tidy up
playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
void Nav_MotionEstimator::Nav_CommandRobot(double * SpeedInput, double T)
{
struct timeval Start_t, End_t;
double mtime, seconds, useconds;
gettimeofday(&Start_t, NULL);
// lock position inputs
#ifdef USEGAZEBO
// lock position inputs
posIface->Lock(1);
posIface->data->cmdVelocity.pos.x = SpeedInput[0];
posIface->data->cmdVelocity.yaw = SpeedInput[1];
posIface->Unlock();
#elif defined PLAYERPLUGGING
#else
cout << "V: SpeedInput[0] " << SpeedInput[0] << " I : " << SpeedInput[1] << endl;
playerc_position2d_set_cmd_vel(position2d, SpeedInput[0], 0, SpeedInput[1], 1);
#endif
usleep(T * 1000000);
gettimeofday(&End_t, NULL);
seconds = End_t.tv_sec - Start_t.tv_sec;
useconds = End_t.tv_usec - Start_t.tv_usec;
mtime = (((seconds) * 1000 + useconds / 1000.0) + 0.5) / 1000;
}
int main(int argc, const char **argv)
{
int i;
playerc_client_t *client;
playerc_position2d_t *position2d;
// Create a client object and connect to the server; the server must
// be running on "localhost" at port 6665
client = playerc_client_create(NULL, "localhost", 6665);
if (playerc_client_connect(client) != 0)
{
fprintf(stderr, "error: %s\n", playerc_error_str());
return -1;
}
// Create a position2d proxy (device id "position2d:0") and susbscribe
// in read/write mode
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE) != 0)
{
fprintf(stderr, "error: %s\n", playerc_error_str());
return -1;
}
// Enable the robots motors
playerc_position2d_enable(position2d, 1);
// Start the robot turning slowing
playerc_position2d_set_cmd_vel(position2d, 0, 0, 0.1, 1);
for (i = 0; i < 200; i++)
{
// Read data from the server and display current robot position
playerc_client_read(client);
printf("position : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
// Shutdown and tidy up
playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
int
main(int argc, const char **argv)
{
int i;
playerc_client_t *client;
playerc_position2d_t *position2d;
// Create a client and connect it to the server.
client = playerc_client_create(NULL, "localhost", 6665);
if (0 != playerc_client_connect(client))
return -1;
// Create and subscribe to a position2d device.
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
return -1;
// Make the robot spin!
if (0 != playerc_position2d_set_cmd_vel(position2d, 0.25, 0, DTOR(40.0), 1))
return -1;
for (i = 0; i < 200; i++)
{
// Wait for new data from server
playerc_client_read(client);
// Print current robot pose
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
// Shutdown
playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
int
main(int argc, char **argv)
{
// Initialize movement parameters (default to a square)
int n = 4;
int l = 65;
int v = 50;
int a = 90;
// Parse command-line arguements
int option_index = 0;
static struct option long_options[] = {
{"line", 0, 0, 0 },
{"triangle", 0, 0, 1 },
{"square", 0, 0, 2 },
{"pentagon", 0, 0, 3 },
{"hexagon", 0, 0, 4 },
{0, 0, 0, 0 }
};
int shape_set = false;
int angle_set = false;
int num_set = false;
int reverse_direction = false;
int smooth_acceleration = false;
int c;
while ((c = getopt_long(argc, argv, "l:v:n:a:rs",
long_options, &option_index)) != -1)
switch (c) {
case 0: // line
n = 2; a = 180; shape_set = true; break;
case 1: // triangle
n = 3; a = 120; shape_set = true; break;
case 2: // square
n = 4; a = 90; shape_set = true; break;
case 3: // pentagon
n = 5; a = 72; shape_set = true; break;
case 4: // hexagon
n = 6; a = 60; shape_set = true; break;
case 'l': // 0 to 200 cm (default 65)
l = atoi(optarg); break;
case 'v': // 0 to 50 cm/sec (default 50)
v = atoi(optarg); break;
case 'n': // 0 to 10 (default 4)
n = atoi(optarg); num_set = true; break;
case 'a': // 0 to 180 degrees (default 90)
a = atoi(optarg); angle_set = true; break;
case 'r':
reverse_direction = true; break;
case 's':
smooth_acceleration = true; break;
case '?':
if (optopt == 'v' || optopt == 'a' || optopt == 'l' || optopt == 'n')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
return 1;
default:
printf("Bad arguments.\n\nUsage: dead_reckoning -n <number of sides> "
"-l <length of each side> -v <velocity (cm/sec)> "
"-a <turning angle (degrees)>");
exit(1);
}
int i = 0;
for (i = optind; i < argc; i++)
printf ("Non-option argument %s\n", argv[i]);
if(!shape_set && (!num_set || !angle_set)) {
printf("\nYou need to specify a shape (by name, or with the arguments "
"'n' and 'a').\n\nUsage: dead_reckoning --<shape> "
"[-l <side length>] [-v <velocity>] [-r] [-s]\n\n"
"Alternative usage: dead_reckoning -n <number of sides> "
"-a <angle of each corner in degrees> "
"[-l <length of each side>] [-v <velocity>]\n\n"
"Valid shapes are: line, triangle, square, pentagon, hexagon\n"
"Side length is in cm. Valid range is [10,200]. Default value is 65.\n"
"Velocity is in cm/sec. Valid range is [10,50]. Default value is 50.\n"
"The -r option reverses the turn direction (clockwise by default)\n"
"The -s option enables smooth acceleration and deceleration\n\n"
"e.g. dead_reckoning --hexagon\n"
"e.g. dead_reckoning --hexagon -l 100\n"
"e.g. dead_reckoning --hexagon -l 100 -v 40\n"
"e.g. dead_reckoning -n 6 -a 60 -l 100 -v 40\n\n");
exit(1);
}
// Initialize player
i = 0;
playerc_client_t *client;
playerc_position2d_t *position2d;
// Create a client and connect it to the server.
client = playerc_client_create(NULL, "localhost", 6665);
if (0 != playerc_client_connect(client))
return -1;
// Create and subscribe to a position2d device.
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
return -1;
// Read initial position
playerc_client_read(client);
double start_x = position2d->px;
double start_y = position2d->py;
double start_a = position2d->pa;
// Convert parameters to motor control arguments
double max_vel = 0.01 * v;
int move_time = l * 28000;
move_time *= 50.0 / (double)v;
double max_turn_speed = 1.57; // 90 degrees per second
int turn_time = (int)(((double)a/90.0) * 560000);
int d = -1;
if (reverse_direction) d = 1;
// Move the robot (e.g. square)
double accel_rate = 0.01; // 2 meters per sec^2
double turn_accel_rate = 0.0314; // 360 degrees per sec^2
int cycle_time = 5000; // 5 miliseconds
for (i = 0; i < n; ++i) {
// FORWARD
int acceleration_time = 0;
double vel = 0;
int t = 0;
if (smooth_acceleration) {
// accelerate (up to max speed in 0.25 seconds)
acceleration_time = (max_vel / accel_rate) * cycle_time;
for (; vel < max_vel && t < move_time / 2;
t += cycle_time, vel += accel_rate, usleep(cycle_time))
if (playerc_position2d_set_cmd_vel(position2d, vel, 0, 0, 1)) return -1;
}
//max forward speed
if (playerc_position2d_set_cmd_vel(position2d, max_vel, 0, 0, 1)) return -1;
usleep(move_time - acceleration_time); // not 2x acceleration time in order to make
// up for ground not covered durring acceleration
// (at an average of half of full speed)
if (smooth_acceleration) {
// decelerate
for (; vel >= 0; vel -= accel_rate, usleep(cycle_time))
if (playerc_position2d_set_cmd_vel(position2d, vel, 0, 0, 1)) return -1;
}
// TURN
double turn_speed = 0;
if (smooth_acceleration) {
// accelerate (up to max speed in 0.25 seconds)
acceleration_time = (max_turn_speed / turn_accel_rate) * cycle_time;
for (t = 0; turn_speed < max_turn_speed && t < move_time / 2;
t += cycle_time, turn_speed += turn_accel_rate, usleep(cycle_time))
if (playerc_position2d_set_cmd_vel(position2d, 0, 0, d * turn_speed, 1)) return -1;
}
// max turn speed
if (playerc_position2d_set_cmd_vel(position2d, 0, 0, d * max_turn_speed, 1)) return -1;
usleep(turn_time - acceleration_time);
if (smooth_acceleration) {
// decelerate
for (; turn_speed >= 0; turn_speed -= turn_accel_rate, usleep(cycle_time))
if (playerc_position2d_set_cmd_vel(position2d, 0, 0, d * turn_speed, 1)) return -1;
}
}
// Stop
if (playerc_position2d_set_cmd_vel(position2d, 0, 0, 0, 1)) return -1;
// Read final position, get error
playerc_client_read(client);
double error_x = position2d->px - start_x;
double error_y = position2d->py - start_y;
double error_a = position2d->pa - start_a;
// Report odometry error
printf("\n\nMovement error according to onboard odometry:\n"
"\tX pos: %f cm\n\tY pos: %f cm\n\tAngle: %f degrees\n",
error_x * 100, error_y * 100, error_a * -57.325);
// Shutdown
playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
int main(int argc, const char **argv) {
int i;
int porta = 6665;
double x, y;
char livre;
char end_ip[20];
// OpenCV Variables
char wndname[30] = "Drawing Demo";
int line_type = CV_AA; // change it to 8 to see non-antialiased graphics
CvPoint pt1, pt2;
IplImage* image;
int width = MAX_X, height = MAX_Y; // 200 x 100 pixels
// Player-Stage Variables
playerc_client_t *client;
playerc_position2d_t *position2d;
playerc_laser_t *laser;
// Create a window
image = cvCreateImage(cvSize(width, height), 8, 3);
cvNamedWindow(wndname, 1);
cvZero(image);
pt1.x = 100;
pt1.y = MAX_Y;
pt2.x = 100;
pt2.y = MAX_Y - 80;
cvLine(image, pt1, pt2, CV_RGB(255, 255, 255), 2, line_type, 0);
pt2.x = 20;
pt2.y = MAX_Y;
cvLine(image, pt1, pt2, CV_RGB(255, 255, 255), 2, line_type, 0);
pt2.x = 180;
pt2.y = MAX_Y;
cvLine(image, pt1, pt2, CV_RGB(255, 255, 255), 2, line_type, 0);
cvShowImage(wndname, image);
cvWaitKey(1000);
cvZero(image);
pt1.x = 20;
pt1.y = MAX_Y;
pt2.x = 160;
pt2.y = MAX_Y - 80;
cvRectangle(image, pt1, pt2, CV_RGB(255, 255, 255), 2, line_type, 0);
cvShowImage(wndname, image);
cvWaitKey(1000);
strcpy(end_ip, "localhost");
if (argc >= 2) /* Get Port */
porta = atoi(argv[1]);
if (argc >= 3) /* Get IP Address */
strcpy(end_ip, argv[2]);
printf("Porta: %d\n", porta);
printf("IP: %s\n", end_ip);
client = playerc_client_create(NULL, end_ip, porta);
if (playerc_client_connect(client) != 0)
return -1;
// Connect to Position
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE) != 0) {
fprintf(stderr, "error: %s\n", playerc_error_str());
return -1;
}
// Enable motor control
playerc_position2d_enable(position2d, 1);
// Connect to Laser
laser = playerc_laser_create(client, 0);
if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE))
return -1;
// Read several times the robot data (delay)
playerc_client_read(client);
playerc_client_read(client);
playerc_client_read(client);
playerc_client_read(client);
playerc_client_read(client);
while (1) {
playerc_client_read(client);
// scan for free 100 cm in front of robot
livre = 1;
cvZero(image);
for (i = 0; i < 360; i++) {
if ((laser->scan[i][0]) < 0.5)
livre = 0;
// Debug: if (laser->scan[i][0] <= 0) printf("#");
if (laser->scan[i][0] < 7.8) {
x = laser->scan[i][0] * cos(laser->scan[i][1] + 3.1415926 / 2.0);
y = laser->scan[i][0] * sin(laser->scan[i][1] + 3.1415926 / 2.0);
pt1.x = (int) (x * 10 + 100);
pt1.y = (int) (MAX_Y - y * 10);
cvCircle(image, pt1, 2, CV_RGB(255, 255, 255), 1, line_type, 0);
}
}
cvShowImage(wndname, image);
// if free moves, otherwise turns
if (livre)
playerc_position2d_set_cmd_vel(position2d, 0.2, 0, 0.0, 1);
else
playerc_position2d_set_cmd_vel(position2d, 0.0, 0, 0.4, 1);
cvWaitKey(10);
}
playerc_laser_unsubscribe(laser);
playerc_laser_destroy(laser);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
/*double
randomInt(int low, int high)
{
return low + (high - low) * (rand()/(RAND_MAX * 1.0 ));
}
// Main function for the program*/
int
main(int argc, const char **argv)
{
playerc_client_t *client;
playerc_position2d_t *position2d;
int cycle, index=0;
double dist,angle,fidAngle = 0,lineAngle=0, fidDist=0, prevYaw=0,posAngle=0;
// Create a client and connect it to the server.
client = playerc_client_create(NULL, "localhost", 6665);
if (0 != playerc_client_connect(client))
return -1;
// Create and subscribe to a position2d device.
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
return -1;
// Initiating random walk movement
/*if (0 != playerc_position2d_set_cmd_vel(position2d, randomInt(0.1,1)
,randomInt(0.1,1),DTOR(randomInt(-20,20)) ,1))
return -1;
fprintf(stdout, "robot random positions \n");*/
//looping in random positions
int i;
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 3
,0 , 0,1))
return -1;
usleep(1000);
}
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 0
,0 , 1,1))
return -1;
usleep(1000);
}
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 3
,0 , 0,1))
return -1;
usleep(1000);
}
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 0
,0 , 1,1))
return -1;
usleep(1000);
}
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 3
,0 , 0,1))
return -1;
usleep(1000);
}
while(1)
{if (0 != playerc_position2d_set_cmd_vel(position2d, 0,0 , 0,1))
return -1;
}
playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
int
main(int argc, const char **argv)
{
int i;
playerc_client_t *client;
player_pose2d_t position2d_vel;
player_pose2d_t position2d_target;
playerc_position2d_t *position2d;
// Create a client and connect it to the server.
client = playerc_client_create(NULL, "localhost", 6665);
if (0 != playerc_client_connect(client))
return -1;
// Create and subscribe to a position2d device.
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
return -1;
// Make the robot spin!
if (0 != playerc_position2d_set_cmd_vel(position2d, 0.25, 0, DTOR(40.0), 1))
return -1;
for (i = 0; i < 50; i++)
{
playerc_client_read(client);
// Print current robot pose
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
/* con el comando playerc_position2d_set_cmd_vel() simplemente se establece una
* consigna de velocidad la cual es independiente de la posición en la que se
* encuentra el robot */
position2d_target.px = 2;
position2d_target.py = -3;
position2d_target.pa = 0;
// Move to pose
playerc_position2d_set_cmd_pose(position2d, position2d_target.px , position2d_target.py, position2d_target.pa , 1);
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
// Stop when reach the target
while (sqrt(pow(position2d->px - position2d_target.px,2.0) + pow(position2d->py - position2d_target.py,2.0)) > 0.05 )
{
playerc_client_read(client);
// Print current robot pose
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
/* Con el comando position2d_set_cmd_pose() se establece una pose de destino.
* Cuando se utiliza este método la información obtenida de la odometría sí que
* afecta a la trayectoria seguida y al punto final */
position2d_target.px = 0;
position2d_target.py = -3;
position2d_target.pa = 0;
position2d_vel.px = 0.6;
position2d_vel.py = 0;
position2d_vel.pa = 0;
// Move to pose
playerc_position2d_set_cmd_pose_with_vel(position2d, position2d_target, position2d_vel, 1);
// Stop when reach the target
while (sqrt(pow(position2d->px - position2d_target.px,2.0) + pow(position2d->py - position2d_target.py,2.0)) > 0.05 )
{
playerc_client_read(client);
// Print current robot pose
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
/* Con el comando playerc_position2d_set_cmd_pose_with_vel se hace lo mismo que
* con playerc_position2d_set_cmd_pose() pero además se puede indicar una
* consigna de velocidad */
// Shutdown
playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
