detectmarker

#!/usr/bin/env python

import sys
import math
import json

import rospy
import tf2_ros 
from tf.transformations import quaternion_from_euler
from geometry_msgs.msg import TransformStamped, Vector3
from aruco_msgs.mng import MarkerArray


def get_aruco_marker(aruco):
# Extract markers from/aruco/markers
    for m in aruco.markers:
        tf = transform_from_marker(m)
        if tf :
           broadcaster.sendTransform(tf)


def transform_from_marker(m):
    global tf_buf


    # Create marker
    marker = PoseStamped()
    marker.pose = m.pose.pose
    marker.header.frame_id = 'camera_link'
    marker.header.stamp = rospy. Time.now()
    
    # Check it transform if avaliable
    if not tf_buf.can transform(marker.header.frame_id, 'map', marker.header.stamp):
        rospy. logwarn_throttle(5.0, "No transform from %s to map" % marker.header.frame_1d)
        return

    # Transform from camera link to map
    poseInMap = tf_buf.transform (marker, 'map')

    # Create transformStamped to publish
    tfarucoInMap = Transformstamped ()
    tfArucoInMap.header.frame_id = 'map'
    tfArucoInMap.child_frame_id = "aruco/detected" + str(m.id)
    tfArucoInMap.transform.translation = poseInMap.pose.position
    tfArucoInMap. transform.rotation = poseInMap.pose.orientation

    return tfArucoInMap




if __name__ == "__main__":
    rospy.init_node ('detectaruco')
    aruco sub = rospy. Subscriber('/aruco/markers', MarkerArray,get aruco marker)
    broadcaster = tf2 ros.TransformBroadcaster()
    tf_buf = tf2_ros.Buffer()
    tf listner = tf2_ros.Transformlistener (tf_buf)
    rospy.spin()