p2osnode.cc
Go to the documentation of this file.
1 /*
2  * P2OS for ROS
3  * Copyright (C) 2009
4  * David Feil-Seifer, Brian Gerkey, Kasper Stoy,
5  * Richard Vaughan, & Andrew Howard
6  *
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software
20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21  *
22  */
23 
24 #include <iostream>
25 
26 #include <ros/ros.h>
27 #include <geometry_msgs/Pose.h>
28 #include <geometry_msgs/PoseStamped.h>
29 #include <p2os_driver/p2os.h>
30 #include <p2os_msgs/MotorState.h>
31 #include <tf/transform_datatypes.h>
32 
33 int main( int argc, char** argv )
34 {
35  ros::init(argc,argv, "p2os");
36  ros::NodeHandle n;
37 
38  P2OSNode *p = new P2OSNode(n);
39 
40  if( p->Setup() != 0 )
41  {
42  ROS_ERROR( "p2os setup failed..." );
43  return -1;
44  }
45 
46  p->ResetRawPositions();
47 
48  ros::Time lastTime;
49 
50  while( ros::ok() )
51  {
52  p->check_and_set_vel();
55 
56  if( p->get_pulse() > 0 )
57  {
58  ros::Time currentTime = ros::Time::now();
59  ros::Duration pulseInterval = currentTime - lastTime;
60  if( pulseInterval.toSec() > p->get_pulse() )
61  {
62  ROS_DEBUG ("sending pulse" );
63  p->SendPulse();
64  lastTime = currentTime;
65  }
66  }
67 
68  // Hack fix to get around the fact that if no commands are sent to the
69  // robot via SendReceive, the driver will never read SIP packets and so
70  // never send data back to clients. We need a better way of doing regular
71  // checks of the serial port - peek in sendreceive, maybe? Because if there
72  // is no data waiting this will sit around waiting until one comes
73  p->SendReceive(NULL,true);
74  p->updateDiagnostics();
75  ros::spinOnce();
76  }
77 
78  if( p->Shutdown() != 0 )
79  {
80  ROS_WARN( "p2os shutdown failed... your robot might be heading for the wall?" );
81  }
82  delete p; //delete pointer
83 
84  ROS_INFO( "Quitting... " );
85  return 0;
86 }
int main(int argc, char **argv)
Definition: p2osnode.cc:33
int SendReceive(P2OSPacket *pkt, bool publish_data=true)
Definition: p2os.cc:724
double get_pulse()
Definition: p2os.h:129
void updateDiagnostics()
Definition: p2os.cc:784
void check_and_set_gripper_state()
Definition: p2os.cc:159
void SendPulse(void)
Definition: p2os.cc:954
int Setup()
Setup the robot for use. Communicates with the robot directly.
Definition: p2os.cc:273
void ResetRawPositions()
Definition: p2os.cc:818
int Shutdown()
Prepare for shutdown.
Definition: p2os.cc:657
void check_and_set_vel()
Definition: p2os.cc:210
Definition: p2os.h:86
void check_and_set_motor_state()
Definition: p2os.cc:140


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 Jun 25 2014 09:37:15