point_cloud_ros: set_og_params.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import numpy as np, math
00004 
00005 import roslib; roslib.load_manifest('point_cloud_ros')
00006 import rospy
00007 import point_cloud_ros.ros_occupancy_grid as rog
00008 from point_cloud_ros.msg import OccupancyGrid
00009 
00010 from visualization_msgs.msg import Marker
00011 
00012 ## return a Marker message object
00013 # @param loc - (x,y,z)
00014 # @param scale - (x,y,z)
00015 # @param color - (r,g,b,a)
00016 # @param shape - 'sphere'
00017 # @param frame_id - string.
00018 def simple_viz_marker(loc, scale, color, shape, frame_id):
00019     marker = Marker()
00020     marker.header.frame_id = frame_id
00021     marker.header.stamp = rospy.rostime.get_rostime()
00022     marker.ns = 'basic_shapes'
00023     marker.id = 0
00024     if shape == 'sphere':
00025         marker.type = Marker.SPHERE
00026     marker.action = Marker.ADD
00027 
00028     marker.pose.position.x = loc[0]
00029     marker.pose.position.y = loc[1]
00030     marker.pose.position.z = loc[2]
00031     marker.pose.orientation.x = 0.0
00032     marker.pose.orientation.y = 0.0
00033     marker.pose.orientation.z = 0.0
00034     marker.pose.orientation.w = 1.0
00035     marker.scale.x = scale[0]
00036     marker.scale.y = scale[1]
00037     marker.scale.z = scale[2]
00038     marker.color.r = color[0]
00039     marker.color.g = color[1]
00040     marker.color.b = color[2]
00041     marker.color.a = color[3]
00042     marker.lifetime = rospy.Duration()
00043     return marker
00044 
00045 if __name__ == '__main__':
00046     rospy.init_node('set_og_param_node')
00047     og_param_pub = rospy.Publisher('/og_params', OccupancyGrid)
00048     marker_pub = rospy.Publisher('/occupancy_grid_viz_marker', Marker)
00049 
00050     rospy.logout('Ready')
00051 
00052     center = np.matrix([0.8, 0., 0.8]).T # for single object bag file
00053     #center = np.matrix([0.6, 0., 0.75]).T
00054     size = np.matrix([0.4, 0.4, 0.4]).T
00055     #resolution = np.matrix([0.01, 0.01, 0.01]).T
00056     resolution = np.matrix([0.005, 0.005, 0.005]).T
00057     occupancy_threshold = 5
00058     frame_id = 'base_link'
00059 
00060     scale = (0.02, 0.02, 0.02)
00061     color = (0., 1., 0., 1.)
00062     shape = 'sphere'
00063     marker = simple_viz_marker(center.A1, scale, color, shape, frame_id)
00064     og_param = rog.og_param_msg(center, size, resolution,
00065                                 occupancy_threshold, frame_id)
00066 
00067     r = rospy.Rate(2)
00068     while not rospy.is_shutdown():
00069         marker_pub.publish(marker)
00070         og_param_pub.publish(og_param)
00071         r.sleep()
00072 
00073     rospy.spin()
00074 
00075