Libbarrett  1.2.4
examples/ex10_haptics.h
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_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Defines