00001 /* 00002 * P2OS for ROS 00003 * Copyright (C) 2009 David Feil-Seifer, Brian Gerkey, Kasper Stoy, 00004 * Richard Vaughan, & Andrew Howard 00005 * Copyright (C) 2018 Hunter L. Allen 00006 * 00007 * This program is free software; you can redistribute it and/or modify 00008 * it under the terms of the GNU General Public License as published by 00009 * the Free Software Foundation; either version 2 of the License, or 00010 * (at your option) any later version. 00011 * 00012 * This program is distributed in the hope that it will be useful, 00013 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00014 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00015 * GNU General Public License for more details. 00016 * 00017 * You should have received a copy of the GNU General Public License 00018 * along with this program; if not, write to the Free Software 00019 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00020 */ 00021 00022 #include <ros/ros.h> 00023 #include <geometry_msgs/Pose.h> 00024 #include <geometry_msgs/PoseStamped.h> 00025 #include <p2os_driver/p2os.hpp> 00026 #include <p2os_msgs/MotorState.h> 00027 #include <tf/transform_datatypes.h> 00028 00029 #include <iostream> 00030 00031 int main(int argc, char ** argv) 00032 { 00033 ros::init(argc, argv, "p2os"); 00034 ros::NodeHandle n; 00035 00036 P2OSNode * p = new P2OSNode(n); 00037 00038 if (p->Setup()) { 00039 ROS_ERROR("p2os setup failed..."); 00040 return -1; 00041 } 00042 00043 p->ResetRawPositions(); 00044 00045 ros::Time lastTime; 00046 00047 while (ros::ok()) { 00048 p->check_and_set_vel(); 00049 p->check_and_set_motor_state(); 00050 p->check_and_set_gripper_state(); 00051 00052 if (p->get_pulse() > 0) { 00053 ros::Time currentTime = ros::Time::now(); 00054 ros::Duration pulseInterval = currentTime - lastTime; 00055 if (pulseInterval.toSec() > p->get_pulse()) { 00056 ROS_DEBUG("sending pulse"); 00057 p->SendPulse(); 00058 lastTime = currentTime; 00059 } 00060 } 00061 00062 // Hack fix to get around the fact that if no commands are sent to the 00063 // robot via SendReceive, the driver will never read SIP packets and so 00064 // never send data back to clients. We need a better way of doing regular 00065 // checks of the serial port - peek in sendreceive, maybe? Because if there 00066 // is no data waiting this will sit around waiting until one comes 00067 p->SendReceive(NULL, true); 00068 p->updateDiagnostics(); 00069 ros::spinOnce(); 00070 } 00071 00072 if (!p->Shutdown()) { 00073 ROS_WARN("p2os shutdown failed... your robot might be heading for the wall?"); 00074 } 00075 delete p; 00076 00077 ROS_INFO("Quitting... "); 00078 return 0; 00079 }