p2osnode.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 
00024 #include <iostream>
00025 
00026 #include <ros/ros.h>
00027 #include <geometry_msgs/Pose.h>
00028 #include <geometry_msgs/PoseStamped.h>
00029 #include <p2os_driver/p2os.h>
00030 #include <p2os_msgs/MotorState.h>
00031 #include <tf/transform_datatypes.h>
00032 
00033 int main(int argc, char** argv)
00034 {
00035     ros::init(argc,argv, "p2os");
00036     ros::NodeHandle n;
00037     
00038     P2OSNode *p = new P2OSNode(n);
00039     
00040     if(p->Setup())
00041     {
00042         ROS_ERROR("p2os setup failed...");
00043         return -1;
00044     }
00045     
00046     p->ResetRawPositions();
00047     
00048     ros::Time lastTime;
00049     
00050     while(ros::ok())
00051     {
00052         p->check_and_set_vel();
00053         p->check_and_set_motor_state();
00054         p->check_and_set_gripper_state();
00055         
00056         if(p->get_pulse() > 0)
00057         {
00058             ros::Time currentTime = ros::Time::now();
00059             ros::Duration pulseInterval = currentTime - lastTime;
00060             if(pulseInterval.toSec() > p->get_pulse())
00061             { 
00062                 ROS_DEBUG ("sending pulse" );
00063                 p->SendPulse();
00064                 lastTime = currentTime;
00065             }
00066         }
00067         
00068         // Hack fix to get around the fact that if no commands are sent to the
00069         // robot via SendReceive, the driver will never read SIP packets and so
00070         // never send data back to clients. We need a better way of doing regular
00071         // checks of the serial port - peek in sendreceive, maybe? Because if there
00072         // is no data waiting this will sit around waiting until one comes
00073         p->SendReceive(NULL,true);
00074         p->updateDiagnostics();
00075         ros::spinOnce();
00076     }
00077     
00078     if(!p->Shutdown())
00079     {
00080         ROS_WARN("p2os shutdown failed... your robot might be heading for the wall?");
00081     }
00082     delete p; //delete pointer
00083     
00084     ROS_INFO( "Quitting... " );
00085     return 0;
00086 }


p2os_driver
Author(s): Hunter L. Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Sat Aug 5 2017 02:20:17