From: Erik Andresen <erik@vontaene.de>
Date: Wed, 4 Sep 2013 09:42:01 +0000 (+0200)
Subject: -Added missing A1,A2,AX,AY to Inputs.msg
X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_roboint.git;a=commitdiff_plain;h=47e8bfcf10b5ddbff2c3b6f66d7c59761e7ba53a

-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 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