p2os_driver: p2os_driver: p2os.cc Source File

00001 /*
00002  *  P2OS for ROS
00003  *  Copyright (C) 2009
00004  *     David Feil-Seifer, Brian Gerkey, Kasper Stoy,
00005  *      Richard Vaughan, & Andrew Howard
00006  *
00007  *
00008  *  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU General Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License
00019  *  along with this program; if not, write to the Free Software
00020  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021  *
00022  */
00023 
00024 #include <ros/ros.h>
00025 #include <p2os_driver/p2os.h>
00026 
00027 #include <termios.h>
00028 #include <fcntl.h>
00029 #include <string.h>
00030 
00031 
00032 P2OSNode::P2OSNode( ros::NodeHandle nh ) :
00033     n(nh), gripper_dirty_(false),
00034     batt_pub_( n.advertise<p2os_driver::BatteryState>("battery_state",1000),
00035                diagnostic_,
00036                diagnostic_updater::FrequencyStatusParam( &desired_freq, &desired_freq, 0.1),
00037                diagnostic_updater::TimeStampStatusParam() ),
00038     ptz_(this)
00039 {
00040   // Use sonar
00041   ros::NodeHandle n_private("~");
00042   n_private.param("use_sonar", use_sonar_, false);
00043 
00044   // read in config options
00045   // bumpstall
00046   n_private.param( "bumpstall", bumpstall, -1 );
00047   // pulse
00048   n_private.param( "pulse", pulse, -1.0 );
00049   // rot_kp
00050   n_private.param( "rot_kp", rot_kp, -1 );
00051   // rot_kv
00052   n_private.param( "rot_kv", rot_kv, -1 );
00053   // rot_ki
00054   n_private.param( "rot_ki", rot_ki, -1 );
00055   // trans_kp
00056   n_private.param( "trans_kp", trans_kp, -1 );
00057   // trans_kv
00058   n_private.param( "trans_kv", trans_kv, -1 );
00059   // trans_ki
00060   n_private.param( "trans_ki", trans_ki, -1 );
00061   // !!! port !!!
00062   std::string def = DEFAULT_P2OS_PORT;
00063   n_private.param( "port", psos_serial_port, def );
00064   ROS_INFO( "using serial port: [%s]", psos_serial_port.c_str() );
00065   n_private.param( "use_tcp", psos_use_tcp, false );
00066   std::string host = DEFAULT_P2OS_TCP_REMOTE_HOST;
00067   n_private.param( "tcp_remote_host", psos_tcp_host, host );
00068   n_private.param( "tcp_remote_port", psos_tcp_port, DEFAULT_P2OS_TCP_REMOTE_PORT );
00069   // radio
00070   n_private.param( "radio", radio_modemp, 0 );
00071   // joystick
00072   n_private.param( "joystick", joystick, 0 );
00073   // direct_wheel_vel_control
00074   n_private.param( "direct_wheel_vel_control", direct_wheel_vel_control, 0 );
00075   // max xpeed
00076   double spd;
00077   n_private.param( "max_xspeed", spd, MOTOR_DEF_MAX_SPEED);
00078   motor_max_speed = (int)rint(1e3*spd);
00079   // max_yawspeed
00080   n_private.param( "max_yawspeed", spd, MOTOR_DEF_MAX_TURNSPEED);
00081   motor_max_turnspeed = (short)rint(RTOD(spd));
00082   // max_xaccel
00083   n_private.param( "max_xaccel", spd, 0.0);
00084   motor_max_trans_accel = (short)rint(1e3*spd);
00085   // max_xdecel
00086   n_private.param( "max_xdecel", spd, 0.0);
00087   motor_max_trans_decel = (short)rint(1e3*spd);
00088   // max_yawaccel
00089   n_private.param( "max_yawaccel", spd, 0.0);
00090   motor_max_rot_accel = (short)rint(RTOD(spd));
00091   // max_yawdecel
00092   n_private.param( "max_yawdecel", spd, 0.0);
00093   motor_max_rot_decel = (short)rint(RTOD(spd));
00094 
00095   desired_freq = 10;
00096 
00097   // advertise services
00098   pose_pub_ = n.advertise<nav_msgs::Odometry>("pose",1000);
00099   mstate_pub_ = n.advertise<p2os_driver::MotorState>("motor_state",1000);
00100   grip_state_pub_ = n.advertise<p2os_driver::GripperState>("gripper_state",1000);
00101   ptz_state_pub_ = n.advertise<p2os_driver::PTZState>("ptz_state",1000);
00102   sonar_pub_ = n.advertise<p2os_driver::SonarArray>("sonar", 1000);
00103   aio_pub_ = n.advertise<p2os_driver::AIO>("aio", 1000);
00104   dio_pub_ = n.advertise<p2os_driver::DIO>("dio", 1000);
00105 
00106   // subscribe to services
00107   cmdvel_sub_ = n.subscribe("cmd_vel", 1, &P2OSNode::cmdvel_cb, this);
00108   cmdmstate_sub_ = n.subscribe("cmd_motor_state", 1, &P2OSNode::cmdmotor_state,
00109                                this);
00110   gripper_sub_ = n.subscribe("gripper_control", 1, &P2OSNode::gripperCallback,
00111                              this);
00112   ptz_cmd_sub_ = n.subscribe("ptz_control", 1, &P2OSPtz::callback, &ptz_);
00113 
00114   veltime = ros::Time::now();
00115 
00116         // add diagnostic functions
00117         diagnostic_.add("Motor Stall", this, &P2OSNode::check_stall );
00118         diagnostic_.add("Battery Voltage", this, &P2OSNode::check_voltage );
00119 
00120   // initialize robot parameters (player legacy)
00121   initialize_robot_params();
00122 }
00123 
00124 P2OSNode::~P2OSNode()
00125 {
00126 }
00127 
00128 void
00129 P2OSNode::cmdmotor_state( const p2os_driver::MotorStateConstPtr &msg)
00130 {
00131   motor_dirty = true;
00132   cmdmotor_state_ = *msg;
00133 }
00134 
00135 void
00136 P2OSNode::check_and_set_motor_state()
00137 {
00138   if( !motor_dirty ) return;
00139   motor_dirty = false;
00140 
00141   unsigned char val = (unsigned char) cmdmotor_state_.state;
00142   unsigned char command[4];
00143   P2OSPacket packet;
00144   command[0] = ENABLE;
00145   command[1] = ARGINT;
00146   command[2] = val;
00147   command[3] = 0;
00148   packet.Build(command,4);
00149 
00150   // Store the current motor state so that we can set it back
00151   p2os_data.motors.state = cmdmotor_state_.state;
00152   SendReceive(&packet,false);
00153 }
00154 
00155 void
00156 P2OSNode::check_and_set_gripper_state()
00157 {
00158   if( !gripper_dirty_ ) return;
00159   gripper_dirty_ = false;
00160 
00161   // Send the gripper command
00162   unsigned char grip_val = (unsigned char) gripper_state_.grip.state;
00163   unsigned char grip_command[4];
00164   P2OSPacket grip_packet;
00165   grip_command[0] = GRIPPER;
00166   grip_command[1] = ARGINT;
00167   grip_command[2] = grip_val;
00168   grip_command[3] = 0;
00169   grip_packet.Build(grip_command,4);
00170   SendReceive(&grip_packet,false);
00171 
00172   // Send the lift command
00173   unsigned char lift_val = (unsigned char) gripper_state_.lift.state;
00174   unsigned char lift_command[4];
00175   P2OSPacket lift_packet;
00176   lift_command[0] = GRIPPER;
00177   lift_command[1] = ARGINT;
00178   lift_command[2] = lift_val;
00179   lift_command[3] = 0;
00180   lift_packet.Build(lift_command,4);
00181   SendReceive(&lift_packet,false);
00182 }
00183 
00184 void
00185 P2OSNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
00186 {
00187 
00188   if( fabs( msg->linear.x - cmdvel_.linear.x ) > 0.01 || fabs( msg->angular.z-cmdvel_.angular.z) > 0.01 )
00189   {
00190     veltime = ros::Time::now();
00191     ROS_DEBUG( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
00192     vel_dirty = true;
00193     cmdvel_ = *msg;
00194   }
00195   else
00196   {
00197     ros::Duration veldur = ros::Time::now() - veltime;
00198     if( veldur.toSec() > 2.0 && ((fabs(cmdvel_.linear.x) > 0.01) || (fabs(cmdvel_.angular.z) > 0.01)) )
00199     {
00200       ROS_DEBUG( "maintaining old speed: %0.3f|%0.3f", veltime.toSec(), ros::Time::now().toSec() );
00201       vel_dirty = true;
00202       veltime = ros::Time::now();
00203     }
00204   }
00205 
00206 }
00207 
00208 void
00209 P2OSNode::check_and_set_vel()
00210 {
00211   if( !vel_dirty ) return;
00212 
00213   ROS_DEBUG( "setting vel: [%0.2f,%0.2f]",cmdvel_.linear.x,cmdvel_.angular.z);
00214   vel_dirty = false;
00215 
00216   unsigned short absSpeedDemand, absturnRateDemand;
00217   unsigned char motorcommand[4];
00218   P2OSPacket motorpacket;
00219 
00220   int vx = (int) (cmdvel_.linear.x*1e3);
00221   int va = (int)rint(RTOD(cmdvel_.angular.z));
00222 
00223   {
00224     // non-direct wheel control
00225     motorcommand[0] = VEL;
00226     if( vx >= 0 ) motorcommand[1] = ARGINT;
00227     else motorcommand[1] = ARGNINT;
00228 
00229     absSpeedDemand = (unsigned short)abs(vx);
00230     if( absSpeedDemand <= this->motor_max_speed )
00231     {
00232       motorcommand[2] = absSpeedDemand & 0x00FF;
00233       motorcommand[3] = (absSpeedDemand & 0xFF00) >> 8;
00234     }
00235     else
00236     {
00237       ROS_WARN( "speed demand thresholded! (true: %u, max: %u)", absSpeedDemand, motor_max_speed );
00238       motorcommand[2] = motor_max_speed & 0x00FF;
00239       motorcommand[3] = (motor_max_speed & 0xFF00) >> 8;
00240     }
00241     motorpacket.Build(motorcommand, 4);
00242     SendReceive(&motorpacket);
00243 
00244     motorcommand[0] = RVEL;
00245     if( va >= 0 ) motorcommand[1] = ARGINT;
00246     else motorcommand[1] = ARGNINT;
00247 
00248     absturnRateDemand = (unsigned short)abs(va);
00249     if( absturnRateDemand <= motor_max_turnspeed )
00250     {
00251       motorcommand[2] = absturnRateDemand & 0x00FF;
00252       motorcommand[3] = (absturnRateDemand & 0xFF00) >> 8;
00253     }
00254     else
00255     {
00256       ROS_WARN("Turn rate demand threshholded!");
00257       motorcommand[2] = this->motor_max_turnspeed & 0x00FF;
00258       motorcommand[3] = (this->motor_max_turnspeed & 0xFF00) >> 8;
00259     }
00260 
00261     motorpacket.Build(motorcommand,4);
00262     SendReceive(&motorpacket);
00263   }
00264 }
00265 
00266 void P2OSNode::gripperCallback(const p2os_driver::GripperStateConstPtr &msg)
00267 {
00268   gripper_dirty_ = true;
00269   gripper_state_ = *msg;
00270 }
00271 
00272 int
00273 P2OSNode::Setup()
00274 {
00275   int i;
00276   int bauds[] = {B9600, B38400, B19200, B115200, B57600};
00277   int numbauds = sizeof(bauds);
00278   int currbaud = 0;
00279   sippacket = NULL;
00280   lastPulseTime = 0.0;
00281 
00282   struct termios term;
00283   unsigned char command;
00284   P2OSPacket packet, receivedpacket;
00285   int flags=0;
00286   bool sent_close = false;
00287   enum
00288   {
00289     NO_SYNC,
00290     AFTER_FIRST_SYNC,
00291     AFTER_SECOND_SYNC,
00292     READY
00293   } psos_state;
00294 
00295   psos_state = NO_SYNC;
00296 
00297   char name[20], type[20], subtype[20];
00298   int cnt;
00299 
00300 
00301   // use serial port
00302 
00303   ROS_INFO("P2OS connection opening serial port %s...",psos_serial_port.c_str());
00304 
00305   if((this->psos_fd = open(this->psos_serial_port.c_str(),
00306                    O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 )
00307   {
00308     ROS_ERROR("P2OS::Setup():open():");
00309     return(1);
00310   }
00311 
00312   if(tcgetattr( this->psos_fd, &term ) < 0 )
00313   {
00314     ROS_ERROR("P2OS::Setup():tcgetattr():");
00315     close(this->psos_fd);
00316     this->psos_fd = -1;
00317     return(1);
00318   }
00319 
00320   cfmakeraw( &term );
00321   cfsetispeed(&term, bauds[currbaud]);
00322   cfsetospeed(&term, bauds[currbaud]);
00323 
00324   if(tcsetattr(this->psos_fd, TCSAFLUSH, &term ) < 0)
00325   {
00326     ROS_ERROR("P2OS::Setup():tcsetattr():");
00327     close(this->psos_fd);
00328     this->psos_fd = -1;
00329     return(1);
00330   }
00331 
00332   if(tcflush(this->psos_fd, TCIOFLUSH ) < 0)
00333   {
00334     ROS_ERROR("P2OS::Setup():tcflush():");
00335     close(this->psos_fd);
00336     this->psos_fd = -1;
00337     return(1);
00338   }
00339 
00340   if((flags = fcntl(this->psos_fd, F_GETFL)) < 0)
00341   {
00342     ROS_ERROR("P2OS::Setup():fcntl()");
00343     close(this->psos_fd);
00344     this->psos_fd = -1;
00345     return(1);
00346   }
00347   // Sync:
00348 
00349   int num_sync_attempts = 3;
00350   while(psos_state != READY)
00351   {
00352     switch(psos_state)
00353     {
00354       case NO_SYNC:
00355         command = SYNC0;
00356         packet.Build(&command, 1);
00357         packet.Send(this->psos_fd);
00358         usleep(P2OS_CYCLETIME_USEC);
00359         break;
00360       case AFTER_FIRST_SYNC:
00361         ROS_INFO("turning off NONBLOCK mode...");
00362         if(fcntl(this->psos_fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
00363         {
00364           ROS_ERROR("P2OS::Setup():fcntl()");
00365           close(this->psos_fd);
00366           this->psos_fd = -1;
00367           return(1);
00368         }
00369         command = SYNC1;
00370         packet.Build(&command, 1);
00371         packet.Send(this->psos_fd);
00372         break;
00373       case AFTER_SECOND_SYNC:
00374         command = SYNC2;
00375         packet.Build(&command, 1);
00376         packet.Send(this->psos_fd);
00377         break;
00378       default:
00379         ROS_WARN("P2OS::Setup():shouldn't be here...");
00380         break;
00381     }
00382     usleep(P2OS_CYCLETIME_USEC);
00383 
00384     if(receivedpacket.Receive(this->psos_fd))
00385     {
00386       if((psos_state == NO_SYNC) && (num_sync_attempts >= 0))
00387       {
00388         num_sync_attempts--;
00389         usleep(P2OS_CYCLETIME_USEC);
00390         continue;
00391       }
00392       else
00393       {
00394         // couldn't connect; try different speed.
00395         if(++currbaud < numbauds)
00396         {
00397           cfsetispeed(&term, bauds[currbaud]);
00398           cfsetospeed(&term, bauds[currbaud]);
00399           if( tcsetattr(this->psos_fd, TCSAFLUSH, &term ) < 0 )
00400           {
00401             ROS_ERROR("P2OS::Setup():tcsetattr():");
00402             close(this->psos_fd);
00403             this->psos_fd = -1;
00404             return(1);
00405           }
00406 
00407           if(tcflush(this->psos_fd, TCIOFLUSH ) < 0 )
00408           {
00409             ROS_ERROR("P2OS::Setup():tcflush():");
00410             close(this->psos_fd);
00411             this->psos_fd = -1;
00412             return(1);
00413           }
00414           num_sync_attempts = 3;
00415           continue;
00416         }
00417         else
00418         {
00419           // tried all speeds; bail
00420           break;
00421         }
00422       }
00423     }
00424     switch(receivedpacket.packet[3])
00425     {
00426       case SYNC0:
00427         ROS_INFO( "SYNC0" );
00428         psos_state = AFTER_FIRST_SYNC;
00429         break;
00430       case SYNC1:
00431         ROS_INFO( "SYNC1" );
00432         psos_state = AFTER_SECOND_SYNC;
00433         break;
00434       case SYNC2:
00435         ROS_INFO( "SYNC2" );
00436         psos_state = READY;
00437         break;
00438       default:
00439         // maybe P2OS is still running from last time.  let's try to CLOSE
00440         // and reconnect
00441         if(!sent_close)
00442         {
00443           ROS_DEBUG("sending CLOSE");
00444           command = CLOSE;
00445           packet.Build( &command, 1);
00446           packet.Send(this->psos_fd);
00447           sent_close = true;
00448           usleep(2*P2OS_CYCLETIME_USEC);
00449           tcflush(this->psos_fd,TCIFLUSH);
00450           psos_state = NO_SYNC;
00451         }
00452         break;
00453     }
00454     usleep(P2OS_CYCLETIME_USEC);
00455   }
00456   if(psos_state != READY)
00457   {
00458     if(this->psos_use_tcp)
00459     ROS_INFO("Couldn't synchronize with P2OS.\n"
00460            "  Most likely because the robot is not connected %s %s",
00461            this->psos_use_tcp ? "to the ethernet-serial bridge device " : "to the serial port",
00462            this->psos_use_tcp ? this->psos_tcp_host.c_str() : this->psos_serial_port.c_str());
00463     close(this->psos_fd);
00464     this->psos_fd = -1;
00465     return(1);
00466   }
00467   cnt = 4;
00468   cnt += snprintf(name, sizeof(name), "%s", &receivedpacket.packet[cnt]);
00469   cnt++;
00470   cnt += snprintf(type, sizeof(type), "%s", &receivedpacket.packet[cnt]);
00471   cnt++;
00472   cnt += snprintf(subtype, sizeof(subtype), "%s", &receivedpacket.packet[cnt]);
00473   cnt++;
00474 
00475   std::string hwID = std::string( name ) + std::string(": ") + std::string(type) + std::string("/") + std::string( subtype );
00476   diagnostic_.setHardwareID(hwID);
00477 
00478   command = OPEN;
00479   packet.Build(&command, 1);
00480   packet.Send(this->psos_fd);
00481   usleep(P2OS_CYCLETIME_USEC);
00482   command = PULSE;
00483   packet.Build(&command, 1);
00484   packet.Send(this->psos_fd);
00485   usleep(P2OS_CYCLETIME_USEC);
00486 
00487   ROS_INFO("Done: Connected to %s, a %s %s", name, type, subtype);
00488 
00489   // now, based on robot type, find the right set of parameters
00490   for(i=0;i<PLAYER_NUM_ROBOT_TYPES;i++)
00491   {
00492     if(!strcasecmp(PlayerRobotParams[i].Class.c_str(),type) &&
00493        !strcasecmp(PlayerRobotParams[i].Subclass.c_str(),subtype))
00494     {
00495       //ROS_INFO( "P2OS: using params: %d", i );
00496       param_idx = i;
00497       break;
00498     }
00499   }
00500   if(i == PLAYER_NUM_ROBOT_TYPES)
00501   {
00502     ROS_WARN("P2OS: Warning: couldn't find parameters for this robot; "
00503             "using defaults");
00504     param_idx = 0;
00505   }
00506 
00507   //sleep(1);
00508 
00509   // first, receive a packet so we know we're connected.
00510   if(!sippacket)
00511   {
00512     sippacket = new SIP(param_idx);
00513   }
00514 /*
00515   sippacket->x_offset = 0;
00516   sippacket->y_offset = 0;
00517   sippacket->angle_offset = 0;
00518 
00519   SendReceive((P2OSPacket*)NULL,false);
00520 */
00521   // turn off the sonars at first
00522   this->ToggleSonarPower(0);
00523   // if requested, set max accel/decel limits
00524   P2OSPacket accel_packet;
00525   unsigned char accel_command[4];
00526   if(this->motor_max_trans_accel > 0)
00527   {
00528     accel_command[0] = SETA;
00529     accel_command[1] = ARGINT;
00530     accel_command[2] = this->motor_max_trans_accel & 0x00FF;
00531     accel_command[3] = (this->motor_max_trans_accel & 0xFF00) >> 8;
00532     accel_packet.Build(accel_command, 4);
00533     this->SendReceive(&accel_packet,false);
00534   }
00535 
00536   if(this->motor_max_trans_decel < 0)
00537   {
00538     accel_command[0] = SETA;
00539     accel_command[1] = ARGNINT;
00540     accel_command[2] = abs(this->motor_max_trans_decel) & 0x00FF;
00541     accel_command[3] = (abs(this->motor_max_trans_decel) & 0xFF00) >> 8;
00542     accel_packet.Build(accel_command, 4);
00543     this->SendReceive(&accel_packet,false);
00544   }
00545   if(this->motor_max_rot_accel > 0)
00546   {
00547     accel_command[0] = SETRA;
00548     accel_command[1] = ARGINT;
00549     accel_command[2] = this->motor_max_rot_accel & 0x00FF;
00550     accel_command[3] = (this->motor_max_rot_accel & 0xFF00) >> 8;
00551     accel_packet.Build(accel_command, 4);
00552     this->SendReceive(&accel_packet,false);
00553   }
00554   if(this->motor_max_rot_decel < 0)
00555   {
00556     accel_command[0] = SETRA;
00557     accel_command[1] = ARGNINT;
00558     accel_command[2] = abs(this->motor_max_rot_decel) & 0x00FF;
00559     accel_command[3] = (abs(this->motor_max_rot_decel) & 0xFF00) >> 8;
00560     accel_packet.Build(accel_command, 4);
00561     this->SendReceive(&accel_packet,false);
00562   }
00563 
00564 
00565   // if requested, change PID settings
00566   P2OSPacket pid_packet;
00567   unsigned char pid_command[4];
00568   if(this->rot_kp >= 0)
00569   {
00570     pid_command[0] = ROTKP;
00571     pid_command[1] = ARGINT;
00572     pid_command[2] = this->rot_kp & 0x00FF;
00573     pid_command[3] = (this->rot_kp & 0xFF00) >> 8;
00574     pid_packet.Build(pid_command, 4);
00575     this->SendReceive(&pid_packet);
00576   }
00577   if(this->rot_kv >= 0)
00578   {
00579     pid_command[0] = ROTKV;
00580     pid_command[1] = ARGINT;
00581     pid_command[2] = this->rot_kv & 0x00FF;
00582     pid_command[3] = (this->rot_kv & 0xFF00) >> 8;
00583     pid_packet.Build(pid_command, 4);
00584     this->SendReceive(&pid_packet);
00585   }
00586   if(this->rot_ki >= 0)
00587   {
00588     pid_command[0] = ROTKI;
00589     pid_command[1] = ARGINT;
00590     pid_command[2] = this->rot_ki & 0x00FF;
00591     pid_command[3] = (this->rot_ki & 0xFF00) >> 8;
00592     pid_packet.Build(pid_command, 4);
00593     this->SendReceive(&pid_packet);
00594   }
00595   if(this->trans_kp >= 0)
00596   {
00597     pid_command[0] = TRANSKP;
00598     pid_command[1] = ARGINT;
00599     pid_command[2] = this->trans_kp & 0x00FF;
00600     pid_command[3] = (this->trans_kp & 0xFF00) >> 8;
00601     pid_packet.Build(pid_command, 4);
00602     this->SendReceive(&pid_packet);
00603   }
00604   if(this->trans_kv >= 0)
00605   {
00606     pid_command[0] = TRANSKV;
00607     pid_command[1] = ARGINT;
00608     pid_command[2] = this->trans_kv & 0x00FF;
00609     pid_command[3] = (this->trans_kv & 0xFF00) >> 8;
00610     pid_packet.Build(pid_command, 4);
00611     this->SendReceive(&pid_packet);
00612   }
00613   if(this->trans_ki >= 0)
00614   {
00615     pid_command[0] = TRANSKI;
00616     pid_command[1] = ARGINT;
00617     pid_command[2] = this->trans_ki & 0x00FF;
00618     pid_command[3] = (this->trans_ki & 0xFF00) >> 8;
00619     pid_packet.Build(pid_command, 4);
00620     this->SendReceive(&pid_packet);
00621   }
00622 
00623 
00624   // if requested, change bumper-stall behavior
00625   // 0 = don't stall
00626   // 1 = stall on front bumper contact
00627   // 2 = stall on rear bumper contact
00628   // 3 = stall on either bumper contact
00629   if(this->bumpstall >= 0)
00630   {
00631     if(this->bumpstall > 3)
00632       ROS_INFO("ignoring bumpstall value %d; should be 0, 1, 2, or 3",
00633                     this->bumpstall);
00634     else
00635     {
00636       ROS_INFO("setting bumpstall to %d", this->bumpstall);
00637       P2OSPacket bumpstall_packet;;
00638       unsigned char bumpstall_command[4];
00639       bumpstall_command[0] = BUMP_STALL;
00640       bumpstall_command[1] = ARGINT;
00641       bumpstall_command[2] = (unsigned char)this->bumpstall;
00642       bumpstall_command[3] = 0;
00643       bumpstall_packet.Build(bumpstall_command, 4);
00644       this->SendReceive(&bumpstall_packet,false);
00645     }
00646   }
00647 
00648   // Turn on the sonar
00649   if(use_sonar_) {
00650     this->ToggleSonarPower(1);
00651     ROS_DEBUG("Sonar array powered on.");
00652   }
00653   ptz_.setup();
00654 
00655   return(0);
00656 }
00657 
00658 int
00659 P2OSNode::Shutdown()
00660 {
00661   unsigned char command[20],buffer[20];
00662   P2OSPacket packet;
00663 
00664   if (ptz_.isOn())
00665   {
00666     ptz_.shutdown();
00667   }
00668 
00669   memset(buffer,0,20);
00670 
00671   if(this->psos_fd == -1)
00672     return -1;
00673 
00674   command[0] = STOP;
00675   packet.Build(command, 1);
00676   packet.Send(this->psos_fd);
00677   usleep(P2OS_CYCLETIME_USEC);
00678 
00679   command[0] = CLOSE;
00680   packet.Build(command, 1);
00681   packet.Send(this->psos_fd);
00682   usleep(P2OS_CYCLETIME_USEC);
00683 
00684   close(this->psos_fd);
00685   this->psos_fd = -1;
00686   ROS_INFO("P2OS has been shutdown");
00687   delete this->sippacket;
00688   this->sippacket = NULL;
00689 
00690   return 0;
00691 }
00692 
00693 
00694 void
00695 P2OSNode::StandardSIPPutData(ros::Time ts)
00696 {
00697 
00698   p2os_data.position.header.stamp = ts;
00699   pose_pub_.publish( p2os_data.position );
00700   p2os_data.odom_trans.header.stamp = ts;
00701   odom_broadcaster.sendTransform( p2os_data.odom_trans );
00702 
00703   p2os_data.batt.header.stamp = ts;
00704   batt_pub_.publish( p2os_data.batt );
00705   mstate_pub_.publish( p2os_data.motors );
00706 
00707   // put sonar data
00708   p2os_data.sonar.header.stamp = ts;
00709   sonar_pub_.publish( p2os_data.sonar );
00710 
00711   // put aio data
00712   aio_pub_.publish( p2os_data.aio);
00713   // put dio data
00714   dio_pub_.publish( p2os_data.dio);
00715 
00716   // put gripper and lift data
00717   grip_state_pub_.publish( p2os_data.gripper );
00718   ptz_state_pub_.publish( ptz_.getCurrentState() );
00719 
00720   // put bumper data
00721   // put compass data
00722 
00723 }
00724 
00725 /* send the packet, then receive and parse an SIP */
00726 int
00727 P2OSNode::SendReceive(P2OSPacket* pkt, bool publish_data)
00728 {
00729   P2OSPacket packet;
00730 
00731   if((this->psos_fd >= 0) && this->sippacket)
00732   {
00733     if(pkt)
00734       pkt->Send(this->psos_fd);
00735 
00736     /* receive a packet */
00737     pthread_testcancel();
00738     if(packet.Receive(this->psos_fd))
00739     {
00740       ROS_ERROR("RunPsosThread(): Receive errored");
00741       pthread_exit(NULL);
00742     }
00743 
00744     if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
00745        (packet.packet[3] == 0x30 || packet.packet[3] == 0x31 ||
00746        packet.packet[3] == 0x32 || packet.packet[3] == 0x33 ||
00747        packet.packet[3] == 0x34))
00748     {
00749 
00750       /* It is a server packet, so process it */
00751       this->sippacket->ParseStandard( &packet.packet[3] );
00752       this->sippacket->FillStandard(&(this->p2os_data));
00753 
00754       if(publish_data)
00755         this->StandardSIPPutData(packet.timestamp);
00756     }
00757     else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
00758             packet.packet[3] == SERAUX)
00759     {
00760       // This is an AUX serial packet
00761       if(ptz_.isOn())
00762       {
00763         int len = packet.packet[2] - 3;
00764         if (ptz_.cb_.gotPacket())
00765         {
00766           ROS_ERROR("PTZ got a message, but alread has the complete packet.");
00767         }
00768         else
00769         {
00770           for (int i=4; i < 4+len; ++i)
00771           {
00772             ptz_.cb_.putOnBuf(packet.packet[i]);
00773           }
00774         }
00775       }
00776     }
00777     else
00778     {
00779       ROS_ERROR("Received other packet!");
00780       packet.PrintHex();
00781     }
00782   }
00783 
00784   return(0);
00785 }
00786 
00787 void
00788 P2OSNode::updateDiagnostics()
00789 {
00790   diagnostic_.update();
00791 }
00792 
00793 void
00794 P2OSNode::check_voltage( diagnostic_updater::DiagnosticStatusWrapper &stat )
00795 {
00796         double voltage = sippacket->battery / 10.0;
00797         if( voltage < 11.0 )
00798         {
00799                 stat.summary( diagnostic_msgs::DiagnosticStatus::ERROR, "battery voltage critically low" );
00800         }
00801         else if( voltage < 11.75 )
00802         {
00803                 stat.summary( diagnostic_msgs::DiagnosticStatus::WARN, "battery voltage getting low" );
00804 
00805         }
00806         else stat.summary( diagnostic_msgs::DiagnosticStatus::OK, "battery voltage OK" );
00807 
00808         stat.add("voltage", voltage );
00809 }
00810 
00811 void
00812 P2OSNode::check_stall( diagnostic_updater::DiagnosticStatusWrapper &stat )
00813 {
00814         if( sippacket->lwstall || sippacket->rwstall )
00815         {
00816                 stat.summary( diagnostic_msgs::DiagnosticStatus::ERROR, "wheel stalled" );
00817         }
00818         else stat.summary( diagnostic_msgs::DiagnosticStatus::OK, "no wheel stall" );
00819 
00820         stat.add("left wheel stall", sippacket->lwstall );
00821         stat.add("right wheel stall", sippacket->rwstall );
00822 }
00823 
00824 void
00825 P2OSNode::ResetRawPositions()
00826 {
00827   P2OSPacket pkt;
00828   unsigned char p2oscommand[4];
00829 
00830   if(this->sippacket)
00831   {
00832     this->sippacket->rawxpos = 0;
00833     this->sippacket->rawypos = 0;
00834     this->sippacket->xpos = 0;
00835     this->sippacket->ypos = 0;
00836     p2oscommand[0] = SETO;
00837     p2oscommand[1] = ARGINT;
00838     pkt.Build(p2oscommand, 2);
00839     this->SendReceive(&pkt,false);
00840     ROS_INFO("resetting raw positions" );
00841   }
00842 }
00843 
00844 /* toggle sonars on/off, according to val */
00845 void
00846 P2OSNode::ToggleSonarPower(unsigned char val)
00847 {
00848   unsigned char command[4];
00849   P2OSPacket packet;
00850 
00851   command[0] = SONAR;
00852   command[1] = ARGINT;
00853   command[2] = val;
00854   command[3] = 0;
00855   packet.Build(command, 4);
00856   SendReceive(&packet,false);
00857 }
00858 
00859 /* toggle motors on/off, according to val */
00860 void
00861 P2OSNode::ToggleMotorPower(unsigned char val)
00862 {
00863   unsigned char command[4];
00864   P2OSPacket packet;
00865   ROS_INFO( "motor state: %d\n", p2os_data.motors.state );
00866   p2os_data.motors.state = (int) val;
00867   command[0] = ENABLE;
00868   command[1] = ARGINT;
00869   command[2] = val;
00870   command[3] = 0;
00871   packet.Build(command, 4);
00872   SendReceive(&packet,false);
00873 }
00874 
00876 //  Actarray stuff
00878 
00879 // Ticks to degrees from the ARIA software
00880 inline double P2OSNode::TicksToDegrees (int joint, unsigned char ticks)
00881 {
00882   if ((joint < 0) || (joint >= sippacket->armNumJoints))
00883     return 0;
00884 
00885   double result;
00886   int pos = ticks - sippacket->armJoints[joint].centre;
00887   result = 90.0 / static_cast<double> (sippacket->armJoints[joint].ticksPer90);
00888   result = result * pos;
00889   if ((joint >= 0) && (joint <= 2))
00890     result = -result;
00891 
00892   return result;
00893 }
00894 
00895 // Degrees to ticks from the ARIA software
00896 inline unsigned char P2OSNode::DegreesToTicks (int joint, double degrees)
00897 {
00898   double val;
00899 
00900   if ((joint < 0) || (joint >= sippacket->armNumJoints))
00901     return 0;
00902 
00903   val = static_cast<double> (sippacket->armJoints[joint].ticksPer90) * degrees / 90.0;
00904   val = round (val);
00905   if ((joint >= 0) && (joint <= 2))
00906     val = -val;
00907   val += sippacket->armJoints[joint].centre;
00908 
00909   if (val < sippacket->armJoints[joint].min)
00910     return sippacket->armJoints[joint].min;
00911   else if (val > sippacket->armJoints[joint].max)
00912     return sippacket->armJoints[joint].max;
00913   else
00914     return static_cast<int> (round (val));
00915 }
00916 
00917 inline double P2OSNode::TicksToRadians (int joint, unsigned char ticks)
00918 {
00919   double result = DTOR (TicksToDegrees (joint, ticks));
00920   return result;
00921 }
00922 
00923 inline unsigned char P2OSNode::RadiansToTicks (int joint, double rads)
00924 {
00925   unsigned char result = static_cast<unsigned char> (DegreesToTicks (joint, RTOD (rads)));
00926   return result;
00927 }
00928 
00929 inline double P2OSNode::RadsPerSectoSecsPerTick (int joint, double speed)
00930 {
00931   double degs = RTOD (speed);
00932   double ticksPerDeg = static_cast<double> (sippacket->armJoints[joint].ticksPer90) / 90.0f;
00933   double ticksPerSec = degs * ticksPerDeg;
00934   double secsPerTick = 1000.0f / ticksPerSec;
00935 
00936   if (secsPerTick > 127)
00937     return 127;
00938   else if (secsPerTick < 1)
00939     return 1;
00940   return secsPerTick;
00941 }
00942 
00943 inline double P2OSNode::SecsPerTicktoRadsPerSec (int joint, double msecs)
00944 {
00945   double ticksPerSec = 1.0 / (static_cast<double> (msecs) / 1000.0);
00946   double ticksPerDeg = static_cast<double> (sippacket->armJoints[joint].ticksPer90) / 90.0f;
00947   double degs = ticksPerSec / ticksPerDeg;
00948   double rads = DTOR (degs);
00949 
00950   return rads;
00951 }
00952 
00953 void P2OSNode::SendPulse (void)
00954 {
00955   unsigned char command;
00956   P2OSPacket packet;
00957 
00958   command = PULSE;
00959   packet.Build(&command, 1);
00960   SendReceive(&packet);
00961 }