challenge/msg_img_tf.py at master · chillvamp/challenge · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
# -*- coding: utf-8 -*-
"""
Created on Sun Jan 19 15:17:33 2020

@author: oscar
"""

#!/usr/bin/env python

import cv2
from geometry_msgs.msg import Quaternion
import numpy as np
import rospy
import message_filters
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import Image
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseStamped
from tf.transformations import *
from tf.msg import tfMessage
from tf.transformations import euler_from_quaternion
import tf
from cv_bridge import CvBridge, CvBridgeError
bridge = CvBridge()
global tf_listener
global tf_broadcaster


from std_msgs.msg import String
def shutdown(self):

        exit()

def callback(img_msg):

    tf_broadcaster=tf.TransformBroadcaster()
    (trans,rot)=tf_listener.lookupTransform('hand_palm_link', 'map', rospy.Time(0))

    goal_hole=np.array((270,243))


################################################################ DO THINGS HERE
    cv2_img = bridge.imgmsg_to_cv2(img_msg, "bgr8")
    gray = cv2.cvtColor(cv2_img,cv2.COLOR_BGR2GRAY)
    """orb = cv2.ORB_create()
    kp = orb.detect(cv2_img,None)

    kp, des = orb.compute(cv2_img, kp)
    img2 = cv2.drawKeypoints(cv2_img, kp, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)


   # np.save('imagen.npy',cv2_img)

    """
    circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1.2, 100)
    if circles is not None:
        print (circles)
        circles = np.round(circles[0, :]).astype("int")
        for (x, y, r) in circles:
		# draw the circle in the output image, then draw a rectangle
		# corresponding to the center of the circle
		cv2.circle(cv2_img, (x, y), r, (0, 255, 0), 4)

        if (r>100):
            px_m=0
        else:
            px_m=.06/r
            tf_broadcaster.sendTransform(
        (   (x-goal_hole[0])*.5*px_m,(y-goal_hole[1])*.5*px_m, 0.15),tf.transformations.quaternion_from_euler(3.1416, 0, 1.57),
        rospy.Time.now(),"chido","hand_palm_link"
        )


        #tf_broadcaster.sendTransform((x-goal_hole[0])*px_m, (y-goal_hole[1])*px_m, 0),


#################

    cv2.namedWindow("hand_cam")

    cv2.imshow("hand_cam", cv2_img)
    k = cv2.waitKey(1)
    if k & 0xFF == ord('q'):
        # q key pressed so quit
        print("Quitting...")
        kill_node=True
        cv2.destroyAllWindows()
        exit()

    elif k & 0xFF == ord('c'):
        # c key pressed so capture frame to image file
        cap_cnt=0
        cap_name = "capture_{}.png".format(cap_cnt)
        cv2.imwrite(cap_name, cv2_img)
        print("Saving {}!".format(cap_name))


def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    global tf_listener
    rospy.init_node('hand_camera_listener', anonymous=True)

    rospy.on_shutdown(shutdown)
    tf_listener = tf.TransformListener()


    rospy.Subscriber("/hsrb/hand_camera/image_raw", Image, callback)
    global kill_node
    kill_node=False
    # spin() simply keeps python from exiting until this node is stopped
    if not kill_node:
        rospy.spin()


if __name__ == '__main__':
    listener()