def vector2pixel(self, vec):
pixel = Pixel()
cw = self.ci_.width
ch = self.ci_.height
pixel.u = cw * vec.x
pixel.v = ch * vec.y
if (pixel.u < 0): pixel.u = 0
if (pixel.u > cw - 1): pixel.u = cw - 1
if (pixel.v < 0): pixel.v = 0
if (pixel.v > ch - 1): pixel.v = 0
return pixel
def ground2pixel(self, point):
ground_point = np.array([point.x, point.y, 1.0])
image_point = np.dot(self.Hinv, ground_point)
image_point = image_point / image_point[2]
pixel = Pixel()
if not self.rectified_input:
distorted_pixel = self.pcm_.project3dToPixel(image_point)
pixel.u = distorted_pixel[0]
pixel.v = distorted_pixel[1]
else:
pixel.u = image_point[0]
pixel.v = image_point[1]
def JoyButton(self, joy_msg):
if (joy_msg.buttons[0] == 1):
rospy.loginfo("[%s] You Press Button A " % (self.node_name))
state = BoolStamped()
state.data = False
self.pub_A.publish(state)
elif (joy_msg.buttons[1] == 1):
rospy.loginfo("[%s] You Press Button B " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_B.publish(state)
elif (joy_msg.buttons[2] == 1):
rospy.loginfo("[%s] You Press Button X " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_X.publish(state)
elif (joy_msg.buttons[3] == 1):
rospy.loginfo("[%s] You Press Button Y " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_Y.publish(state)
else:
some_active = sum(joy_msg.buttons)
if some_active:
rospy.loginfo("[%s] No binding buttons " % (self.node_name))
#gripperrrrrr_cmd
if (joy_msg.buttons[0] == 1):
rospy.loginfo("Grip Open")
g_cmd = Pixel()
g_cmd.u = 1
self.pub_Grip.publish(g_cmd)
elif (joy_msg.buttons[1] == 1):
rospy.loginfo("Grip Close")
g_cmd = Pixel()
g_cmd.u = 2
self.pub_Grip.publish(g_cmd)
else:
rospy.loginfo("Grip Stop")
g_cmd = Pixel()
g_cmd.u = 0
self.pub_Grip.publish(g_cmd)
def ground2pixel(self, point):
# TODO check whether z=0 or z=1.
# I think z==1 (jmichaux)
ground_point = np.array([point.x, point.y, 1.0])
image_point = self.Hinv * ground_point
image_point = np.abs(image_point / image_point[2])
pixel = Pixel()
if not self.rectified_input:
distorted_pixel = self.pcm_.project3dToPixel(image_point)
pixel.u = distorted_pixel[0]
pixel.v = distorted_pixel[1]
else:
pixel.u = image_point[0]
pixel.v = image_point[1]
def ground2pixel(self, point):
if point.z != 0:
msg = 'This method assumes that the point is a ground point (z=0). '
msg += 'However, the point is (%s,%s,%s)' % (point.x, point.y,
point.z)
raise ValueError(msg)
ground_point = np.array([point.x, point.y, 1.0])
# An applied mathematician would cry for this
# image_point = np.dot(self.Hinv, ground_point)
# A better way:
image_point = np.linalg.solve(self.H, ground_point)
image_point = image_point / image_point[2]
pixel = Pixel()
# if not self.rectified_input:
# dtu.logger.debug('project3dToPixel')
# distorted_pixel = self.pcm.project3dToPixel(image_point)
# pixel.u = distorted_pixel[0]
# pixel.v = distorted_pixel[1]
# else:
pixel.u = image_point[0]
pixel.v = image_point[1]
return pixel
def vector2pixel(self, vec):
""" Converts a [0,1]*[0,1] representation to [0, W]x[0, H]. """
pixel = Pixel()
cw = self.ci.width
ch = self.ci.height
pixel.u = cw * vec.x
pixel.v = ch * vec.y
return pixel