5 #include "roboint/Output.h"
6 #include "roboint/Motor.h"
7 #include "roboint/Inputs.h"
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;
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);
22 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
23 pthread_mutex_lock(&pwm_mutex);
25 pwm_next[msg->num*2] = abs(msg->speed);
26 pwm_next[msg->num*2+1] = 0;
28 pwm_next[msg->num*2] = 0;
29 pwm_next[msg->num*2+1] = abs(msg->speed);
31 pthread_mutex_unlock(&pwm_mutex);
35 int main(int argc, char **argv)
41 // Get List of USB devices
42 InitFtUsbDeviceList();
44 // Get handle on a device
45 if ((hFt = GetFtUsbDeviceHandle(0)) == NULL) {
46 fprintf(stderr, "No ft Device found\n");
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);
54 StartFtTransferArea(hFt, NULL);
55 transfer_area = GetFtTransferAreaAddress(hFt);
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.
64 * You must call one of the versions of ros::init() before using any other
65 * part of the ROS system.
67 ros::init(argc, argv, "libft_adapter");
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.
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);
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.
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.
96 ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
97 ros::Rate loop_rate(100);
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;
108 for (int i=0; i<=7; i++) {
109 msg.output[i] = pwm[i];
110 pwm[i] = pwm_next[i];
113 transfer_area->M_Main &= ~(1<<(i));
115 transfer_area->M_Main |= (1<<(i));
117 transfer_area->MPWM_Main[i] = pwm[i];
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);
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.
135 chatter_pub.publish(msg);
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.
147 StopFtTransferArea(hFt);