本文整理汇总了Python中visualization_msgs.msg.Marker.CUBE属性的典型用法代码示例。如果您正苦于以下问题:Python Marker.CUBE属性的具体用法?Python Marker.CUBE怎么用?Python Marker.CUBE使用的例子?那么, 这里精选的属性代码示例或许可以为您提供帮助。您也可以进一步了解该属性所在类visualization_msgs.msg.Marker
的用法示例。
在下文中一共展示了Marker.CUBE属性的8个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: handle_image_msg
# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 别名]
def handle_image_msg(msg):
assert msg._type == 'sensor_msgs/Image'
with g_fusion_lock:
g_fusion.filter(EmptyObservation(msg.header.stamp.to_sec()))
if g_fusion.last_state_mean is not None:
pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)
yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, pose[3])
br = ros_tf.TransformBroadcaster()
br.sendTransform(tuple(pose[:3]), tuple(yaw_q), rospy.Time.now(), 'obj_fuse_centroid', 'velodyne')
if last_known_box_size is not None:
# bounding box
marker = Marker()
marker.header.frame_id = "obj_fuse_centroid"
marker.header.stamp = rospy.Time.now()
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.scale.x = last_known_box_size[2]
marker.scale.y = last_known_box_size[1]
marker.scale.z = last_known_box_size[0]
marker.color = ColorRGBA(r=0., g=1., b=0., a=0.5)
marker.lifetime = rospy.Duration()
pub = rospy.Publisher("obj_fuse_bbox", Marker, queue_size=10)
pub.publish(marker)
示例2: to_marker
# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 别名]
def to_marker(self, robot):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.pose.position.x = self.x
marker.pose.position.y = self.y
marker.pose.position.z = 0
quaternion = tf.transformations.quaternion_from_euler(0, 0, self.theta)
marker.pose.orientation.x = quaternion[0]
marker.pose.orientation.y = quaternion[1]
marker.pose.orientation.z = quaternion[2]
marker.pose.orientation.w = quaternion[3]
marker.scale.x = robot.width
marker.scale.y = robot.height
marker.scale.z = 0.2
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.type = Marker.CUBE
marker.action = marker.ADD
return marker
示例3: sync_callback
# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 别名]
def sync_callback(msg1, msg2):
# msg1: /image_raw # msg2: /velodyne_points: velodyne_points
func_start = time.time()
timestamp1 = msg1.header.stamp.to_nsec()
print('image_callback: msg : seq=%d, timestamp=%19d' % (msg1.header.seq, timestamp1))
timestamp2 = msg2.header.stamp.to_nsec()
print('velodyne_callback: msg : seq=%d, timestamp=%19d' % (msg2.header.seq, timestamp2))
arr = msg_to_arr(msg2)
lidar = np.array([[item[0], item[1], item[2], item[3]] for item in arr])
camera_image = bridge.imgmsg_to_cv2(msg1, "bgr8")
print("camera_image is {}".format(camera_image.shape))
top_view = point_cloud_2_top(lidar, res=0.2, zres=0.5, side_range=(-45,45), fwd_range=(-45,45),
height_range=(-3, 0.5))
top_image = draw_top_image(top_view[:, :, -1])
if 0: # if show the images
cemara_show_image = cv2.resize(camera_image,(camera_image.shape[1]//2, camera_image.shape[0]//2))
top_show_image_width = camera_image.shape[0]//2
top_show_image = cv2.resize(top_image,(top_show_image_width, top_show_image_width))
show_image = np.concatenate((top_show_image, cemara_show_image), axis=1)
cv2.imshow("top", show_image)
cv2.waitKey(1)
# use test data until round2 pipeline is ok
np_reshape = lambda np_array: np_array.reshape(1, *(np_array.shape))
top_view = np_reshape(top)
front_view = np_reshape(front)
rgb_view = np_reshape(rgb)
np.save(os.path.join(sys.path[0], "../MV3D/data/", "top.npy"), top_view)
np.save(os.path.join(sys.path[0], "../MV3D/data/", "rgb.npy"), rgb_view)
start = time.time()
boxes3d = rpc.predict()
end = time.time()
print("predict boxes len={} use predict time: {} seconds.".format(len(boxes3d), end-start))
if len(boxes3d) > 0:
translation, size, rotation = boxes3d_decompose(np.array(boxes3d))
# publish (boxes3d) to tracker_node
markerArray = MarkerArray()
for i in range(len(boxes3d)):
m = Marker()
m.type = Marker.CUBE
m.header.frame_id = "velodyne"
m.header.stamp = msg2.header.stamp
m.scale.x, m.scale.y, m.scale.z = size[i][0],size[i][1],size[i][2]
m.pose.position.x, m.pose.position.y, m.pose.position.z = \
translation[i][0], translation[i][1], translation[i][2]
m.pose.orientation.x, m.pose.orientation.y, m.pose.orientation.z, m.pose.orientation.w = \
rotation[i][0], rotation[i][1], rotation[i][2], 0.
m.color.a, m.color.r, m.color.g, m.color.b = \
1.0, 0.0, 1.0, 0.0
markerArray.markers.append(m)
pub.publish(markerArray)
func_end = time.time()
print("sync_callback use {} seconds".format(func_end - func_start))
示例4: simple_obs
# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 别名]
def simple_obs(self):
self.no_obs()
pose_stamped = geometry_msgs.msg.PoseStamped()
pose_stamped.header.frame_id = "/world_link"
pose_stamped.header.stamp = rospy.Time(0)
pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.2)) )
self.scene.add_box("obs1", pose_stamped,(0.1,0.1,1))
self.scene.add_box("box1", pose_stamped,(0.05, 0.05, 0.95))
obs = MarkerArray()
obj = Marker()
obj.header.frame_id = "/world_link"
obj.header.stamp = rospy.Time(0)
obj.ns = 'obstacle'
obj.id = 1
obj.type = Marker.CUBE
obj.action = Marker.ADD
obj.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.2)) )
obj.scale.x = 0.1
obj.scale.y = 0.1
obj.scale.z = 1.0
obj.color.r = 0.0
obj.color.g = 1.0
obj.color.b = 0.0
obj.color.a = 1.0
obs.markers.append(obj)
self.marker_pub.publish(obs)
示例5: handle_msg_car
# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 别名]
def handle_msg_car(msg, who):
assert isinstance(msg, Odometry)
global last_cap_r, last_cap_f, last_cap_yaw
if who == 'cap_r':
last_cap_r = rtk_position_to_numpy(msg)
elif who == 'cap_f' and last_cap_r is not None:
cap_f = rtk_position_to_numpy(msg)
cap_r = last_cap_r
last_cap_f = cap_f
last_cap_yaw = get_yaw(cap_f, cap_r)
elif who == 'obs_r' and last_cap_f is not None and last_cap_yaw is not None:
md = None
for obs in metadata:
if obs['obstacle_name'] == 'obs1':
md = obs
assert md, 'obs1 metadata not found'
# find obstacle rear RTK to centroid vector
lrg_to_gps = [md['rear_gps_l'], -md['rear_gps_w'], md['rear_gps_h']]
lrg_to_centroid = [md['l'] / 2., -md['w'] / 2., md['h'] / 2.]
obs_r_to_centroid = np.subtract(lrg_to_centroid, lrg_to_gps)
# in the fixed GPS frame
cap_f = last_cap_f
obs_r = rtk_position_to_numpy(msg)
# in the capture vehicle front RTK frame
cap_to_obs = np.dot(rotMatZ(-last_cap_yaw), obs_r - cap_f)
cap_to_obs_centroid = cap_to_obs + obs_r_to_centroid
br = tf.TransformBroadcaster()
br.sendTransform(tuple(cap_to_obs_centroid), (0,0,0,1), rospy.Time.now(), 'obs_centroid', 'gps_antenna_front')
# publish obstacle bounding box
marker = Marker()
marker.header.frame_id = "obs_centroid"
marker.header.stamp = rospy.Time.now()
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.scale.x = md['l']
marker.scale.y = md['w']
marker.scale.z = md['h']
marker.color.r = 0.2
marker.color.g = 0.5
marker.color.b = 0.2
marker.color.a = 0.5
marker.lifetime = rospy.Duration()
pub = rospy.Publisher("obs_bbox", Marker, queue_size=10)
pub.publish(marker)
示例6: handle_radar_msg
# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 别名]
def handle_radar_msg(msg, dont_fuse):
assert msg._md5sum == '6a2de2f790cb8bb0e149d45d297462f8'
publish_rviz_topics = True
with g_fusion_lock:
# do we have any estimation?
if g_fusion.last_state_mean is not None:
pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)
observations = RadarObservation.from_msg(msg, RADAR_TO_LIDAR, 0.9115)
# find nearest observation to current object position estimation
distance_threshold = 4.4
nearest = None
nearest_dist = 1e9
for o in observations:
dist = [o.x - pose[0], o.y - pose[1], o.z - pose[2]]
dist = np.sqrt(np.array(dist).dot(dist))
if dist < nearest_dist and dist < distance_threshold:
nearest_dist = dist
nearest = o
if nearest is not None:
# FUSION
if not dont_fuse:
g_fusion.filter(nearest)
if publish_rviz_topics:
header = Header()
header.frame_id = '/velodyne'
header.stamp = rospy.Time.now()
point = np.array([[nearest.x, nearest.y, nearest.z]], dtype=np.float32)
rospy.Publisher(name='obj_radar_nearest',
data_class=PointCloud2,
queue_size=1).publish(pc2.create_cloud_xyz32(header, point))
#pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)
#br = ros_tf.TransformBroadcaster()
#br.sendTransform(tuple(pose), (0,0,0,1), rospy.Time.now(), 'car_fuse_centroid', 'velodyne')
# if last_known_box_size is not None:
# # bounding box
# marker = Marker()
# marker.header.frame_id = "car_fuse_centroid"
# marker.header.stamp = rospy.Time.now()
# marker.type = Marker.CUBE
# marker.action = Marker.ADD
# marker.scale.x = last_known_box_size[2]
# marker.scale.y = last_known_box_size[1]
# marker.scale.z = last_known_box_size[0]
# marker.color = ColorRGBA(r=1., g=1., b=0., a=0.5)
# marker.lifetime = rospy.Duration()
# pub = rospy.Publisher("car_fuse_bbox", Marker, queue_size=10)
# pub.publish(marker)
示例7: _img_callback
# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 别名]
def _img_callback(self, msg):
frame_index = self.timestamp_map[msg.header.stamp.to_nsec()]
for i, f in enumerate(self.frame_map[frame_index]):
trans = f.trans
rotq = f.rotq
h, w, l = f.size
if f.gt:
# This is a bit hackish but will show one (the first) gt with fixed color
# and will show multiple predicted objects, but colors per object will change
# if the number of objects predicted changes.
bid = 16
oid = 17
color = kelly_colors_dict['very_light_blue']
else:
bid = i * 2
oid = i * 2 + 1
color = kelly_colors_list[i % len(kelly_colors_list)]
if self.sphere:
s = max(l, w, h)
self._publish_marker(
Marker.SPHERE, bid, msg.header.stamp,
trans, rotq, [s, s, s],
color)
self._publish_marker(
Marker.ARROW, oid, msg.header.stamp,
trans, rotq, [l/2, w/2, h/2],
color)
else:
#FIXME change Marker types based on object type (ie car vs pedestrian)
if f.object_type == 'Pedestrian':
self._publish_marker(
Marker.CYLINDER, bid, msg.header.stamp,
trans, rotq, [w, w, h],
color)
self._publish_marker(
Marker.ARROW, oid, msg.header.stamp,
trans, rotq, [w/2, w/2, w/2],
color)
else:
self._publish_marker(
Marker.CUBE, bid, msg.header.stamp,
trans, rotq, [l, w, h],
color)
self._publish_marker(
Marker.ARROW, oid, msg.header.stamp,
trans, rotq, [l/2, w/2, h/2],
color)
self._send_transform(trans, rotq, i)
示例8: complex_obs
# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 别名]
def complex_obs(self):
self.no_obs()
pose_stamped = geometry_msgs.msg.PoseStamped()
pose_stamped.header.frame_id = "/world_link"
pose_stamped.header.stamp = rospy.Time(0)
pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.4)) )
self.scene.add_box("obs1", pose_stamped,(0.1,0.1,0.8))
self.scene.add_box("box1", pose_stamped,(0.05,0.05,0.75))
pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.0, 0.8)) )
self.scene.add_box("obs2", pose_stamped,(0.1,0.5,0.1))
self.scene.add_box("box2", pose_stamped,(0.05,0.45,0.05))
obs = MarkerArray()
obj1 = Marker()
obj1.header.frame_id = "/world_link"
obj1.header.stamp = rospy.Time(0)
obj1.ns = 'obstacle'
obj1.id = 1
obj1.type = Marker.CUBE
obj1.action = Marker.ADD
obj1.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.4)) )
obj1.scale.x = 0.1
obj1.scale.y = 0.1
obj1.scale.z = 0.8
obj1.color.r = 0.0
obj1.color.g = 1.0
obj1.color.b = 0.0
obj1.color.a = 1.0
obs.markers.append(obj1)
obj2 = Marker()
obj2.header.frame_id = "/world_link"
obj2.header.stamp = rospy.Time(0)
obj2.ns = 'obstacle'
obj2.id = 2
obj2.type = Marker.CUBE
obj2.action = Marker.ADD
obj2.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.0, 0.8)) )
obj2.scale.x = 0.1
obj2.scale.y = 0.5
obj2.scale.z = 0.1
obj2.color.r = 0.0
obj2.color.g = 1.0
obj2.color.b = 0.0
obj2.color.a = 1.0
obs.markers.append(obj2)
self.marker_pub.publish(obs)