Libbarrett
1.2.4
|
00001 /* 00002 Copyright 2010, 2011, 2012 Barrett Technology <support@barrett.com> 00003 00004 This file is part of libbarrett. 00005 00006 This version of libbarrett is free software: you can redistribute it 00007 and/or modify it under the terms of the GNU General Public License as 00008 published by the Free Software Foundation, either version 3 of the 00009 License, or (at your option) any later version. 00010 00011 This version of libbarrett is distributed in the hope that it will be 00012 useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 GNU General Public License for more details. 00015 00016 You should have received a copy of the GNU General Public License along 00017 with this version of libbarrett. If not, see 00018 <http://www.gnu.org/licenses/>. 00019 00020 Further, non-binding information about licensing is available at: 00021 <http://wiki.barrett.com/libbarrett/wiki/LicenseNotes> 00022 */ 00023 00024 /* 00025 * ex10_haptics.h 00026 * 00027 * Created on: Apr 14, 2010 00028 * Author: cd 00029 * Author: dc 00030 */ 00031 00032 #ifndef NETWORK_HAPTICS_H_ 00033 #define NETWORK_HAPTICS_H_ 00034 00035 00036 #include <stdexcept> 00037 00038 #include <unistd.h> /* for close() */ 00039 #include <sys/socket.h> /* For sockets */ 00040 #include <fcntl.h> /* To change socket to nonblocking mode */ 00041 #include <arpa/inet.h> /* For inet_pton() */ 00042 00043 #include <barrett/os.h> 00044 #include <barrett/detail/ca_macro.h> 00045 #include <barrett/units.h> 00046 #include <barrett/systems/abstract/single_io.h> 00047 #include <barrett/thread/disable_secondary_mode_warning.h> 00048 00049 class NetworkHaptics : public barrett::systems::SingleIO<barrett::units::CartesianPosition::type, barrett::units::CartesianForce::type> { 00050 BARRETT_UNITS_FIXED_SIZE_TYPEDEFS; 00051 00052 public: 00053 static const int SIZE_OF_MSG = 3 * sizeof(double); 00054 00055 explicit NetworkHaptics(barrett::systems::ExecutionManager* em, char* remoteHost, int port_src = 5557, int port_dest = 5556, const std::string& sysName = "NetworkHaptics") : 00056 barrett::systems::SingleIO<cp_type, cf_type>(sysName), sock(-1), cp(0.0), numMissed(0), cf(0.0) 00057 { 00058 int err; 00059 long flags; 00060 int buflen; 00061 unsigned int buflenlen; 00062 struct sockaddr_in bind_addr; 00063 struct sockaddr_in their_addr; 00064 00065 /* Create socket */ 00066 sock = socket(PF_INET, SOCK_DGRAM, 0); 00067 if (sock == -1) 00068 { 00069 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Could not create socket.") % __func__ ).raise<std::runtime_error>(); 00070 } 00071 00072 /* Set socket to non-blocking, set flag associated with open file */ 00073 flags = fcntl(sock, F_GETFL, 0); 00074 if (flags < 0) 00075 { 00076 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Could not get socket flags.") % __func__).raise<std::runtime_error>(); 00077 } 00078 flags |= O_NONBLOCK; 00079 err = fcntl(sock, F_SETFL, flags); 00080 if (err < 0) 00081 { 00082 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Could not set socket flags.") % __func__ ).raise<std::runtime_error>(); 00083 } 00084 00085 /* Maybe set UDP buffer size? */ 00086 buflenlen = sizeof(buflen); 00087 err = getsockopt(sock, SOL_SOCKET, SO_SNDBUF, (char *)&buflen, &buflenlen); 00088 if (err) 00089 { 00090 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Could not get output buffer size.") % __func__ ).raise<std::runtime_error>(); 00091 } 00092 barrett::logMessage("%s: Note, output buffer is %d bytes.") % __func__ % buflen; 00093 00094 buflenlen = sizeof(buflen); 00095 buflen = 5 * SIZE_OF_MSG; 00096 err = setsockopt(sock, SOL_SOCKET, SO_SNDBUF, (char *)&buflen, buflenlen); 00097 if (err) 00098 { 00099 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Could not set output buffer size.") % __func__ ).raise<std::runtime_error>(); 00100 } 00101 00102 buflenlen = sizeof(buflen); 00103 err = getsockopt(sock, SOL_SOCKET, SO_SNDBUF, (char *)&buflen, &buflenlen); 00104 if (err) 00105 { 00106 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Could not get output buffer size.") % __func__ ).raise<std::runtime_error>(); 00107 } 00108 barrett::logMessage("%s: Note, output buffer is %d bytes.") % __func__ % buflen; 00109 00110 /* Set up the bind address */ 00111 bind_addr.sin_family = AF_INET; 00112 bind_addr.sin_port = htons(port_src); 00113 bind_addr.sin_addr.s_addr = htonl(INADDR_ANY); 00114 err = bind(sock, (struct sockaddr *)&bind_addr, sizeof(bind_addr)); 00115 if (err == -1) 00116 { 00117 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Could not bind to socket on port %d") % __func__ % port_src).raise<std::runtime_error>(); 00118 } 00119 00120 /* Set up the other guy's address */ 00121 their_addr.sin_family = AF_INET; 00122 their_addr.sin_port = htons(port_dest); 00123 err = ! inet_pton(AF_INET, remoteHost, &their_addr.sin_addr); 00124 if (err) 00125 { 00126 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Bad IP argument '%s'.") %__func__ % remoteHost).raise<std::runtime_error>(); 00127 } 00128 00129 /* Call "connect" to set datagram destination */ 00130 err = connect(sock, (struct sockaddr *)&their_addr, sizeof(struct sockaddr)); 00131 if (err) 00132 { 00133 (barrett::logMessage("(NetworkHaptics::NetworkHaptics): Ctor failed %s: Could not set datagram destination.") % __func__ ).raise<std::runtime_error>(); 00134 } 00135 00136 00137 if (em != NULL) { 00138 em->startManaging(*this); 00139 } 00140 } 00141 00142 virtual ~NetworkHaptics() { 00143 mandatoryCleanUp(); 00144 close(sock); 00145 } 00146 00147 protected: 00148 virtual void operate() { 00149 cp = input.getValue(); 00150 00151 { 00152 // send() and recv() cause switches to secondary mode. The socket is 00153 // non-blocking, so this *probably* won't impact the control-loop 00154 // timing that much... 00155 barrett::thread::DisableSecondaryModeWarning dsmw; 00156 00157 send(sock, cp.data(), SIZE_OF_MSG, 0); 00158 00159 ++numMissed; 00160 while (recv(sock, cf.data(), SIZE_OF_MSG, 0) == SIZE_OF_MSG) { 00161 numMissed = 0; 00162 } 00163 if (numMissed > 10) { 00164 cf.setZero(); 00165 } 00166 } 00167 00168 outputValue->setData(&cf); 00169 } 00170 00171 int sock; 00172 cp_type cp; 00173 int numMissed; 00174 cf_type cf; 00175 00176 private: 00177 DISALLOW_COPY_AND_ASSIGN(NetworkHaptics); 00178 }; 00179 00180 #endif /* NETWORK_HAPTICS_H_ */