ROS中Opencv的canny边缘检测

roslaunch turtlebot_stage turtlebot_in_stage.launch
 

查看rostopic list

import rospy
import actionlib
import numpy as np
import cv2,cv_bridge
from copy import copy
from sensor_msgs.msg import Image
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from nav_msgs.msg import OccupancyGrid
from move_base_msgs.msg  import MoveBaseAction,MoveBaseGoal,MoveBaseActionGoal

def callback(mapData):

	data=mapData.data
	w=mapData.info.width
	print "width: "+str(w)
	h=mapData.info.height
	print "height: "+str(h)
	resolution=mapData.info.resolution
	print "resolution: "+str(resolution)
	Xstartx=mapData.info.origin.position.x
	print "Xstartx: "+str(Xstartx)
	Xstarty=mapData.info.origin.position.y
	print "Xstarty: "+str(Xstarty)
	 
	img = np.zeros((h, w, 1), np.uint8)
	
	for i in range(0,h):
		for j in range(0,w):
			if data[i*w+j]==100:
				img[i,j]=0
			elif data[i*w+j]==0:
				img[i,j]=255
			elif data[i*w+j]==-1:
				img[i,j]=205
	
	
       	o=cv2.inRange(img,0,1)
	edges = cv2.Canny(img,0,255)
	
	contours, hierarchy = cv2.findContours(o,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)		
	cv2.drawContours(o, contours, -1, (255,255,255), 5)
	color=cv2.cvtColor(img,cv2.COLOR_GRAY2BGR)
	cv2.imshow("contours",color)
	cv2.waitKey()
	cv2.destroyAllWindows()
      
rospy.init_node('sub--1')
mapData=OccupancyGrid()
sub=rospy.Subscriber('/map',OccupancyGrid,callback)
rospy.spin()

 

查看Odometry属于那个类型

 

运行

得出地图边缘检测结果，地图size中的width，height，分辨率，地图的初始位置。