p2os.cc
Go to the documentation of this file.
00001 /*
00002  *  P2OS for ROS
00003  *  Copyright (C) 2009
00004  *     Hunter Allen, 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 #include <termios.h>
00024 #include <fcntl.h>
00025 #include <string.h>
00026  
00027 #include <ros/ros.h>
00028 #include <p2os_driver/p2os.h>
00029 
00030 P2OSNode::P2OSNode( ros::NodeHandle nh ) :
00031     n(nh), gripper_dirty_(false),
00032     batt_pub_( n.advertise<p2os_msgs::BatteryState>("battery_state",1000),
00033                diagnostic_,
00034                diagnostic_updater::FrequencyStatusParam( &desired_freq, &desired_freq, 0.1),
00035                diagnostic_updater::TimeStampStatusParam() ),
00036     ptz_(this)
00037 {
00043     // Use sonar
00044     ros::NodeHandle n_private("~");
00045     n_private.param(std::string("odom_frame_id"), odom_frame_id,std::string("odom"));
00046     n_private.param(std::string("base_link_frame_id"), base_link_frame_id,std::string("base_link"));
00047     n_private.param("use_sonar", use_sonar_, false);
00048     
00049     // read in config options
00050     // bumpstall
00051     n_private.param( "bumpstall", bumpstall, -1 );
00052     // pulse
00053     n_private.param( "pulse", pulse, -1.0 );
00054     // rot_kp
00055     n_private.param( "rot_kp", rot_kp, -1 );
00056     // rot_kv
00057     n_private.param( "rot_kv", rot_kv, -1 );
00058     // rot_ki
00059     n_private.param( "rot_ki", rot_ki, -1 );
00060     // trans_kp
00061     n_private.param( "trans_kp", trans_kp, -1 );
00062     // trans_kv
00063     n_private.param( "trans_kv", trans_kv, -1 );
00064     // trans_ki
00065     n_private.param( "trans_ki", trans_ki, -1 );
00066     // !!! port !!!
00067     std::string def = DEFAULT_P2OS_PORT;
00068     n_private.param( "port", psos_serial_port, def );
00069     ROS_INFO( "using serial port: [%s]", psos_serial_port.c_str() );
00070     n_private.param( "use_tcp", psos_use_tcp, false );
00071     std::string host = DEFAULT_P2OS_TCP_REMOTE_HOST;
00072     n_private.param( "tcp_remote_host", psos_tcp_host, host );
00073     n_private.param( "tcp_remote_port", psos_tcp_port, DEFAULT_P2OS_TCP_REMOTE_PORT );
00074     // radio
00075     n_private.param( "radio", radio_modemp, 0 );
00076     // joystick
00077     n_private.param( "joystick", joystick, 0 );
00078     // direct_wheel_vel_control
00079     n_private.param( "direct_wheel_vel_control", direct_wheel_vel_control, 0 );
00080     // max xpeed
00081     double spd;
00082     n_private.param( "max_xspeed", spd, MOTOR_DEF_MAX_SPEED);
00083     motor_max_speed = (int)rint(1e3*spd);
00084     // max_yawspeed
00085     n_private.param( "max_yawspeed", spd, MOTOR_DEF_MAX_TURNSPEED);
00086     motor_max_turnspeed = (short)rint(RTOD(spd));
00087     // max_xaccel
00088     n_private.param( "max_xaccel", spd, 0.0);
00089     motor_max_trans_accel = (short)rint(1e3*spd);
00090     // max_xdecel
00091     n_private.param( "max_xdecel", spd, 0.0);
00092     motor_max_trans_decel = (short)rint(1e3*spd);
00093     // max_yawaccel
00094     n_private.param( "max_yawaccel", spd, 0.0);
00095     motor_max_rot_accel = (short)rint(RTOD(spd));
00096     // max_yawdecel
00097     n_private.param( "max_yawdecel", spd, 0.0);
00098     motor_max_rot_decel = (short)rint(RTOD(spd));
00099     
00100     desired_freq = 10;
00101     
00102     // advertise services
00103     pose_pub_ = n.advertise<nav_msgs::Odometry>("pose", 1000);
00104     //advertise topics
00105     mstate_pub_ = n.advertise<p2os_msgs::MotorState>("motor_state",1000);
00106     grip_state_pub_ = n.advertise<p2os_msgs::GripperState>("gripper_state",1000);
00107     ptz_state_pub_ = n.advertise<p2os_msgs::PTZState>("ptz_state",1000);
00108     sonar_pub_ = n.advertise<p2os_msgs::SonarArray>("sonar", 1000);
00109     aio_pub_ = n.advertise<p2os_msgs::AIO>("aio", 1000);
00110     dio_pub_ = n.advertise<p2os_msgs::DIO>("dio", 1000);
00111     
00112     // subscribe to services
00113     cmdvel_sub_ = n.subscribe("cmd_vel", 1, &P2OSNode::cmdvel_cb, this);
00114     cmdmstate_sub_ = n.subscribe("cmd_motor_state", 1, &P2OSNode::cmdmotor_state,
00115                                  this);
00116     gripper_sub_ = n.subscribe("gripper_control", 1, &P2OSNode::gripperCallback,
00117                                this);
00118     ptz_cmd_sub_ = n.subscribe("ptz_control", 1, &P2OSPtz::callback, &ptz_);
00119     
00120     veltime = ros::Time::now();
00121     
00122     // add diagnostic functions
00123     diagnostic_.add("Motor Stall", this, &P2OSNode::check_stall );
00124     diagnostic_.add("Battery Voltage", this, &P2OSNode::check_voltage );
00125     
00126     // initialize robot parameters (player legacy)
00127     initialize_robot_params();
00128 }
00129 
00130 P2OSNode::~P2OSNode()
00131 {
00132 }
00133 
00134 void P2OSNode::cmdmotor_state( const p2os_msgs::MotorStateConstPtr &msg)
00135 {
00136     motor_dirty = true;
00137     cmdmotor_state_ = *msg;
00138 }
00139 
00140 void P2OSNode::check_and_set_motor_state()
00141 {
00142     if( !motor_dirty ) return;
00143     motor_dirty = false;
00144     
00145     unsigned char val = (unsigned char) cmdmotor_state_.state;
00146     unsigned char command[4];
00147     P2OSPacket packet;
00148     command[0] = ENABLE;
00149     command[1] = ARGINT;
00150     command[2] = val;
00151     command[3] = 0;
00152     packet.Build(command,4);
00153     
00154     // Store the current motor state so that we can set it back
00155     p2os_data.motors.state = cmdmotor_state_.state;
00156     SendReceive(&packet,false);
00157 }
00158 
00159 void P2OSNode::check_and_set_gripper_state()
00160 {
00161     if( !gripper_dirty_ ) return;
00162     gripper_dirty_ = false;
00163     
00164     // Send the gripper command
00165     unsigned char grip_val = (unsigned char) gripper_state_.grip.state;
00166     unsigned char grip_command[4];
00167     P2OSPacket grip_packet;
00168     grip_command[0] = GRIPPER;
00169     grip_command[1] = ARGINT;
00170     grip_command[2] = grip_val;
00171     grip_command[3] = 0;
00172     grip_packet.Build(grip_command,4);
00173     SendReceive(&grip_packet,false);
00174     
00175     // Send the lift command
00176     unsigned char lift_val = (unsigned char) gripper_state_.lift.state;
00177     unsigned char lift_command[4];
00178     P2OSPacket lift_packet;
00179     lift_command[0] = GRIPPER;
00180     lift_command[1] = ARGINT;
00181     lift_command[2] = lift_val;
00182     lift_command[3] = 0;
00183     lift_packet.Build(lift_command,4);
00184     SendReceive(&lift_packet,false);
00185 }
00186 
00187 void P2OSNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
00188 {
00189     
00190     if( fabs( msg->linear.x - cmdvel_.linear.x ) > 0.01 || fabs( msg->angular.z-cmdvel_.angular.z) > 0.01 )
00191     {
00192         veltime = ros::Time::now();
00193         ROS_DEBUG( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
00194         vel_dirty = true;
00195         cmdvel_ = *msg;
00196     }
00197     else
00198     {
00199         ros::Duration veldur = ros::Time::now() - veltime;
00200         if( veldur.toSec() > 2.0 && ((fabs(cmdvel_.linear.x) > 0.01) || (fabs(cmdvel_.angular.z) > 0.01)) )
00201         {
00202             ROS_DEBUG( "maintaining old speed: %0.3f|%0.3f", veltime.toSec(), ros::Time::now().toSec() );
00203             vel_dirty = true;
00204             veltime = ros::Time::now();
00205         }
00206     }
00207     
00208 }
00209 
00210 void P2OSNode::check_and_set_vel()
00211 {
00212     if( !vel_dirty ) return;
00213     
00214     ROS_DEBUG( "setting vel: [%0.2f,%0.2f]",cmdvel_.linear.x,cmdvel_.angular.z);
00215     vel_dirty = false;
00216     
00217     unsigned short absSpeedDemand, absturnRateDemand;
00218     unsigned char motorcommand[4];
00219     P2OSPacket motorpacket;
00220     
00221     int vx = (int) (cmdvel_.linear.x*1e3);
00222     int va = (int)rint(RTOD(cmdvel_.angular.z));
00223     
00224     {
00225         // non-direct wheel control
00226         motorcommand[0] = VEL;
00227         if( vx >= 0 ) motorcommand[1] = ARGINT;
00228         else motorcommand[1] = ARGNINT;
00229         
00230         absSpeedDemand = (unsigned short)abs(vx);
00231         if( absSpeedDemand <= this->motor_max_speed )
00232         {
00233             motorcommand[2] = absSpeedDemand & 0x00FF;
00234             motorcommand[3] = (absSpeedDemand & 0xFF00) >> 8;
00235         }
00236         else
00237         {
00238             ROS_WARN( "speed demand thresholded! (true: %u, max: %u)", absSpeedDemand, motor_max_speed );
00239             motorcommand[2] = motor_max_speed & 0x00FF;
00240             motorcommand[3] = (motor_max_speed & 0xFF00) >> 8;
00241         }
00242         motorpacket.Build(motorcommand, 4);
00243         SendReceive(&motorpacket);
00244         
00245         motorcommand[0] = RVEL;
00246         if( va >= 0 ) motorcommand[1] = ARGINT;
00247         else motorcommand[1] = ARGNINT;
00248         
00249         absturnRateDemand = (unsigned short)abs(va);
00250         if( absturnRateDemand <= motor_max_turnspeed )
00251         {
00252             motorcommand[2] = absturnRateDemand & 0x00FF;
00253             motorcommand[3] = (absturnRateDemand & 0xFF00) >> 8;
00254         }
00255         else
00256         {
00257             ROS_WARN("Turn rate demand threshholded!");
00258             motorcommand[2] = this->motor_max_turnspeed & 0x00FF;
00259             motorcommand[3] = (this->motor_max_turnspeed & 0xFF00) >> 8;
00260         }
00261         
00262         motorpacket.Build(motorcommand,4);
00263         SendReceive(&motorpacket);
00264     }
00265 }
00266 
00267 void P2OSNode::gripperCallback(const p2os_msgs::GripperStateConstPtr &msg)
00268 {
00269     gripper_dirty_ = true;
00270     gripper_state_ = *msg;
00271 }
00272 
00273 int 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.\n   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             param_idx = i;
00496             break;
00497         }
00498     }
00499     if(i == PLAYER_NUM_ROBOT_TYPES)
00500     {
00501         ROS_WARN("P2OS: Warning: couldn't find parameters for this robot; "
00502                  "using defaults");
00503         param_idx = 0;
00504     }
00505     
00506     //sleep(1);
00507     
00508     // first, receive a packet so we know we're connected.
00509     if(!sippacket)
00510     {
00511         sippacket = new SIP(param_idx);
00512         sippacket->odom_frame_id = odom_frame_id;
00513         sippacket->base_link_frame_id = base_link_frame_id;
00514     }
00515     /*
00516   sippacket->x_offset = 0;
00517   sippacket->y_offset = 0;
00518   sippacket->angle_offset = 0;
00519   
00520   SendReceive((P2OSPacket*)NULL,false);
00521 */
00522     // turn off the sonars at first
00523     this->ToggleSonarPower(0);
00524     // if requested, set max accel/decel limits
00525     P2OSPacket accel_packet;
00526     unsigned char accel_command[4];
00527     if(this->motor_max_trans_accel > 0)
00528     {
00529         accel_command[0] = SETA;
00530         accel_command[1] = ARGINT;
00531         accel_command[2] = this->motor_max_trans_accel & 0x00FF;
00532         accel_command[3] = (this->motor_max_trans_accel & 0xFF00) >> 8;
00533         accel_packet.Build(accel_command, 4);
00534         this->SendReceive(&accel_packet,false);
00535     }
00536     
00537     if(this->motor_max_trans_decel < 0)
00538     {
00539         accel_command[0] = SETA;
00540         accel_command[1] = ARGNINT;
00541         accel_command[2] = abs(this->motor_max_trans_decel) & 0x00FF;
00542         accel_command[3] = (abs(this->motor_max_trans_decel) & 0xFF00) >> 8;
00543         accel_packet.Build(accel_command, 4);
00544         this->SendReceive(&accel_packet,false);
00545     }
00546     if(this->motor_max_rot_accel > 0)
00547     {
00548         accel_command[0] = SETRA;
00549         accel_command[1] = ARGINT;
00550         accel_command[2] = this->motor_max_rot_accel & 0x00FF;
00551         accel_command[3] = (this->motor_max_rot_accel & 0xFF00) >> 8;
00552         accel_packet.Build(accel_command, 4);
00553         this->SendReceive(&accel_packet,false);
00554     }
00555     if(this->motor_max_rot_decel < 0)
00556     {
00557         accel_command[0] = SETRA;
00558         accel_command[1] = ARGNINT;
00559         accel_command[2] = abs(this->motor_max_rot_decel) & 0x00FF;
00560         accel_command[3] = (abs(this->motor_max_rot_decel) & 0xFF00) >> 8;
00561         accel_packet.Build(accel_command, 4);
00562         this->SendReceive(&accel_packet,false);
00563     }
00564     
00565     
00566     // if requested, change PID settings
00567     P2OSPacket pid_packet;
00568     unsigned char pid_command[4];
00569     if(this->rot_kp >= 0)
00570     {
00571         pid_command[0] = ROTKP;
00572         pid_command[1] = ARGINT;
00573         pid_command[2] = this->rot_kp & 0x00FF;
00574         pid_command[3] = (this->rot_kp & 0xFF00) >> 8;
00575         pid_packet.Build(pid_command, 4);
00576         this->SendReceive(&pid_packet);
00577     }
00578     if(this->rot_kv >= 0)
00579     {
00580         pid_command[0] = ROTKV;
00581         pid_command[1] = ARGINT;
00582         pid_command[2] = this->rot_kv & 0x00FF;
00583         pid_command[3] = (this->rot_kv & 0xFF00) >> 8;
00584         pid_packet.Build(pid_command, 4);
00585         this->SendReceive(&pid_packet);
00586     }
00587     if(this->rot_ki >= 0)
00588     {
00589         pid_command[0] = ROTKI;
00590         pid_command[1] = ARGINT;
00591         pid_command[2] = this->rot_ki & 0x00FF;
00592         pid_command[3] = (this->rot_ki & 0xFF00) >> 8;
00593         pid_packet.Build(pid_command, 4);
00594         this->SendReceive(&pid_packet);
00595     }
00596     if(this->trans_kp >= 0)
00597     {
00598         pid_command[0] = TRANSKP;
00599         pid_command[1] = ARGINT;
00600         pid_command[2] = this->trans_kp & 0x00FF;
00601         pid_command[3] = (this->trans_kp & 0xFF00) >> 8;
00602         pid_packet.Build(pid_command, 4);
00603         this->SendReceive(&pid_packet);
00604     }
00605     if(this->trans_kv >= 0)
00606     {
00607         pid_command[0] = TRANSKV;
00608         pid_command[1] = ARGINT;
00609         pid_command[2] = this->trans_kv & 0x00FF;
00610         pid_command[3] = (this->trans_kv & 0xFF00) >> 8;
00611         pid_packet.Build(pid_command, 4);
00612         this->SendReceive(&pid_packet);
00613     }
00614     if(this->trans_ki >= 0)
00615     {
00616         pid_command[0] = TRANSKI;
00617         pid_command[1] = ARGINT;
00618         pid_command[2] = this->trans_ki & 0x00FF;
00619         pid_command[3] = (this->trans_ki & 0xFF00) >> 8;
00620         pid_packet.Build(pid_command, 4);
00621         this->SendReceive(&pid_packet);
00622     }
00623     
00624     
00625     // if requested, change bumper-stall behavior
00626     // 0 = don't stall
00627     // 1 = stall on front bumper contact
00628     // 2 = stall on rear bumper contact
00629     // 3 = stall on either bumper contact
00630     if(this->bumpstall >= 0)
00631     {
00632         if(this->bumpstall > 3)
00633             ROS_INFO("ignoring bumpstall value %d; should be 0, 1, 2, or 3",
00634                      this->bumpstall);
00635         else
00636         {
00637             ROS_INFO("setting bumpstall to %d", this->bumpstall);
00638             P2OSPacket bumpstall_packet;;
00639             unsigned char bumpstall_command[4];
00640             bumpstall_command[0] = BUMP_STALL;
00641             bumpstall_command[1] = ARGINT;
00642             bumpstall_command[2] = (unsigned char)this->bumpstall;
00643             bumpstall_command[3] = 0;
00644             bumpstall_packet.Build(bumpstall_command, 4);
00645             this->SendReceive(&bumpstall_packet,false);
00646         }
00647     }
00648     
00649     // Turn on the sonar
00650     if(use_sonar_) {
00651         this->ToggleSonarPower(1);
00652         ROS_DEBUG("Sonar array powered on.");
00653     }
00654     ptz_.setup();
00655     
00656     return(0);
00657 }
00658 
00659 int 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 P2OSNode::SendReceive(P2OSPacket* pkt, bool publish_data)
00727 {
00728     P2OSPacket packet;
00729     
00730     if((this->psos_fd >= 0) && this->sippacket)
00731     {
00732         if(pkt)
00733             pkt->Send(this->psos_fd);
00734         
00735         /* receive a packet */
00736         pthread_testcancel();
00737         if(packet.Receive(this->psos_fd))
00738         {
00739             ROS_ERROR("RunPsosThread(): Receive errored");
00740             pthread_exit(NULL);
00741         }
00742         
00743         if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
00744                 (packet.packet[3] == 0x30 || packet.packet[3] == 0x31 ||
00745                  packet.packet[3] == 0x32 || packet.packet[3] == 0x33 ||
00746                  packet.packet[3] == 0x34))
00747         {
00748             
00749             /* It is a server packet, so process it */
00750             this->sippacket->ParseStandard( &packet.packet[3] );
00751             this->sippacket->FillStandard(&(this->p2os_data));
00752             
00753             if(publish_data)
00754                 this->StandardSIPPutData(packet.timestamp);
00755         }
00756         else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
00757                 packet.packet[3] == SERAUX)
00758         {
00759             // This is an AUX serial packet
00760             if(ptz_.isOn())
00761             {
00762                 int len = packet.packet[2] - 3;
00763                 if (ptz_.cb_.gotPacket())
00764                 {
00765                     ROS_ERROR("PTZ got a message, but alread has the complete packet.");
00766                 }
00767                 else
00768                 {
00769                     for (int i=4; i < 4+len; ++i)
00770                     {
00771                         ptz_.cb_.putOnBuf(packet.packet[i]);
00772                     }
00773                 }
00774             }
00775         }
00776         else
00777         {
00778             ROS_ERROR("Received other packet!");
00779             packet.PrintHex();
00780         }
00781     }
00782     
00783     return(0);
00784 }
00785 
00786 void P2OSNode::updateDiagnostics()
00787 {
00788     diagnostic_.update();
00789 }
00790 
00791 void P2OSNode::check_voltage( diagnostic_updater::DiagnosticStatusWrapper &stat )
00792 {
00793     double voltage = sippacket->battery / 10.0;
00794     if( voltage < 11.0 )
00795     {
00796         stat.summary( diagnostic_msgs::DiagnosticStatus::ERROR, "battery voltage critically low" );
00797     }
00798     else if( voltage < 11.75 )
00799     {
00800         stat.summary( diagnostic_msgs::DiagnosticStatus::WARN, "battery voltage getting low" );
00801         
00802     }
00803     else stat.summary( diagnostic_msgs::DiagnosticStatus::OK, "battery voltage OK" );
00804     
00805     stat.add("voltage", voltage );
00806 }
00807 
00808 void P2OSNode::check_stall( diagnostic_updater::DiagnosticStatusWrapper &stat )
00809 {
00810     if( sippacket->lwstall || sippacket->rwstall )
00811     {
00812         stat.summary( diagnostic_msgs::DiagnosticStatus::ERROR, "wheel stalled" );
00813     }
00814     else stat.summary( diagnostic_msgs::DiagnosticStatus::OK, "no wheel stall" );
00815     
00816     stat.add("left wheel stall", sippacket->lwstall );
00817     stat.add("right wheel stall", sippacket->rwstall );
00818 }
00819 
00820 void P2OSNode::ResetRawPositions()
00821 {
00822     P2OSPacket pkt;
00823     unsigned char p2oscommand[4];
00824     
00825     if(this->sippacket)
00826     {
00827         this->sippacket->rawxpos = 0;
00828         this->sippacket->rawypos = 0;
00829         this->sippacket->xpos = 0;
00830         this->sippacket->ypos = 0;
00831         p2oscommand[0] = SETO;
00832         p2oscommand[1] = ARGINT;
00833         pkt.Build(p2oscommand, 2);
00834         this->SendReceive(&pkt,false);
00835         ROS_INFO("resetting raw positions" );
00836     }
00837 }
00838 
00839 /* toggle sonars on/off, according to val */
00840 void P2OSNode::ToggleSonarPower(unsigned char val)
00841 {
00842     unsigned char command[4];
00843     P2OSPacket packet;
00844     
00845     command[0] = SONAR;
00846     command[1] = ARGINT;
00847     command[2] = val;
00848     command[3] = 0;
00849     packet.Build(command, 4);
00850     SendReceive(&packet,false);
00851 }
00852 
00858 void P2OSNode::ToggleMotorPower(unsigned char val)
00859 {
00860     unsigned char command[4];
00861     P2OSPacket packet;
00862     ROS_INFO( "motor state: %d\n", p2os_data.motors.state );
00863     p2os_data.motors.state = (int) val;
00864     command[0] = ENABLE;
00865     command[1] = ARGINT;
00866     command[2] = val;
00867     command[3] = 0;
00868     packet.Build(command, 4);
00869     SendReceive(&packet,false);
00870 }
00871 
00873 //  Actarray stuff
00875 
00876 // Ticks to degrees from the ARIA software
00878 inline double P2OSNode::TicksToDegrees (int joint, unsigned char ticks)
00879 {
00880     if ((joint < 0) || (joint >= sippacket->armNumJoints))
00881         return 0;
00882     
00883     double result;
00884     int pos = ticks - sippacket->armJoints[joint].centre;
00885     result = 90.0 / static_cast<double> (sippacket->armJoints[joint].ticksPer90);
00886     result = result * pos;
00887     if ((joint >= 0) && (joint <= 2))
00888         result = -result;
00889     
00890     return result;
00891 }
00892 
00893 // Degrees to ticks from the ARIA software
00895 inline unsigned char P2OSNode::DegreesToTicks (int joint, double degrees)
00896 {
00897     double val;
00898     
00899     if ((joint < 0) || (joint >= sippacket->armNumJoints))
00900         return 0;
00901     
00902     val = static_cast<double> (sippacket->armJoints[joint].ticksPer90) * degrees / 90.0;
00903     val = round (val);
00904     if ((joint >= 0) && (joint <= 2))
00905         val = -val;
00906     val += sippacket->armJoints[joint].centre;
00907     
00908     if (val < sippacket->armJoints[joint].min)
00909         return sippacket->armJoints[joint].min;
00910     else if (val > sippacket->armJoints[joint].max)
00911         return sippacket->armJoints[joint].max;
00912     else
00913         return static_cast<int> (round (val));
00914 }
00915 
00917 inline double P2OSNode::TicksToRadians (int joint, unsigned char ticks)
00918 {
00919     double result = DTOR (TicksToDegrees (joint, ticks));
00920     return result;
00921 }
00922 
00924 inline unsigned char P2OSNode::RadiansToTicks (int joint, double rads)
00925 {
00926     unsigned char result = static_cast<unsigned char> (DegreesToTicks (joint, RTOD (rads)));
00927     return result;
00928 }
00929 
00931 inline double P2OSNode::RadsPerSectoSecsPerTick (int joint, double speed)
00932 {
00933     double degs = RTOD (speed);
00934     double ticksPerDeg = static_cast<double> (sippacket->armJoints[joint].ticksPer90) / 90.0f;
00935     double ticksPerSec = degs * ticksPerDeg;
00936     double secsPerTick = 1000.0f / ticksPerSec;
00937     
00938     if (secsPerTick > 127)
00939         return 127;
00940     else if (secsPerTick < 1)
00941         return 1;
00942     return secsPerTick;
00943 }
00944 
00946 inline double P2OSNode::SecsPerTicktoRadsPerSec (int joint, double msecs)
00947 {
00948     double ticksPerSec = 1.0 / (static_cast<double> (msecs) / 1000.0);
00949     double ticksPerDeg = static_cast<double> (sippacket->armJoints[joint].ticksPer90) / 90.0f;
00950     double degs = ticksPerSec / ticksPerDeg;
00951     double rads = DTOR (degs);
00952     
00953     return rads;
00954 }
00955 
00956 void P2OSNode::SendPulse (void)
00957 {
00958     unsigned char command;
00959     P2OSPacket packet;
00960     
00961     command = PULSE;
00962     packet.Build(&command, 1);
00963     SendReceive(&packet);
00964 }


p2os_driver
Author(s): Hunter Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Wed Aug 26 2015 15:15:07