def __init__(self):
self.pub_out = rospy.Publisher("command",
ChannelFloat32,
tcp_nodelay=True)
self.output = ChannelFloat32()
self.input = ChannelFloat32()
self.output.name = "output"
self.output.values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.input.name = "input"
self.input.values = [0, 0, 0, 0]
self.fan_temp_on = 60
self.fan_temp_off = 55
self.lights = [LightControl("light_controller", self, [3, 6, 2])]
self.motors = [
TrajectoryControl("tray_controller", "tray_joint", self, 0, 0)
]
self.fans = [FanControl("fan_controller", self, 7)]
rospy.Subscriber("state", ChannelFloat32, self.InputCallback)
#reset all motors
for m in self.motors:
m.move0()
def createPointCloudMsg(points, rgbs):
pc_msg = PointCloud()
pc_msg.header.frame_id = "aruco_slam_world"
r_c = ChannelFloat32()
g_c = ChannelFloat32()
b_c = ChannelFloat32()
r_c.name = "r"
g_c.name = "g"
b_c.name = "b"
for i in range(len(points)):
p = points[i]
_p = Point32()
_p.x = p[0]
_p.y = p[1]
_p.z = p[2]
pc_msg.points.append(_p)
rgb = rgbs[i]
r = abs(rgb[2] - 255)
g = abs(rgb[1] - 255)
b = abs(rgb[0] - 255)
r_c.values.append(r)
g_c.values.append(g)
b_c.values.append(b)
pc_msg.channels.append(r_c)
pc_msg.channels.append(g_c)
pc_msg.channels.append(b_c)
return pc_msg
def tensor_2d_to_pointcloud(self, ndarray, axis0_size_m, frame="/map_ned"):
msg = PointCloud()
msg.header.frame_id = frame
msg.header.stamp = rospy.get_rostime()
# Shift and scale to 0-1 range
map_data_np = ndarray[:, :, :3]
map_data_np = map_data_np - np.min(map_data_np)
map_data_np /= (np.max(map_data_np) + 1e-9)
c = np.linspace(0, axis0_size_m, map_data_np.shape[1])
cgrid = np.asarray(np.meshgrid(c, c))
r = ChannelFloat32()
r.name = "r"
g = ChannelFloat32()
g.name = "g"
b = ChannelFloat32()
b.name = "b"
msg.channels.append(r)
msg.channels.append(g)
msg.channels.append(b)
for x in range(map_data_np.shape[0]):
for y in range(map_data_np.shape[1]):
p = Point32()
p.x = cgrid[0, x, y]
p.y = cgrid[1, x, y]
p.z = 0.05
r.values.append(float(map_data_np[x, y, 0]))
g.values.append(float(map_data_np[x, y, 1]))
b.values.append(float(map_data_np[x, y, 2]))
msg.points.append(p)
return msg
def callback(data):
global left
global right
#global frame
img = bridge.imgmsg_to_cv2(data)
image = process_image(img)
image = bridge.cv2_to_imgmsg(image)
pub_image.publish(image)
left_points, right_points = pub_scatter(left, right)
left_msg = PointCloud()
left_msg.header.stamp = rospy.Time.now()
left_msg.header.frame_id = "footprint"
left_msg.points = left_points
left_msg_chan = ChannelFloat32()
left_msg_chan.name = 'value'
left_msg_chan.values = [float(3)]
left_msg.channels = [left_msg_chan]
right_msg = PointCloud()
right_msg.header.stamp = rospy.Time.now()
right_msg.header.frame_id = "footprint"
right_msg.points = right_points
right_msg_chan = ChannelFloat32()
right_msg_chan.name = 'value'
right_msg_chan.values = [float(3)]
right_msg.channels = [right_msg_chan]
pub_point_cloud_right.publish(right_msg)
pub_point_cloud_left.publish(left_msg)
def local_rand_cloud():
#Create a new cloud object and stuff it
ROS_pointcloud = PointCloud() #sensor_msgs.msg.PointCloud()
x = random() * 5 #const offset easy to change.
#stuff point cloud with random points in a rectangle which drifts over time.
#Add an extra intensity channel as well.
intensity_channel = ChannelFloat32()
intensity_channel.name = 'intensities'
for i in range(3000):
intensity_channel.values += [random()]
for i in range(3000):
ROS_pointcloud.points += [
Point32(random() * 5 - x, random(), intensity_channel.values[i])
]
#rgb color has maximum value of 0xFFFFFF
rgb_channel = ChannelFloat32()
rgb_channel.name = 'rgb'
rgb_channel.values = normList(intensity_channel.values, 0xFF)
#Separate r,g,b channels range betweeon 0 and 1.0.
r, g, b = [], [], []
for pt in ROS_pointcloud.points:
b += [pt.y]
r += [pt.z]
g += [0]
r_channel = ChannelFloat32(name='r', values=normList(r))
g_channel = ChannelFloat32(name='g', values=normList(g))
b_channel = ChannelFloat32(name='b', values=b)
ROS_pointcloud.channels = (intensity_channel, rgb_channel, r_channel,
g_channel, b_channel)
return ROS_pointcloud
def pt_cloud_setup(self, nx, ny, gx, gy):
pt_cloud = PointCloud()
pt_cloud.header = std_msgs.msg.Header()
pt_cloud.header.stamp = rospy.Time.now()
pt_cloud.header.frame_id = self.frameid
pt_cloud.points = [None] * (nx * ny)
colors = ['r', 'g', 'b']
for color in colors:
ch = Channel()
ch.name = color
ch.values = []
pt_cloud.channels.append(ch)
cnt = 0
for i1 in range(0, nx):
for i2 in range(0, ny):
pt_cloud.points[cnt] = Point(i1 * gx, i2 * gy, 0)
pt_cloud.channels[0].values.append(0.0)
pt_cloud.channels[1].values.append(0.0)
pt_cloud.channels[2].values.append(0.0)
cnt = cnt + 1
return pt_cloud
def __init__(self):
self.kernal = self.make_kernal(11, 'circle')
self.kernal1 = self.make_kernal(6, 'rect')
self.kernal2 = self.make_kernal(11, 'rect')
self.kernal3 = self.make_kernal(25, 'circle')
self.kernal4 = self.make_kernal(5, 'circle')
self.kernal5 = self.make_kernal(5, 'rect')
self.kernal6 = self.make_kernal(25, 'circle')
self.kernal_size = 25
self.kernal7 = self.make_kernal(self.kernal_size, 'circle')
self.kernal8 = self.make_kernal(2, 'rect')
self.kernal9 = self.make_kernal(2, 'rect')
self.kernal10 = self.make_kernal(45, 'circle')
self.M = np.load('M_gs1_newSensor_427_320.npy')
# self.ROImask = np.load('mask_GS2.npy self.previous_slip = False
self.previous_u_sum = np.array([0])
self.previous_v_sum = np.array([0])
self.static_flag = False
self.index = 0
self.cols, self.rows, self.cha = 320, 427, 3
self.x1_last = []
self.y1_last = []
self.highbar_top = 130
self.lowbar_top = 100
self.highbar_down = 80
self.lowbar_down = 60
self.img_counter = 0
self.thre = 80
self.scale = 1
self.con_flag = False
self.refresh = False
self.restart = False
self.initial_flag = True
self.length_flag = True
self.marker_refflag = True
self.showimage = True
self.thre_slip_dis = 4.5
self.thre_slip_num = 7
self.slip_indicator = False
self.slip_monitor = ChannelFloat32()
self.tran_matrix_msg = ChannelFloat32()
self.slip_pub = rospy.Publisher('/raspicam_node1/slip_monitor',
ChannelFloat32,
queue_size=1)
#change to your own rostopic name to subsribe gelslim raw images
self.image_sub = rospy.Subscriber("/raspicam_node1/image/compressed",
CompressedImage,
self.call_back,
queue_size=1,
buff_size=2**24)
self.s = rospy.Service('raspicam_node1/restart_detector',
std_srvs.srv.Empty,
self.restart_detector_server)
self.useSlipDetection = True #True to detect slip, False to detect collision
self.collideThre = 1.5
def get_new_pointcloud(self):
from sensor_msgs.msg import PointCloud
from geometry_msgs.msg import Point32
from sensor_msgs.msg import ChannelFloat32
Points = [Point32(1,2,3), Point32(4,5,6)]
Channels = [ChannelFloat32("myname1", [1.23, 4.56]),
ChannelFloat32("myname2", [1.231, 4.561])]
return PointCloud(None, Points, Channels)
def callback(blobs_msg):
puck_array = PuckArray()
vis_pucks = PointCloud()
vis_pucks.header.frame_id = '/base_link'
channel = ChannelFloat32()
channel.name = "intensity"
for blob in blobs_msg.blobs:
try:
puck = Puck()
puck.xi = blob.x
puck.yi = blob.y
puck.type = blob.type
(puck.position.x, puck.position.y) = corresDict[puck.xi, puck.yi]
puck_array.pucks.append(puck)
vis_pucks.points.append(puck.position)
channel.values.append(0)
except:
continue
puck_publisher.publish(puck_array)
vis_pucks.channels.append(channel)
vis_pucks_publisher.publish(vis_pucks)
def to_pointcloud(self, frame):
"""Returns a PointCloud message corresponding to slice.
Args:
frame: Frame ID.
Returns:
A sensor_msgs.msg.PointCloud.
"""
# Construct PointCloud message.
cloud = PointCloud()
cloud.header.frame_id = frame
cloud.header.stamp = self.timestamp
# Convert bins to list of Point32 messages.
nbins = self.config["nbins"]
r_step = self.range / nbins
x_unit = math.cos(self.heading) * r_step
y_unit = math.sin(self.heading) * r_step
cloud.points = [
Point32(x=x_unit * r, y=y_unit * r, z=0.00)
for r in range(1, nbins + 1)
]
# Set intensity channel.
channel = ChannelFloat32()
channel.name = "intensity"
channel.values = self.bins
cloud.channels = [channel]
return cloud
def get_point_cloud_as_msg(self):
"""Return a ROS point cloud sensor message with the points representing
the plume's particles and one channel containing the time of creation
for each particle.
> **Returns**
The plume particles as a `sensor_msgs/PointCloud` message or `None`
if no particles are found.
"""
if self._pnts is None or self._time_creation is None:
return None
pc = PointCloud()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'world'
if self._pnts is None:
return None
pc.points = [
Point32(x, y, z) for x, y, z in zip(self.x, self.y, self.z)
]
channel = ChannelFloat32()
channel.name = 'time_creation'
if self._time_creation is None:
return None
channel.values = self._time_creation.tolist()
pc.channels.append(channel)
return pc
def publish_variables(self, event):
t = rospy.get_time()
for var in self._variables:
pc_msg = PointCloud()
pc_msg.header.stamp = rospy.Time.now()
pc_msg.header.frame_id = 'world'
for name in self._vehicle_pos:
value = self._interpolate(var, self._vehicle_pos[name][0],
self._vehicle_pos[name][1],
self._vehicle_pos[name][2], t)
# Updating the point cloud for this variable
pc_msg.points.append(
Point32(self._vehicle_pos[name][0],
self._vehicle_pos[name][1],
self._vehicle_pos[name][2]))
pc_msg.channels.append(ChannelFloat32())
pc_msg.channels[-1].name = 'intensity'
pc_msg.channels[-1].values.append(value)
# Create the message objects
if 'temperature' in var.lower():
msg = Temperature()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/base_link' % name
# TODO Read from the unit of temperature from NC file
# Converting to Celsius
msg.temperature = value - 273.15
else:
msg = FloatStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/base_link' % name
msg.data = value
self._topics[name][var].publish(msg)
self._pc_variables[var].publish(pc_msg)
return True
def create_point_cloud(self, sortedRewards, min_len):
c = PointCloud()
c.header.seq = 1
c.header.stamp = rospy.Time.now()
c.header.frame_id = '/map'
c.points = []
channel = ChannelFloat32()
channel.name = "Values"
channel.values = []
c.channels = [channel]
for element in sortedRewards[0:min_len]:
p = Point()
x = self.map_msg.info.origin.orientation.x
y = self.map_msg.info.origin.orientation.y
z = self.map_msg.info.origin.orientation.z
w = self.map_msg.info.origin.orientation.w
roll, pitch, yaw = euler_from_quaternion((x, y, z, w))
offset_x = self.map_msg.info.origin.position.x
offset_y = self.map_msg.info.origin.position.y
p.y = (element[0] * np.cos(yaw) + element[1] *
np.sin(yaw)) * self.map_msg.info.resolution + offset_y
p.x = (element[1] * np.cos(yaw) - element[0] *
np.sin(yaw)) * self.map_msg.info.resolution + offset_x
c.points.append(p)
channel.values.append(element[2])
return c
def create_point_cloud(self, xs, ys, vals):
c = PointCloud()
c.header.seq = 1
c.header.stamp = rospy.Time.now()
c.header.frame_id = '/map'
c.points = []
channel = ChannelFloat32()
channel.name = "Values"
channel.values = []
c.channels = [channel]
for i in range(len(xs)):
p = Point()
x = self.map_msg.info.origin.orientation.x
y = self.map_msg.info.origin.orientation.y
z = self.map_msg.info.origin.orientation.z
w = self.map_msg.info.origin.orientation.w
roll, pitch, yaw = euler_from_quaternion((x, y, z, w))
offset_x = self.map_msg.info.origin.position.x
offset_y = self.map_msg.info.origin.position.y
p.y = (xs[i] * np.cos(yaw) + ys[i] *
np.sin(yaw)) * self.map_msg.info.resolution + offset_y
p.x = (ys[i] * np.cos(yaw) - xs[i] *
np.sin(yaw)) * self.map_msg.info.resolution + offset_x
c.points.append(p)
channel.values.append(vals[i])
return c
def cb_shovel(msg):
global excav_x, excav_y, _pub
data = ChannelFloat32()
data.name = "Dist, Angle"
data.values = [np.sqrt((msg.x - excav_x)**2 + (msg.y - excav_y)**2), msg.theta]
# print("Data: {}".format(data))
if _pub:
_pub.publish(data)
def __init__(self, name):
self.name = name #name of node and npy file
self.pub = rospy.Publisher(name, PointCloud, queue_size=5)
self.seg_data = np.load('/home/sun/catkin_ws/src/img_jpg/data/' +
self.name + '.npy') #seg result npy
self.custom_point = PointCloud() # data to send
self.custom_point.header.frame_id = "camera_link"
self.channel = ChannelFloat32()
def publish(self):
data = map(float, self.intf.get_input())
for a in self.analog_ch:
data[a] = self.intf.get_analog(a) / float(1023)
ch = ChannelFloat32()
ch.name = "avr_measurements"
ch.values = data
self.pub_marker.publish(ch)
def __init__(self):
self._cv_bridge = CvBridge()
self._sub = rospy.Subscriber('/map',
mp,
self.callback,
queue_size=1000)
self._pub = rospy.Publisher('Semantic_Map', PointCloud, queue_size=1)
self._currentframe = Image()
self._framequeue = []
self._kp = PointCloud()
self.smp = PointCloud()
self._mpqueue = []
self._kpqueue = []
self._labelqueue = []
self.smp.channels = [ChannelFloat32(), ChannelFloat32()]
self.smp.channels[0].name = 'category'
self.smp.channels[1].name = 'rgb'
self.temp = 0
def emitPointCloud(points):
global cloud_pub
head = Header()
head.seq = 1
head.stamp = rospy.Time.now()
head.frame_id = "/map"
points = [ Point32( x, y, 0.0 ) for x, y in points ]
pc = PointCloud( head, points, [ ChannelFloat32("", []) for _ in range(len(points))])
cloud_pub.publish( pc )
def __init__(self):
rospy.init_node('radar2pcd_pa_node.py', anonymous=True)
rospy.Subscriber('radar_messages', radar_msg, self.callback)
self.pcpub = rospy.Publisher('radar_pcd', PointCloud, queue_size=10)
# PointCloud
self.out_Pc = PointCloud()
self.out_Pc.header = std_msgs.msg.Header()
self.out_Pc.header.stamp = rospy.Time.now()
self.out_Pc.header.frame_id = "radar"
num_targets = 48
# -- None here represent data type # this data type is point
self.out_Pc.points = [None] * num_targets
# -- channels must be ChannelFloat32()
self.out_Pc.channels = [ChannelFloat32(), ChannelFloat32(), \
ChannelFloat32(), ChannelFloat32(), ChannelFloat32(), \
ChannelFloat32(), ChannelFloat32(), ChannelFloat32(), \
ChannelFloat32(), ChannelFloat32()]
self.out_Pc.channels[0].values = [None] * num_targets
self.out_Pc.channels[1].values = [None] * num_targets
self.out_Pc.channels[2].values = [None] * num_targets
self.out_Pc.channels[3].values = [None] * num_targets
self.out_Pc.channels[4].values = [None] * num_targets
self.out_Pc.channels[5].values = [None] * num_targets
self.out_Pc.channels[6].values = [None] * num_targets
self.out_Pc.channels[7].values = [None] * num_targets
self.out_Pc.channels[8].values = [None] * num_targets
self.out_Pc.channels[9].values = [None] * num_targets
self.out_Pc.channels[0].name = 'ID'
self.out_Pc.channels[1].name = 'distance'
self.out_Pc.channels[2].name = 'velocity'
self.out_Pc.channels[3].name = 'power'
self.out_Pc.channels[4].name = 'angle'
self.out_Pc.channels[5].name = 'distance_deviation'
self.out_Pc.channels[6].name = 'angle_deviation'
self.out_Pc.channels[7].name = 'velocity_deviation'
self.out_Pc.channels[8].name = 'proability_target'
self.out_Pc.channels[9].name = 'is_target'
def tensor_3d_to_pointcloud(self, ndarray, axis0_size_m, frame="/map_ned"):
msg = PointCloud()
msg.header.frame_id = frame
msg.header.stamp = rospy.get_rostime()
# Shift and scale to 0-1 range
map_data_np = ndarray[:, :, :, :3]
map_data_np = map_data_np - np.min(map_data_np)
map_data_np /= (np.max(map_data_np) + 1e-9)
c = np.linspace(0, axis0_size_m, map_data_np.shape[1])
cgrid = np.asarray(np.meshgrid(c, c, c))
r = ChannelFloat32()
r.name = "r"
g = ChannelFloat32()
g.name = "g"
b = ChannelFloat32()
b.name = "b"
msg.channels.append(r)
msg.channels.append(g)
msg.channels.append(b)
for x in range(map_data_np.shape[0]):
for y in range(map_data_np.shape[1]):
for z in range(map_data_np.shape[2]):
col = map_data_np[x, y, z, :]
# TODO: Clear up this hack
if np.linalg.norm(col) < 0.1:
continue
# Add multiple overlapping points to increase alpha value
intensity = int((np.linalg.norm(col)) * 10)
for i in range(intensity):
p = Point32()
p.x = cgrid[0, x, y, z]
p.y = cgrid[1, x, y, z]
p.z = cgrid[2, x, y, z]
r.values.append(min(float(col[0]) * 10, 1.0))
g.values.append(min(float(col[1]) * 10, 1.0))
b.values.append(min(float(col[2]) * 10, 1.0))
msg.points.append(p)
print(f"Generated pointcloud with {len(msg.points)} points")
return msg
def GetLocalPC(self, data):
pc2_msg = self.lp.projectLaser(data)
point_generator = pc2.read_points(pc2_msg)
pc_msg = PointCloud()
pc_msg.header = pc2_msg.header
pc_msg.channels.append(ChannelFloat32())
for p in point_generator:
pc_msg.points.append(Point32(p[0], p[1], p[2]))
pc_msg.channels[0].values.append(0)
return pc_msg
def PointCloud_Gen(points, frameID='rslidar'):
from sensor_msgs.msg import PointCloud, ChannelFloat32
from geometry_msgs.msg import Point32
import numpy as np
##=========PointCloud===============
points = np.array(points, np.float32)
point_cloud = points.reshape((-1, 3))
pointx = point_cloud[:, 0].flatten()
pointy = point_cloud[:, 1].flatten()
pointz = point_cloud[:, 2].flatten() * -1.0
# intensity = point_cloud[:, 3].flatten()
intensity = np.ones(pointx.shape, dtype=np.float32)
# labels = point_cloud[:,6].flatten()
seg_point = PointCloud()
seg_point.header.frame_id = frameID
channels1 = ChannelFloat32()
seg_point.channels.append(channels1)
seg_point.channels[0].name = "rgb"
channels2 = ChannelFloat32()
seg_point.channels.append(channels2)
seg_point.channels[1].name = "intensity"
for i in range(point_cloud.shape[0]):
seg_point.channels[1].values.append(intensity[i])
if True: # labels[i] == 1:
seg_point.channels[0].values.append(255)
geo_point = Point32(pointx[i], pointy[i], pointz[i])
seg_point.points.append(geo_point)
# else:
# seg_point.channels[0].values.append(255255255)
# geo_point = Point32(pointx[i], pointy[i], pointz[i])
# seg_point.points.append(geo_point)
# elif result[i] == 2:
# seg_point.channels[0].values.append(255255255)
# geo_point = Point32(pointx[i], pointy[i], pointz[i])
# seg_point.points.append(geo_point)
# elif result[i] == 3:
# seg_point.channels[0].values.append(255000)
# geo_point = Point32(pointx[i], pointy[i], pointz[i])
# seg_point.points.append(geo_point)
return seg_point
def sub_track(track, begin, end):
track2 = TrackMsg()
track2.header = track.header
track2.id = track.id
track2.pose = track.pose[begin:end]
for channel in track.channels:
ch = ChannelFloat32()
ch.name = channel.name
ch.values = channel.values[begin:end]
track2.channels.append(ch)
return track2
def ppl_cb(self, msg):
self.p = PointCloud()
self.p.header.frame_id = 'map'
self.p.header.stamp = rospy.get_rostime()
chan = ChannelFloat32()
chan.name = "intensity"
for n, i in enumerate(msg.persons):
self.p.points.append(i.pose.position)
self.p.points[n].z = 0.0
chan.values.append(100)
self.p.channels.append(chan)
def readtrack(filename):
rospy.loginfo("Opening logfile '%s'", filename)
msg = TrackMsg()
channel_w = None
channel_h = None
for line in fileinput.input(filename):
line = line.strip()
# rospy.loginfo("Reading line: '%s'",line)
data = line.strip().split()
data_float = [float(column) for column in data]
if len(data) < 3:
continue
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "/"
msg.id = 0
pose = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
if len(data) >= 3:
pose.position.x = data_float[0]
pose.position.y = data_float[1]
pose.position.z = data_float[2]
if len(data) >= 7:
pose.orientation.x = data_float[3]
pose.orientation.y = data_float[4]
pose.orientation.z = data_float[5]
pose.orientation.w = data_float[6]
if len(data) >= 9:
if channel_w is None or channel_h is None:
channel_w = ChannelFloat32('width', [])
channel_h = ChannelFloat32('height', [])
msg.channels.append(channel_w)
msg.channels.append(channel_h)
channel_w.values.append(data_float[7])
channel_h.values.append(data_float[8])
msg.pose.append(pose)
return msg
def publish_poops_to_cost_map(self, poops):
"""Always publishes 30 poops. Keeps unused ones at map position 25, 25."""
NUM_POOPS = 30
poops_with_z = [(p[0], p[1], 0) for p in poops]
unused_poops = [(25, 25, 25)] * (NUM_POOPS - len(poops))
poop_positions = poops_with_z + unused_poops
#print poop_positions
point_cloud = PointCloud()
# cost map doesn't like the map coordinate frame
point_cloud.header = Header(stamp=Time.now(), frame_id='odom_combined')
point_cloud.points = [self._map_to_odom_combined(pos)
for pos in poop_positions]
#print 'new points', point_cloud.points
point_cloud.channels = [
ChannelFloat32(name='intensities', values=[2000] * NUM_POOPS),
ChannelFloat32(name='index', values=[0] * NUM_POOPS), #values=range(NUM_POOPS)),
ChannelFloat32(name='distances', values=[2] * NUM_POOPS),
ChannelFloat32(name='stamps', values=[0.002] * NUM_POOPS),
]
self.publisher.publish(point_cloud)
def get_lidar_pcd(data):
lidar_pcd = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'lidar'
lidar_pcd.header = header
channel1 = ChannelFloat32()
channel1.name = "intensity"
channel2 = ChannelFloat32()
channel2.name = "ring"
for idx in range(data.shape[0]):
lidar_pcd.points.append(
Point32(data[idx, 0], data[idx, 1], data[idx, 2]))
channel1.values.append(data[idx, 3])
channel2.values.append(data[idx, 4])
lidar_pcd.channels.append(channel1)
lidar_pcd.channels.append(channel2)
return lidar_pcd
def getMsg_dynamic(lidar_data):
gen = point_cloud2.read_points(lidar_data, skip_nans=True)
points_list = []
for p in gen:
if p[4] == 7:
points_list.append([p[0], p[1], p[2], p[3]])
pcl_data = pcl.PointCloud_PointXYZRGB()
pcl_data.from_list(points_list)
# downsampling 실행 코드 부분
print("Input :", pcl_data)
LEAF_SIZE = 0.1
cloud = do_voxel_grid_downssampling(pcl_data, LEAF_SIZE)
print("Output :", cloud)
# ROI 실행 코드 부분
filter_axis = 'x'
axis_min = 0.1
axis_max = 7.0
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
filter_axis = 'y'
axis_min = -4.0
axis_max = 4.0
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
filter_axis = 'z'
axis_min = 0.0
axis_max = 2.0
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
test = PointCloud()
get_in = ChannelFloat32()
get_in.name = 'VLP_intensity'
test.points = []
for p in cloud:
# if p[1] > 0:
park = Point32()
park.x = p[0]
park.y = p[1]
park.z = 0
get_in.values.append(p[3])
test.points.append(park)
#print("Input :", cnt)
test.channels.append(get_in)
test.header.frame_id = 'world'
test.header.stamp = rospy.Time.now()
pub.publish(test)
def callback(self, path):
rospy.loginfo("Republishing Path as PointCloud...")
self.cloud = PointCloud()
self.cloud.header.frame_id = "rtab_init"
self.cloud.points = [x.pose.position for x in path.poses]
self.cloud.channels = [ChannelFloat32()]
self.cloud.channels[0].name = "intensity"
self.cloud.channels[0].values = [1.0 for x in path.poses]
self.pub.publish(self.cloud)
