Pr2_Player.cc
Go to the documentation of this file.
00001 #include <libplayerc++/playerc++.h>
00002 #include <iostream>
00003 
00004 int main()
00005 {
00006   // We throw exceptions on creation if we fail
00007   try
00008   {
00009     using namespace PlayerCc;
00010     using namespace std;
00011 
00012     // Create a player client object, using the variables assigned by the
00013     // call to parse_args()
00014     PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM);
00015 
00016     // Subscribe to the simulation proxy
00017     ActArrayProxy ap_l(&robot, 0); // left arm
00018     ActArrayProxy ap_r(&robot, 1); // right arm
00019     ActArrayProxy ap_c(&robot, 2); // caster steering
00020     ActArrayProxy ap_s(&robot, 3); // spine slider
00021     ActArrayProxy ap_h(&robot, 4); // head
00022 
00023     Position2dProxy flc(&robot, 0); // front left caster differential drive
00024     Position2dProxy frc(&robot, 1); // front right caster differential drive
00025     Position2dProxy rlc(&robot, 2); // rear left caster differential drive
00026     Position2dProxy rrc(&robot, 3); // rear right caster differential drive
00027 
00028     // Print out some stuff
00029     std::cout << robot << std::endl;
00030 
00031     flc.SetMotorEnable(true);
00032     frc.SetMotorEnable(true);
00033     rlc.SetMotorEnable(true);
00034     rrc.SetMotorEnable(true);
00035 
00036     for (;;)
00037     {
00038       // This blocks until new data comes
00039       robot.Read();
00040 
00041       flc.SetSpeed(1, 0);
00042       frc.SetSpeed(1, 0);
00043       rlc.SetSpeed(1, 0);
00044       rrc.SetSpeed(1, 0);
00045 
00046       ap_h.MoveTo(0, -M_PI/4.0); // head tilt
00047       ap_h.MoveTo(1,  M_PI/6.0); // head neck yaw
00048       ap_h.MoveTo(2, -M_PI/4.0); // hokuyo pitch
00049       ap_h.MoveTo(3,  M_PI/5.0); // left cam yaw
00050       ap_h.MoveTo(4, -M_PI/3.0); // left cam pitch
00051       ap_h.MoveTo(5, -M_PI/5.0); // right cam yaw
00052       ap_h.MoveTo(6,  M_PI/4.0); // right cam pitch
00053 
00054       ap_l.MoveTo(0,  M_PI/4.0);
00055       ap_l.MoveTo(1,  M_PI/4.0);
00056       ap_l.MoveTo(2,  M_PI/4.0);
00057       ap_l.MoveTo(3,  M_PI/5.0);
00058       ap_l.MoveTo(4,  M_PI/3.0);
00059       ap_l.MoveTo(5,  M_PI/2.0);
00060       ap_l.MoveTo(6,  M_PI/1.0);
00061 
00062       ap_r.MoveTo(0, -M_PI/3.0);
00063       ap_r.MoveTo(1, -M_PI/3.0);
00064       ap_r.MoveTo(2, -M_PI/3.0);
00065       ap_r.MoveTo(3, -M_PI/2.0);
00066       ap_r.MoveTo(4, -M_PI/3.0);
00067       ap_r.MoveTo(5, -M_PI/1.0);
00068       ap_r.MoveTo(6, -M_PI/2.0);
00069 
00070       ap_c.MoveTo(0, -    M_PI/4.0);
00071       ap_c.MoveTo(1, -3.0*M_PI/4.0);
00072       ap_c.MoveTo(2,      M_PI/4.0);
00073       ap_c.MoveTo(3,  3.0*M_PI/4.0);
00074 
00075       ap_s.MoveTo(0,0.5);
00076 
00077       player_actarray_actuator_t data0 = ap_l.GetActuatorData(0);
00078       printf("0 Pos[%f] Speed[%f]\n",data0.position, data0.speed);
00079       player_actarray_actuator_t data1 = ap_l.GetActuatorData(1);
00080       printf("1 Pos[%f] Speed[%f]\n",data1.position, data1.speed);
00081       player_actarray_actuator_t data2 = ap_l.GetActuatorData(2);
00082       printf("2 Pos[%f] Speed[%f]\n",data2.position, data2.speed);
00083     }
00084   }
00085   catch (PlayerCc::PlayerError e)
00086   {
00087     std::cerr << "Error:" << e << std::endl;
00088     return -1;
00089   }
00090 }


pr2_gazebo_plugins
Author(s): John Hsu
autogenerated on Sat Sep 5 2015 11:22:50