Extracting output data from launch file node
Hi, I am using ROS hydro 12.04 LTS. Currently I successfully launching the motion tracking node based on openni and kinect. There is the data shown at the terminal whenever there is a motion in front of the kinect camera. However, when I look at rqt_graph , there is no published topic for that data and I am wondering why.
Can anyone suggest on how to extract those data to matlab or other program? I can share the developed package if u want to try it. Thanks for your concern.
I had tried to write a code publishing to a topic on the "blob-parameter" line below but there is an error where the global name for std_msgs not defined and error in code syntax : #!/usr/bin/env python
-- coding: utf-8 --
import rospy import sys import cv2 import cv2.cv as cv from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge, CvBridgeError import numpy as np from numpy import * import os.path import time from std_msgs.msg import String
def bg_diff(fn_bg,im_in,th,blur):
im_bg = fn_bg
diff = cv2.absdiff(im_in,im_bg)
mask = diff < th
hight = im_bg.shape[0]
width = im_bg.shape[1]
im_mask = np.zeros((hight,width),np.uint8)
im_mask[mask]=255
im_mask = cv2.medianBlur(im_mask,blur)
im_edge = cv2.Canny(im_mask,100,200)
return im_bg, im_in, im_mask, im_edge
def nothing(x): pass
class bgsub2(): def __init__(self): self.node_name = "bgsub"
rospy.init_node(self.node_name)
self.th = rospy.get_param("~move_threshold", "60")
rospy.on_shutdown(self.cleanup)
cv2.namedWindow("Motion Edge")
cv2.createTrackbar("threshold", "Motion Edge", self.th, 255, nothing)
self.isStart = True
self.cv_window_name = self.node_name
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
cv.NamedWindow("Depth Image", cv.CV_WINDOW_NORMAL)
cv.MoveWindow("Depth Image", 25, 350)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
self.depth_sub = rospy.Subscriber("input_depth_image", Image, self.depth_callback, queue_size=1)
rospy.loginfo("Waiting for image topics...")
rospy.wait_for_message("input_rgb_image", Image)
rospy.wait_for_message("input_depth_image", Image)
rospy.loginfo("Ready.")
def image_callback(self, ros_image):
try:
frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
except CvBridgeError, e:
print e
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
self.th = cv2.getTrackbarPos("threshold", "Motion Edge")
if self.isStart == True:
self.frame_prev = frame
self.isStart = False
return
im_bg,im_in,im_mask,im_edge = bg_diff(self.frame_prev,frame,self.th,blur=7)
cv2.imshow("Input",im_in)
cv2.imshow("Background",im_bg)
img_size = im_edge.shape
params = cv2.SimpleBlobDetector_Params()
params.filterByArea = True
params.minArea = 10
Blob-parameter detector
detector = cv2.SimpleBlobDetector(params)
keypoints = detector.detect(im_edge)
print len(keypoints)
if len(keypoints) != 0:
ptx=0
pty=0
for key in keypoints:
ptx+=key.pt[0]
pty+=key.pt[1]
pt= ( int(ptx/ len(keypoints)), int(pty/ len(keypoints)))
print pt
self.pt = rospy.Publisher('pointer', std_msgs.msg.String, queue_size=1)
else:
pass
im_edge = cv2.drawKeypoints(im_edge, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
cv2.imshow("Motion Edge",im_edge)
self.frame_prev = frame
self.keystroke = cv2.waitKey(5)
if self.keystroke != -1:
cc = chr(self.keystroke & 255).lower()
if cc == 'q':
rospy.signal_shutdown("User hit q ...