crated node to subscribe to the the avlar markers and the publish the…

… ar tag id number and the position on anther node

CMakeLists.txt

@@ -12,8 +12,25 @@ find_package(catkin REQUIRED COMPONENTS
   message_runtime
   rospy
   std_msgs
+  geometry_msgs
 )
 
+
+add_message_files(
+  FILES
+#  AlvarMarkers.msg
+  ar_tag.msg
+  )
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  ar_track_alvar_msgs
+
+)
+
+
+
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 

msg/ar_tag.msg

@@ -0,0 +1,3 @@
+uint32 id
+geometry_msgs/Pose pose
+

package.xml

@@ -52,6 +52,7 @@
   <build_depend>message_generation</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>ar_track_alvar_msgs</build_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>message_runtime</exec_depend>

src/artransforms.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import String
+from ar_track_alvar_msgs.msg import AlvarMarkers,AlvarMarker
+from ar_tag_demo.msg import ar_tag
+import rospy
+import sys
+import numpy as np
+
+from geometry_msgs.msg import Twist
+
+
+class ar_transform:
+
+    def __init__(self):
+        self.pub=rospy.Publisher("ar_pose_id", ar_tag)
+        self.sub=rospy.Subscriber("ar_pose_marker", AlvarMarkers,self.callback)
+
+
+    def callback(self,data):
+        tagnumber=len(data.markers)
+        for i in range(tagnumber):
+
+            ar=ar_tag()
+            ar.id=data.markers[i].id
+            ar.pose=data.markers[i].pose.pose
+            print ar
+
+            self.pub.publish(ar)
+
+
+
+def main(args):
+	rospy.init_node('ar_info', anonymous=True)
+	ic= ar_transform()
+	try:
+		rospy.spin()
+	except KeyboardInterrupt:
+		print("Shutting down")
+	cv2.destroyAllWindows()
+
+
+if __name__ == '__main__':
+    main(sys.argv)
+
+