-Added missing A1,A2,AX,AY to Inputs.msg

-Start counting Inputs from 0
-use the semlock in libft_adapter

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 16e33b2d049a13fdbdd88d55da7b1a10d66aa758..885c2136b25fcb10af4b80e5be421b99335083a4 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,7 +49,7 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-   INCLUDE_DIRS include
+#  INCLUDE_DIRS include
 #  LIBRARIES roboint
    CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
 #  DEPENDS system_lib

diff --git a/msg/Inputs.msg b/msg/Inputs.msg
index 86240c3f1b58f5d2c27b727a89f48f049018b8ab..fb541705c54b2500d1e3b9b8ffae4c22a04ae423 100644 (file)
--- a/msg/Inputs.msg
+++ b/msg/Inputs.msg
@@ -1,2 +1,8 @@
-bool[9] input
+bool[8] input
+int16 ax
+int16 ay
+int16 a1
+int16 a2
+int16 av
 int16 d1
+int16 d2

diff --git a/package.xml b/package.xml
index 7aac638f1f9b323147c505eeea9374cb264fe98f..acdc3f69f5fb569d9b46a493650d393160bfe770 100644 (file)
--- a/package.xml
+++ b/package.xml
@@ -50,6 +50,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>tf</build_depend>
+  <run_depend>tf</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py
index cfc23fa70aa2868697be8f1b2847a33506f2d093..73e2ae8bdc09f146af6187477525cb02b984ada7 100755 (executable)
--- a/scripts/robo_explorer.py
+++ b/scripts/robo_explorer.py
@@ -2,6 +2,8 @@
 import roslib; roslib.load_manifest('roboint')
 import rospy
 import tf
+import tf.broadcaster
+import tf.transformations
 from math import sin, cos, pi
 from geometry_msgs.msg import Twist, TransformStamped, Point32
 from sensor_msgs.msg import LaserScan
@@ -21,7 +23,7 @@ class RoboExplorer:
                self.y = 0
                self.alpha = 0
                self.last_in = None
-               self.tf_broadcaster = tf.TransformBroadcaster()
+               self.tf_broadcaster = tf.broadcaster.TransformBroadcaster()
                self.last_time = rospy.Time.now()
                self.x_last = 0
                self.y_last = 0
@@ -46,7 +48,7 @@ class RoboExplorer:
                        self.last_time = current_time
 
        def update_odometry(self, msg, current_time):
-               in_now = msg.input[1:3]
+               in_now = msg.input[:2]
                if self.last_in is not None:
                        in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs
                        # fix in_diff from actual motor direction

diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp
index 27936b7f4fcb8aa90760dc87d6529b29767e7fa5..588f11e8a338046b1fe2cb1c449925f91d4f2296 100644 (file)
--- a/src/libft_adapter.cpp
+++ b/src/libft_adapter.cpp
@@ -100,11 +100,18 @@ int main(int argc, char **argv)
        while(ros::ok()) {
                roboint::Inputs msg;
 
-               msg.input[0] = 0; // unused, Hardware starts at 1
-               for (int i=1; i<=8; i++) {
-                       msg.input[i] = (transfer_area->E_Main & (1<<(i-1))) >> (i-1);
+               sem_wait(&hFt->lock);
+               for (int i=0; i<=7; i++) {
+                       msg.input[i] = (transfer_area->E_Main & (1<<i)) >> i;
                }
+               msg.ax = transfer_area->AX;
+               msg.ay = transfer_area->AY;
+               msg.a1 = transfer_area->A1;
+               msg.a2 = transfer_area->A2;
+               msg.av = transfer_area->AV;
                msg.d1 = transfer_area->D1;
+               msg.d2 = transfer_area->D2;
+               sem_post(&hFt->lock);
 
                /**
                 * The publish() function is how you send messages. The parameter