rename chatter_pub to pub_inputs

[ros_roboint.git] / src / libft_adapter.cpp

diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp
index bacd2d7f79f033853b8f7e7faf38594c6e901426..925dfd54ea37dd444717e126563e283ac6420850 100644 (file)
--- a/src/libft_adapter.cpp
+++ b/src/libft_adapter.cpp
@@ -14,7 +14,7 @@ static pthread_mutex_t pwm_mutex = PTHREAD_MUTEX_INITIALIZER;
 
 void cb_set_output(const ::roboint::OutputConstPtr& msg) {
        pthread_mutex_lock(&pwm_mutex);
-       pwm_next[msg->num] = msg->speed;
+       pwm_next[msg->num] = abs(msg->speed);
        pthread_mutex_unlock(&pwm_mutex);
 }
 
@@ -22,11 +22,11 @@ void cb_set_output(const ::roboint::OutputConstPtr& msg) {
 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
        pthread_mutex_lock(&pwm_mutex);
        if (msg->speed > 0) {
-               pwm_next[msg->num*2] = msg->speed;
+               pwm_next[msg->num*2] = abs(msg->speed);
                pwm_next[msg->num*2+1] = 0;
        } else {
                pwm_next[msg->num*2] = 0;
-               pwm_next[msg->num*2+1] = msg->speed;
+               pwm_next[msg->num*2+1] = abs(msg->speed);
        }
        pthread_mutex_unlock(&pwm_mutex);
 }
@@ -93,19 +93,20 @@ int main(int argc, char **argv)
         * than we can send them, the number here specifies how many messages to
         * buffer up before throwing some away.
         */
-       ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
+       ros::Publisher pub_inputs = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
        ros::Rate loop_rate(100);
 
        while(ros::ok()) {
                roboint::Inputs msg;
 
                pthread_mutex_lock(&pwm_mutex);
+               msg.header.stamp = ros::Time::now();
                sem_wait(&hFt->lock);
                for (int i=0; i<=7; i++) {
                        msg.input[i] = (transfer_area->E_Main & (1<<i)) >> i;
                }
                for (int i=0; i<=7; i++) {
-                       msg.output[i] = abs(pwm[i]);
+                       msg.output[i] = pwm[i];
                        pwm[i] = pwm_next[i];
 
                        if (pwm[i] == 0) {
@@ -113,7 +114,7 @@ int main(int argc, char **argv)
                        } else {
                                transfer_area->M_Main |= (1<<(i));
                        }
-                       transfer_area->MPWM_Main[i] = abs(pwm[i]);
+                       transfer_area->MPWM_Main[i] = pwm[i];
                }
                msg.ax = transfer_area->AX;
                msg.ay = transfer_area->AY;
@@ -131,7 +132,7 @@ int main(int argc, char **argv)
                 * given as a template parameter to the advertise<>() call, as was done
                 * in the constructor above.
                 */
-               chatter_pub.publish(msg);
+               pub_inputs.publish(msg);
 
                /**
                 * ros::spin() will enter a loop, pumping callbacks.  With this version, all