Publish nav_msgs::OccupancyGrid message in python

最新推荐文章于 2024-05-01 17:51:09 发布

莫马达

最新推荐文章于 2024-05-01 17:51:09 发布

阅读量446

点赞数

分类专栏： ROS python 文章标签： python

本文链接：https://blog.csdn.net/have_fun_/article/details/114386726

版权

ROS 同时被 2 个专栏收录

5 篇文章 0 订阅

订阅专栏

python

2 篇文章 0 订阅

订阅专栏

Publish nav_msgs::OccupancyGrid message in python

here is the demo.

#!/usr/bin/python3
'''
A Demo, to publish ros map message (nav_msgs::OccupancyGrid), is written 
in python.

Has been tested on ubuntu16.04 ros kinetic x86_64 platform

To use python3 as the interpreter, opencv for python3 will be needed, 
'''
import rospy
from nav_msgs.msg import OccupancyGrid
from std_msgs.msg import Int8MultiArray
import numpy as np
import sys
python3 = True if sys.hexversion > 0x03000000 else False
if python3:
    # remove ros python2 library
    ros_cv_path = '/opt/ros/kinetic/lib/python2.7/dist-packages'
    if ros_cv_path in sys.path:
        sys.path.remove(ros_cv_path)

import cv2 as cv

def RosInit():
    rospy.init_node('map_pub_node')
    pub = rospy.Publisher('map', OccupancyGrid, queue_size = 1000)
    return pub

def MakeMapMsg(img, resolution, position, orientation):
    map_msg = OccupancyGrid()
    map_msg.header.stamp = rospy.Time.now()
    map_msg.info.map_load_time = rospy.Time.now()
    map_msg.info.width = img.shape[0]
    map_msg.info.height = img.shape[1]
    map_msg.info.resolution = resolution
    map_msg.info.origin.position.x = position[0]
    map_msg.info.origin.position.y = position[1]
    map_msg.info.origin.position.z = position[2]
    map_msg.info.origin.orientation.w = orientation[0]
    map_msg.info.origin.orientation.x = orientation[1]
    map_msg.info.origin.orientation.y = orientation[2]
    map_msg.info.origin.orientation.z = orientation[3]
    img = cv.flip(img, 0, dst=None)
    img_data_int8array = [i for row in img.tolist() for i in row]
    map_msg.data = Int8MultiArray(data=img_data_int8array).data
    return map_msg

def DrawMapImg():
    img = np.zeros((600, 600, 1), np.uint8)
    cv.rectangle(img, (200,100), (400,150), 100, -1)
    cv.rectangle(img, (350,100), (400,400), 100, -1)
    return img

def ShowMapImg(img, tval=0):
    cv.imshow('map_msg', img)
    cv.waitKey(tval)

if __name__ == "__main__":
    pub = RosInit()
    img = DrawMapImg()
    ShowMapImg(img)
    msg = MakeMapMsg(img, 1, [0, 0, 0], [1, 0, 0, 0])
    while not rospy.is_shutdown():
        pub.publish(msg)
        rospy.loginfo('map_msg has been published, Ctrl+C to exit')
        rospy.sleep(1)