src/libft_adapter.cpp

   1 #include <stdlib.h>
   2 #include <pthread.h>
   3 #include "roboint.h"
   4 #include "ros/ros.h"
   5 #include "roboint/Output.h"
   6 #include "roboint/Motor.h"
   7 #include "roboint/Inputs.h"
   8
   9
  10 static FT_TRANSFER_AREA *transfer_area = NULL;
  11 static char pwm[8] = {0};
  12 static char pwm_next[8] = {0};
  13 static pthread_mutex_t pwm_mutex = PTHREAD_MUTEX_INITIALIZER;
  14
  15 void cb_set_output(const ::roboint::OutputConstPtr& msg) {
  16         pthread_mutex_lock(&pwm_mutex);
  17         pwm_next[msg->num] = abs(msg->speed);
  18         pthread_mutex_unlock(&pwm_mutex);
  19 }
  20
  21
  22 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
  23         pthread_mutex_lock(&pwm_mutex);
  24         if (msg->speed > 0) {
  25                 pwm_next[msg->num*2] = abs(msg->speed);
  26                 pwm_next[msg->num*2+1] = 0;
  27         } else {
  28                 pwm_next[msg->num*2] = 0;
  29                 pwm_next[msg->num*2+1] = abs(msg->speed);
  30         }
  31         pthread_mutex_unlock(&pwm_mutex);
  32 }
  33
  34
  35 int main(int argc, char **argv)
  36 {
  37         FT_HANDLE hFt = NULL;
  38
  39         InitFtLib();
  40
  41         // Get List of USB devices
  42         InitFtUsbDeviceList();
  43
  44         // Get handle on a device
  45         if ((hFt = GetFtUsbDeviceHandle(0)) == NULL) {
  46                 fprintf(stderr, "No ft Device found\n");
  47                 return 1;
  48         }
  49
  50         // Open connection
  51         OpenFtUsbDevice(hFt);
  52         SetFtDistanceSensorMode(hFt, IF_DS_INPUT_DISTANCE, IF_DS_INPUT_TOL_STD, IF_DS_INPUT_TOL_STD, 100, 100, IF_DS_INPUT_REP_STD, IF_DS_INPUT_REP_STD);
  53
  54         StartFtTransferArea(hFt, NULL);
  55         transfer_area = GetFtTransferAreaAddress(hFt);
  56
  57         /**
  58          * The ros::init() function needs to see argc and argv so that it can perform
  59          * any ROS arguments and name remapping that were provided at the command line. For programmatic
  60          * remappings you can use a different version of init() which takes remappings
  61          * directly, but for most command-line programs, passing argc and argv is the easiest
  62          * way to do it.  The third argument to init() is the name of the node.
  63          *
  64          * You must call one of the versions of ros::init() before using any other
  65          * part of the ROS system.
  66          */
  67         ros::init(argc, argv, "libft_adapter");
  68
  69         /**
  70          * NodeHandle is the main access point to communications with the ROS system.
  71          * The first NodeHandle constructed will fully initialize this node, and the last
  72          * NodeHandle destructed will close down the node.
  73          */
  74         ros::NodeHandle n;
  75
  76         ros::Subscriber sub_set_output = n.subscribe("ft/set_output", 8, cb_set_output);
  77         ros::Subscriber sub_set_motor = n.subscribe("ft/set_motor", 4, cb_set_motor);
  78
  79         /**
  80          * The advertise() function is how you tell ROS that you want to
  81          * publish on a given topic name. This invokes a call to the ROS
  82          * master node, which keeps a registry of who is publishing and who
  83          * is subscribing. After this advertise() call is made, the master
  84          * node will notify anyone who is trying to subscribe to this topic name,
  85          * and they will in turn negotiate a peer-to-peer connection with this
  86          * node.  advertise() returns a Publisher object which allows you to
  87          * publish messages on that topic through a call to publish().  Once
  88          * all copies of the returned Publisher object are destroyed, the topic
  89          * will be automatically unadvertised.
  90          *
  91          * The second parameter to advertise() is the size of the message queue
  92          * used for publishing messages.  If messages are published more quickly
  93          * than we can send them, the number here specifies how many messages to
  94          * buffer up before throwing some away.
  95          */
  96         ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
  97         ros::Rate loop_rate(100);
  98
  99         while(ros::ok()) {
 100                 roboint::Inputs msg;
 101
 102                 pthread_mutex_lock(&pwm_mutex);
 103                 msg.header.stamp = ros::Time::now();
 104                 sem_wait(&hFt->lock);
 105                 for (int i=0; i<=7; i++) {
 106                         msg.input[i] = (transfer_area->E_Main & (1<<i)) >> i;
 107                 }
 108                 for (int i=0; i<=7; i++) {
 109                         msg.output[i] = pwm[i];
 110                         pwm[i] = pwm_next[i];
 111
 112                         if (pwm[i] == 0) {
 113                                 transfer_area->M_Main &= ~(1<<(i));
 114                         } else {
 115                                 transfer_area->M_Main |= (1<<(i));
 116                         }
 117                         transfer_area->MPWM_Main[i] = pwm[i];
 118                 }
 119                 msg.ax = transfer_area->AX;
 120                 msg.ay = transfer_area->AY;
 121                 msg.a1 = transfer_area->A1;
 122                 msg.a2 = transfer_area->A2;
 123                 msg.av = transfer_area->AV;
 124                 msg.d1 = transfer_area->D1;
 125                 msg.d2 = transfer_area->D2;
 126                 sem_post(&hFt->lock);
 127                 pthread_mutex_unlock(&pwm_mutex);
 128
 129                 /**
 130                  * The publish() function is how you send messages. The parameter
 131                  * is the message object. The type of this object must agree with the type
 132                  * given as a template parameter to the advertise<>() call, as was done
 133                  * in the constructor above.
 134                  */
 135                 chatter_pub.publish(msg);
 136
 137                 /**
 138                  * ros::spin() will enter a loop, pumping callbacks.  With this version, all
 139                  * callbacks will be called from within this thread (the main one).  ros::spin()
 140                  * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
 141                  */
 142                 ros::spinOnce();
 143
 144                 loop_rate.sleep();
 145         }
 146
 147         StopFtTransferArea(hFt);
 148
 149         // Close connection
 150         CloseFtDevice(hFt);
 151
 152         CloseFtLib();
 153
 154         return 0;
 155 }
 156