From 47e8bfcf10b5ddbff2c3b6f66d7c59761e7ba53a Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Wed, 4 Sep 2013 11:42:01 +0200
Subject: [PATCH] -Added missing A1,A2,AX,AY to Inputs.msg -Start counting
 Inputs from 0 -use the semlock in libft_adapter

---
 CMakeLists.txt           |  2 +-
 msg/Inputs.msg           |  8 +++++++-
 package.xml              |  1 +
 scripts/robo_explorer.py |  6 ++++--
 src/libft_adapter.cpp    | 13 ++++++++++---
 5 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 16e33b2..885c213 100644
--- 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 86240c3..fb54170 100644
--- 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 7aac638..acdc3f6 100644
--- 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 cfc23fa..73e2ae8 100755
--- 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 27936b7..588f11e 100644
--- 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
-- 
2.39.5