void mapCallback(const nav_msgs::OccupancyGrid& msg)
{
ROS_INFO(“I heard frame_id: [%s]”, msg.header.frame_id.c_str());
resolution = msg.info.resolution;
width = msg.info.width;
height = msg.info.height;
originx = msg.info.origin.position.x;
originy = msg.info.origin.position.y;
mymap = msg;
map_ = cv::Mat(height,width,CV_8U);
for(int i = 0;i< height;i++)
{
for(int j=0;j< width;j++)
{
if(mymap.data[i*width+j] == 100)
{
map_.at<uchar>(i,j) = 255;
}
else
{
map_.at<uchar>(i,j) = 0;
}
}
}
thin(map_,map_,2);
std::vector<cv::Vec4i> l;
cv::HoughLinesP(map_,l,1,0.1,10, 0,10);
ROS_INFO(“detect line:%d”,l.size());
for(int i=0;i< l.size();i++)
{
cv::Vec4i I = l[i];
cv::line(map_,cv::Point(I[0]+100,I[1]+100),cv::Point(I[2]+100,I[3]+100),cv::Scalar(128),1,CV_AA);
}/**/
return;
}