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 periodic(self):
clouds = self.get_clouds()
points = clouds[0].points + clouds[1].points
frame_id = clouds[0].header.frame_id
def norm(p):
return np.hypot(np.hypot(p.x, p.y), p.z)
p_safe = filter(lambda p: norm(p) > self.warn_distance, points)
p_unsafe = filter(lambda p: (norm(p) > self.ignore_closer) and
(norm(p) < self.warn_distance),
points)
if self.publish_clouds:
pc_safe = PointCloud()
pc_safe.header.frame_id = frame_id
pc_safe.points = p_safe
self.pub_safe.publish(pc_safe)
pc_unsafe = PointCloud()
pc_unsafe.header.frame_id = frame_id
pc_unsafe.points = p_unsafe
self.pub_unsafe.publish(pc_unsafe)
if len(p_unsafe) > 0:
# find the closest unsafe point
norms = map(norm, p_unsafe)
closest = np.argmin(norms)
closest_dist = norms[closest]
assert closest_dist > self.ignore_closer
assert closest_dist < self.warn_distance
p0 = p_unsafe[closest]
angle = np.rad2deg(np.arctan2(p0.y, p0.x))
desc = ('Point at distance %.2fm angle %.1fdeg (x:%.2f, y:%.2f) ign: %s warn: %s'
% (norms[closest], angle, p0.x, p0.y, self.ignore_closer, self.warn_distance))
plane=[p0.x, p0.y]
constraints = [constraint_plane(desc=desc, plane=plane)]
else:
constraints = []
msg_safety = Safety()
msg_safety.id_check = 'hokuyo_safety'
msg_safety.header.stamp = rospy.Time.now()
msg_safety.constraints = yaml.dump(constraints)
if p_unsafe:
msg_safety.safe = False
msg_safety.desc = '%d unsafe points' % len(p_unsafe)
else:
msg_safety.safe = True
msg_safety.desc = 'OK'
self.pub_safety.publish(msg_safety)
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 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 sonarLaserCallback(data):
global collision
currSpeed = rosAriaPose.twist.twist
if not collision and (abs(currSpeed.linear.x) >= 0.02):
pose = amclPose.pose.pose
points = polar_to_euclidean([
data.angle_min + (i * data.angle_increment)
for i in range(len(data.ranges))
], data.ranges)
pc = PointCloud()
pc.header.frame_id = "map"
pc.points = [0] * len(points)
counter = 0
for point in points:
tfq = transformations.quaternion_from_euler(0, 0, 3.14)
q = Quaternion(tfq[0], tfq[1], tfq[2], tfq[3])
p = Point32(point[0], point[1], 0)
rotatePose(p, q)
p.x = sonarOffset + p.x
rotatePose(p, pose.orientation)
p.x = pose.position.x + p.x
p.y = pose.position.y + p.y
pc.points[counter] = p
counter = counter + 1
if not collision:
evaluateReadings(pc)
def lidarCallback(data):
global angle
global ekf
global p_acc
p = PointCloud()
p.points = []
p.header = data.header
p.header.frame_id = "map"
p_acc.header.stamp = data.header.stamp
p_acc.header.seq = data.header.seq
current_angle = data.angle_min
pitch = angle.data + ekf.pitch
roll = ekf.roll
for i in data.ranges:
if i < MAX_DISTANCE:
point = Point32()
yaw = current_angle+ekf.yaw
point.x = i * math.cos(yaw) * math.cos(pitch)
point.y = i * math.sin(yaw) * math.cos(pitch)
current_angle += data.angle_increment
point.z = i * math.sin(pitch)
p.points += [point]
p_acc.points += p.points
pub.publish(p)
pub_acc.publish(p_acc)
def callback(self, data):
#rospy.loginfo(rospy.get_caller_id() + "-Received %s,%s,%s,%s", data.num1, data.num2, data.num3, data.num4)
#convert data uints into 3D points
points = PointCloud()
points.header = Header()
points.header.frame_id = 'map'
point1 = Point()
point2 = Point()
point3 = Point()
point4 = Point()
point1.x = self.sensor1_x_from_origin
point1.y = self.sensor1_y_from_origin
point1.z = (self.sensor1_z_from_origin - data.num1)
point2.x = self.sensor2_x_from_origin
point2.y = self.sensor2_y_from_origin
point2.z = self.sensor2_z_from_origin - data.num2
point3.x = self.sensor3_x_from_origin
point3.y = self.sensor3_y_from_origin
point3.z = self.sensor3_z_from_origin - data.num3
point4.x = self.sensor4_x_from_origin
point4.y = self.sensor4_y_from_origin
point4.z = self.sensor4_z_from_origin - data.num4
point_list = [point1, point2, point3, point4]
points.points = point_list
self.sonar_pts.publish(points)
def convert_pointcloud_to_ROS(pts3d, intensities = None, labels = None, colors=None, bgr=True):
ROS_pointcloud = PointCloud()
ROS_pointcloud.points = []
ROS_pointcloud.channels = []
(x,y,z) = (0,1,2)
for pt in np.asarray(pts3d.T):
ROS_pointcloud.points += [Point32(pt[x], pt[y], pt[z])]
intensity_channel = ChannelFloat32(name = 'intensities')
if intensities != None:
for value in intensities:
intensity_channel.values += [value]
ROS_pointcloud.channels += [intensity_channel]
label_channel = ChannelFloat32(name = 'labels')
if labels != None:
for value in labels:
label_channel.values += [value]
ROS_pointcloud.channels += [label_channel]
if colors != None:
r_ch = ChannelFloat32(name='r')
g_ch = ChannelFloat32(name='g')
b_ch = ChannelFloat32(name='b')
rgb_ch = ChannelFloat32(name='rgb')
for (r,g,b) in np.asarray(colors.T):
#NOTE: CV image is in bgr order. Ros expects rgb order
r_ch.values += [b/256]
g_ch.values += [g/256]
b_ch.values += [r/256]
rgb_color = int(b*0xFF*0xFF+ g*0xFF+r) #incorrect
rgb_ch.values += [rgb_color]
ROS_pointcloud.channels += [r_ch, g_ch, b_ch]
return ROS_pointcloud
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 getmapMsg(self, msg):
# if self.first_flag is True:
# self.map = msg
temp = point_cloud2.read_points(msg, skip_nans=True)
path_list = PointCloud()
path_list.points = []
for p in temp:
get_point = Point32()
get_point.x = p[2]
get_point.y = p[0]
get_point.z = p[1]
path_list.points.append(get_point)
print(len(path_list.points))
print(path_list.points[-1])
# print(self.cur_position)
if len(path_list.points) > 10:
for i in range(0, len(path_list.points) - 10):
# print(i)
if self.calc_distance(
path_list.points[-1],
path_list.points[i]) <= 1 and self.first_do is False:
self.first_do = True
self.map.header.frame_id = 'map'
self.map.header.stamp = rospy.Time.now()
self.map.points = path_list.points[i:]
continue
self.pub_map.publish(self.map)
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.config["step"]
x_unit = math.cos(self.heading) * r_step
y_unit = math.sin(self.heading) * r_step
cloud.points = [
Point32(x=self.bins[r] * math.cos(r * self.step),
y=self.bins[r] * math.sin(r * self.step),
z=0.00) for r in range(0, nbins)
]
return cloud
def setup_cloud():
cloud = PointCloud()
cloud.header.stamp = rospy.Time.now()
cloud.header.frame_id = "odom"
# Add points to cloud.
points = []
for shelf in [0, 1, 2, 3]:
# Shelf 1
add_point(points, 2, 0, shelf)
# Shelf 2
add_point(points, 2, -1.5, shelf)
# Shelf 3
add_point(points, 2, -3, shelf)
# Shelf 4
add_point(points, 1, -4, shelf)
# Shelf 5
add_point(points, -0.5, -4, shelf)
# Shelf 6
add_point(points, -2, -4, shelf)
# Shelf 7
add_point(points, -3, -3, shelf)
# Shelf 8
add_point(points, -2, -1, shelf)
cloud.points = points
return cloud
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 get_data(self, var):
PC = PointCloud(
) # create the new object PointCloud to be published later
PC.header = var.header # copy the content of topic to another PointCloud in order to manipulate it
PC.channels = var.channels
PC.points = var.points
"""
Function to get the "3D" map
"""
for i in range((len(PC.points))):
PC.points[i].z = PC.channels[0].values[
i] / 100 # write in the z axis of the points the values of the "intensity" divided by 100 to make it readable on rviz
self.pub2.publish(PC) # publish it into the new topic
"""
Function to filtter the map and get only one point by degree
"""
index_max = PC.channels[0].values.index(max(PC.channels[0].values))
for i in range(len(PC.points)): #396
if i != index_max:
PC.points[i].x = PC.points[i].y = PC.points[i].z = 0
else:
PC.points[i].z = 0
self.pub1.publish(PC) # publish it into the new topic
def broadcaster_particles(self):
# Part for export point clound
message = PointCloud()
message.header.stamp = rospy.get_rostime()
message.header.frame_id = self.parent_frame
message.channels = []
message.channels.append(ChannelFloat32())
message.channels[0].name = "intensities"
if self.direct_to_parent:
message.points = array_to_point32(self.filter.particles)
else:
message.points = array_to_point32(self.filter.particles +
self.state)
message.channels[0].values = tuple(self.filter.weights)
self.pub_particle.publish(message)
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 getFrontiers(self):
if self.gmap is not None:
self.lookingForFrontiers = True
rospy.loginfo("[frontier_search] Looking for frontiers")
#First check to see if the cells are free and if we already decided they are not a frontier
cells = [x for x in range(0, len(self.gmap.data)) if self.gmap.data[x] == 0]
# and x not in self.notFrontier removed from previous line waay too slow!
#The check to see if the remaining are frontiers
frontiers = [cell for cell in cells if -1 in [self.gmap.data[val] for val in self.getNeighbors(cell)]]
#Add all cells that are not frontiers to our list of not frontiers waay too slow!
#self.notFrontier.extend([cell for cell in cells if cell not in frontiers])
#Check to see if the area around them is clear of obstacles
freeFrontiers = [frontier for frontier in frontiers if self.validFrontier(frontier)]
rospy.loginfo("[frontier_search] Found %s frontiers", len(freeFrontiers))
points = self.getPoints(freeFrontiers)
cloud = PointCloud()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'map'
cloud.header = h
cloud.points = points
rospy.loginfo("[frontier_search] Publishing frontiers")
# Publish a point cloud for your viewing pleasure
self.publishFrontiers.publish(cloud)
self.lookingForFrontiers = False
return cloud
else:
rospy.loginfo("[frontier_search] No map available")
return None
def publish(self, yellow_points, white_points, target_path, obstacle_path):
lane_line_yellow_points = PointCloud()
lane_line_yellow_points.header.frame_id = self.frame_id
lane_line_yellow_points.header.stamp = rospy.Time(0)
lane_line_yellow_points.points = yellow_points
self.lane_line_yellow_points_pub.publish(lane_line_yellow_points)
lane_line_white_points = PointCloud()
lane_line_white_points.header.frame_id = self.frame_id
lane_line_white_points.header.stamp = rospy.Time(0)
lane_line_white_points.points = white_points
self.lane_line_white_points_pub.publish(lane_line_white_points)
self.path_pub.publish(target_path)
self.obstacle_path_pub.publish(obstacle_path)
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 detected_robots_callback(self, data):
robots = np.array([[robot.x, robot.y] for robot in data.points])
if robots.shape[0] != 0:
# exclude our robots
ind = np.where(
np.logical_and(
np.linalg.norm(robots - self.coords_main[:2],
axis=1) > self.SEPARATE_ROBOT_DISTANCE,
np.linalg.norm(robots - self.coords_secondary[:2], axis=1)
> self.SEPARATE_ROBOT_DISTANCE))
robots = robots[ind]
# exclude objects outside the field (beacons, ets.)
ind = np.where(
np.logical_and(
np.logical_and(
robots[:, 0] > self.WALL_DIST_X, robots[:, 0] <
self.size_in_meters[0] - self.WALL_DIST_X),
np.logical_and(robots[:, 1] > 0,
robots[:, 1] < self.size_in_meters[1])))
robots = robots[ind]
self.robots = robots
self.robots_upd_time = data.header.stamp
# pub opponent robots
array = PointCloud()
array.header.frame_id = "map"
array.header.stamp = rospy.Time.now()
array.points = [Point(x=robot[0], y=robot[1]) for robot in robots]
self.pub_opponent_robots.publish(array)
def points3d_to_pcl(self, points3d):
pcl = PointCloud()
#transforms the points3d list into a pointcloud
pcl.header = self.recognition_header
pcl.points = points3d
return pcl
def points3d_to_pcl(self, points3d):
pcl = PointCloud()
#transforms the points3d list into a pointcloud
pcl.header = self.recognition_header
pcl.points = points3d
return pcl
def PointCloud2_to_PointCloud(pc):
assert isinstance(pc, PointCloud2)
xyz = pc2xyz(pc)
msg = PointCloud()
msg.header = pc.header
msg.points = [Point32(x,y,z) for (x,y,z) in xyz[0,:,:]]
return msg
def __publish_point_cloud_data(self, stamp):
points = self._wb_device.getPointCloud()
if points:
msg = PointCloud()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.points = [Point32(x=point.x, y=point.y, z=point.z) for point in points]
self.__publisher.publish(msg)
def handle_contact_points(contact_points):
global br
t = PointCloud()
t.header.stamp = rospy.Time.now()
t.header.frame_id = rospy.get_param(
'/extract_point_cloud_of_contacts/world_frame', "world")
t.points = contact_points
br.publish(t)
def transform_polygon(self, polygon, frame_target):
""" Use TF Listener to transform the points in a
PolygonStamped into a different TF frame...via a PointCloud
msg, which is the output data type. """
cloud = PointCloud()
cloud.header = polygon.header
cloud.points = polygon.polygon.points
self.listener.waitForTransform(frame_target, cloud.header.frame_id, cloud.header.stamp, rospy.Duration(1.0))
return self.listener.transformPointCloud(frame_target, cloud)
def publish_weeds(self):
"""
Publish the weeds ont the map.
"""
cloud = PointCloud()
cloud.header.stamp = rospy.Time()
cloud.header.frame_id = "map"
cloud.points = self.weed_map
self.weedmap_pub.publish(cloud) #Publish the weeds
def publish(self):
polygon_msg = PolygonStamped()
pointcloud_msg = PointCloud()
pointcloud_msg.header.frame_id = self.publish_frame
pointcloud_msg.header.stamp = rospy.Time.now()
pointcloud_msg.points = self.corner_points_transformed
self.corner_points_pub.publish(pointcloud_msg)
polygon_msg.header = pointcloud_msg.header
polygon_msg.polygon.points = self.corner_points_transformed
self.foot_state_msg.foot_state.polygon.points = self.corner_points_transformed
self.polygon_pub.publish(polygon_msg)
self.foot_state_pub.publish(self.foot_state_msg)
def publish_tracking(points, odom_tracker_key):
msg = PointCloud()
msg.points = []
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'odom'
for p in points:
msg.points.append(Point(p.x, p.y, 0))
odom_out_publishers[odom_tracker_key].publish(msg)
def _callback(self, data):
self.transformer.waitForTransform('odom_combined', 'map', Time.now(),
rospy.Duration(2))
new_data = PointCloud()
new_data.header.stamp = Time.now()
new_data.header.frame_id = 'odom_combined'
new_data.points = [
self._map_to_odom_combined(p, data.header.stamp)
for p in data.points
]
new_data.channels = data.channels
self.publisher.publish(new_data)
def publish_point_cloud_messages(data_publishers, measured_distances):
point_32_msg = Point32()
point_cloud_msg = PointCloud()
for index, measured_distance in enumerate(measured_distances):
point_32_msg.x = measured_distance
point_cloud_msg.points = [point_32_msg]
point_cloud_msg.header.frame_id = range_sensors_frame_ids[index]
point_cloud_msg.header.stamp = rospy.Time.now()
data_publishers[index].publish(point_cloud_msg)
def GetUnifiedPointcloud(self, pointcloud_list):
pointcloudUnified = PointCloud()
pointcloudUnified.points = []
pointcloudUnified.channels = []
if len(pointcloud_list) > 0:
pointcloudUnified.header = pointcloud_list[0].header # They all have different headers, but just use the first one.
for pointcloud in pointcloud_list:
pointcloudUnified.points.extend(pointcloud.points)
pointcloudUnified.channels.extend(pointcloud.channels)
return pointcloudUnified
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 pubsub():
poindcloud_pub = rospy.Publisher('weed_pointcloud', PointCloud, queue_size=5)
rospy.init_node('pointcloud', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
ptcloud = PointCloud()
time = rospy.Time(0)
ptcloud.header.stamp = time
ptcloud.header.frame_id = 'map'
ptcloud.points = pts
rospy.loginfo(ptcloud)
poindcloud_pub.publish(ptcloud)
rate.sleep()
def lidarinfo(data):
p = PointCloud()
p.points = []
p.header = data.header
current_angle = data.angle_min
for i in data.ranges:
point = Point32()
point.x = i * math.cos(current_angle)
point.y = i * math.sin(current_angle)
current_angle += data.angle_increment
point.z = 0
p.points += [point]
pub.publish(p)
def publish_frontiers(self):
point_cloud = PointCloud()
point_cloud.header = std_msgs.msg.Header()
point_cloud.header.stamp = rospy.Time.now()
point_cloud.header.frame_id = '/orb_slam/map'
off_x, off_y = self.grid.info.origin.position.x, self.grid.info.origin.position.y
point_cloud.points = [Point32(
(x+.5) * self.grid.info.resolution + off_x,
(y+.5) * self.grid.info.resolution + off_y, 0.0
) for (y, x) in self.frontiers
]
publisher = OneTimePublisher("exploration_frontiers", PointCloud, point_cloud)
publisher.start()
def determinarDerivada(scan):
derivada = PointCloud()
derivada.header = scan.header
derivada.points = [Point(0,0,0)]*(len(scan.points)-1)
for i in range(0,len(derivada.points)):
ponto_x = scan.points[i].x
ponto_y = 0
if math.fabs(scan.points[i+1].x-scan.points[i].x) > 0.000001:
ponto_z = (scan.points[i+1].z-scan.points[i].z)/(scan.points[i+1].x-scan.points[i].x)
else:
ponto_z = 0
derivada.points[i] = Point(ponto_x,ponto_y,ponto_z)
return derivada
def GenerateIdealDock(points_count):
global ideal_dock_pointcloud
# rospy.loginfo("GenerateIdealDock with %d points" % points_count)
base_length = 0.2475 # 247.50mm
hypotenuse_length = 0.1985 # 198.50mm
base_angle = 142.5 / 180.0 * math.pi # 142.5 degrees
base_points_count = points_count / 4 * 2 + points_count % 4
hypotenuse_points_count = points_count / 4
base_step_delta = base_length / base_points_count
hypotenuse_delta_x = math.sin(
base_angle) * hypotenuse_length / hypotenuse_points_count
hypotenuse_delta_y = math.cos(
base_angle) * hypotenuse_length / hypotenuse_points_count
ideal_dock_cloud = []
# right hypotenuse points
for i in range(hypotenuse_points_count):
point = Point32()
point.x = hypotenuse_delta_x * (hypotenuse_points_count - i) * (-1)
point.y = -(base_length /
2.0) + hypotenuse_delta_y * (hypotenuse_points_count - i)
point.z = 0.0 # i * 0.01
ideal_dock_cloud.append(point)
# base points
for i in range(base_points_count):
point = Point32(0.0, base_length / 2.0 - i * base_step_delta, 0.0)
ideal_dock_cloud.append(point)
# left hypotenuse points
for i in range(hypotenuse_points_count):
point = Point32()
point.x = hypotenuse_delta_x * (hypotenuse_points_count - i) * (-1)
point.y = (base_length /
2.0) - hypotenuse_delta_y * (hypotenuse_points_count - i)
point.z = 0.0 # i * 0.01
ideal_dock_cloud.append(point)
# ideal dock pointcloud
ideal_dock_pointcloud = PointCloud()
ideal_dock_pointcloud.header.frame_id = "dock"
ideal_dock_pointcloud.header.stamp = rospy.Time.now()
ideal_dock_pointcloud.points = ideal_dock_cloud
return ideal_dock_cloud
def get_data(self, var):
PC = PointCloud(
) # create the new object PointCloud to be published later
PC.header = var.header # copy the content of topic to another PointCloud in order to manipulate it
PC.channels = var.channels
PC.points = var.points
for i in range((len(PC.points))):
PC.points[i].z = PC.channels[0].values[
i] / 100 # write in the z axis of the points the values of the "intensity" divided by 100 to make it readable on rviz
self.pub.publish(PC) # publish it into the new topic
def GetUnifiedPointcloud(self, pointcloud_list):
pointcloudUnified = PointCloud()
pointcloudUnified.points = []
pointcloudUnified.channels = []
if len(pointcloud_list) > 0:
pointcloudUnified.header = pointcloud_list[0].header # They all have different headers, but just use the first one.
pointcloudUnified.channels.append(ChannelFloat32(name='intensity', values=[]))
for pointcloud in pointcloud_list:
pointcloudUnified.points.extend(pointcloud.points)
pointcloudUnified.channels[0].values.extend(pointcloud.channels[0].values)
return pointcloudUnified
def np_points_to_ros(pts):
p_list = []
chlist = []
# p_list = [Point32(p[0,0], p[0,1], p[0,2]) for p in pts.T]
# chlist = np.zeros(pts.shape[1]).tolist()
for p in pts.T:
p_list.append(Point32(p[0,0],p[0,1],p[0,2]))
chlist.append(0.)
ch = ChannelFloat32('t',chlist)
pc = PointCloud()
pc.points = p_list
pc.channels = [ch]
return pc
def gmm_cloud_publish(self, data):
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = "map"
pcl = PointCloud()
pcl.header = h
pcl.points = []
pcl.channels = []
for i in range(0, data.shape[1]):
point = Point32()
point.x = data[0,i]
point.y = data[1,i]
point.z = data[2,i]
pcl.points.append(point)
self.gmmPub.publish(pcl)
def transformLines(self, lines, stamp):
points = []
for l in lines:
p1 = Point32()
p1.x = -((self.imgWidth / 2) - l[0]) / self.fx
p1.y = -((self.imgHeight / 2) - l[1]) / self.fy
p1.z = 1
points.append(p1)
p1 = Point32()
p1.x = -((self.imgWidth / 2) - l[2]) / self.fx
p1.y = -((self.imgHeight / 2) - l[3]) / self.fy
p1.z = 1
points.append(p1)
pcl = PointCloud()
pcl.points = points
pcl.header.stamp = stamp
pcl.header.frame_id = self.camFrame
self.tfListener.waitForTransform(self.camFrame, self.floorFrame, stamp,Duration(5.0) )
transfPcl = self.tfListener.transformPointCloud(self.floorFrame,pcl)
self.orig.header.stamp = stamp
origWorld = self.tfListener.transformPoint(self.floorFrame,self.orig).point
groundPoints = []
for z1World in transfPcl.points:
# print "z1World", z1World
paramLambda = origWorld.z / (origWorld.z - z1World.z)
pos = Point()
pos.x = -self.scale * (origWorld.x + paramLambda * (origWorld.x - z1World.x))
pos.y = -self.scale * (origWorld.y + paramLambda * (origWorld.y - z1World.y))
pos.z = 0
groundPoints.append(pos)
tLines = []
for i in range(0,len(groundPoints)/2, 2):
tLines.append([groundPoints[i],groundPoints[i+1]])
return tLines
def image_callback(self,image):
try: # if there is an image
# Acquire the image, and convert to single channel gray image
curr_image = self.bridge.imgmsg_to_cv2(image, "mono8")
if len(curr_image.shape) > 2:
if curr_image.shape[2] > 1: # color image, convert it to gray
curr_image = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # shape should now be (rows, columns)
elif curr_image.shape[2] == 1: # mono image, with wrong formatting
curr_image = curr_image[:,:,0] # shape should now be (rows, columns)
if self.prev_image is None:
self.prev_image = curr_image
return
# optional: resize the image
# curr_image = cv2.resize(curr_image, (0,0), fx=0.8, fy=0.8)
# Get time stamp
secs = image.header.stamp.secs
nsecs = image.header.stamp.nsecs
curr_time = float(secs) + float(nsecs)*1e-9
#########################################################################
# Image processing stuff happens here. Image is acccessed at curr_image
# Should end up with a list of 3D points, e.g.
# [[x1,y1,0], [x2,y2,0], [x3,y3,0]]
tracked_points = get_random_tracked_points() # for example - these should be based on the actual tracked points in the image
#########################################################################
# draw the image, and tracking points
draw_optic_flow_field(curr_image, tracked_points)
# publish optic flow data to rostopic
msg = PointCloud()
msg.header.stamp.secs = secs
msg.header.stamp.nsecs = nsecs
msg.points = [Point( tracked_point[0], tracked_point[1], tracked_point[2] ) for tracked_point in tracked_points]
self.tracked_points_pub.publish(msg)
self.prev_image = curr_image
except CvBridgeError, e:
print e
#!/usr/bin/env python
import roslib; roslib.load_manifest('lidar_proc')
import rospy
import math
from std_msgs.msg import Float64
from sensor_msgs.msg import LaserScan, PointCloud
from geometry_msgs.msg import Point32
from filters.msg import EKFData
pub = rospy.Publisher('point_cloud_raw', PointCloud)
pub_acc = rospy.Publisher('point_cloud_raw_acc', PointCloud)
angle = Float64()
ekf = EKFData()
MAX_DISTANCE = 4
p_acc = PointCloud()
p_acc.points = []
p.header.frame_id = "map"
def angleCallback(data):
global angle
angle = data
def ekfCallback(data):
global ekf
ekf = data
def lidarCallback(data):
global angle
global ekf
global p_acc
p = PointCloud()
p.points = []
def bblos_main(address):
rospy.loginfo ('bblos_main')
"""Read scans from the platform."""
global getIntensities
global getMeasuredSpeed
global testPlatform
global timeout
global lastreqtime
global velreq
global omegareq
global pubodo
global publaser
# pre-initialise the constant part of the messages
odomsg = Odometry()
odomsg.header.frame_id = globalFrameName
odomsg.child_frame_id = "biba_base";
odomsg.pose.pose.position.z = 0
for i in range(0,36):
odomsg.pose.covariance[i] = 0
for i in range(0,36):
odomsg.twist.covariance[i] = 0
odomsg.twist.twist.linear.y = 0
odomsg.twist.twist.linear.z = 0
odomsg.twist.twist.angular.x = 0
odomsg.twist.twist.angular.y = 0
laser = PointCloud()
laser.header.frame_id = "laser"
laser.channels.append(ChannelFloat32())
laser.points = []
if getIntensities:
laser.channels[0].name = "intensities"
laser.channels[0].values = []
# Use a Rate object to try to have a constant sampling time (which LOS
# cannnot provide anyway)
rate = rospy.Rate(10)
proxy = Connection(address)
proxy.login("User", "none")
while not rospy.is_shutdown():
# First acquire all the data
tstamp = rospy.rostime.get_rostime()
try:
proxy.Watchdog.reset(3.0)
# We can de-activate intensities if requested
if getIntensities:
(tlaser, pose, maxAge, indices, coordinates, intensities) = proxy.Scan.get(Int32(gfCoordinates | gfSync | gfIntensities))
else:
(tlaser, pose, maxAge, indices, coordinates) = proxy.Scan.get(Int32(gfCoordinates | gfSync ))
#rospy.logdebug("info")
#rospy.logdebug(tlaser)
#rospy.logdebug(pose)
#rospy.logdebug(maxAge)
#rospy.logdebug(indices)
#rospy.logdebug("coordinates ")
#rospy.logdebug(coordinates)
#if getIntensities:
# rospy.logdebug(intensities)
(todo, lospose) = proxy.Odometry.getPose()
if getMeasuredSpeed:
# Activate this part if you really need the robot speed
speeds = proxy.Motion.getSpeed()
else:
speeds = [0, velreq, omegareq]
# Implement a timeout if the last requested control message is too
# old
if (rospy.rostime.get_time() - lastreqtime) > timeout:
velreq = 0.0
omerareq = 0.0
# And finally send the latest command
if testPlatform:
proxy.Motion.setSpeed(0.1*math.sin(rospy.rostime.get_time()), 0.1*math.cos(rospy.rostime.get_time()))
else:
proxy.Motion.setSpeed(velreq,omegareq)
# rospy.loginfo("Speed sent %f and %f" % (velreq,omegareq))
except SyntaxError:
traceback.print_exc()
print "Exception while accessing los proxy"
break
except:
print "Interrupted"
break
# Now convert the LOS type to ROS messages
n = len(coordinates)/2
laser.header.stamp = tstamp
if not (n == len(laser.points)):
laser.points = [Point32()]*n
if getIntensities:
laser.channels[0].values = [0]*n
for i in range(0,n):
laser.points[i] = Point32(coordinates[2*i+0], coordinates[2*i+1], 0)
#laser.points[i].x = Point32(coordinates[2*i+0], coordinates[2*i+1], 0)
#laser.points[i].y = Point32(coordinates[2*i+1])
#laser.points[i].z = 0
#rospy.logdebug("points: %f, %f", laser.points[i].x, laser.points[i].y)
if getIntensities:
laser.channels[0].values[i] = intensities[i]
publaser.publish(laser)
# Directly send requested values
odomsg.header.stamp = tstamp
odomsg.twist.twist.linear.x = speeds[1]
odomsg.twist.twist.angular.z = speeds[2]
odomsg.pose.pose.position.x = lospose[0];
odomsg.pose.pose.position.y = lospose[1];
odomsg.pose.pose.position.z = wheelRadius;
q = tf.transformations.quaternion_from_euler(0,0,lospose[2])
odomsg.pose.pose.orientation.x = q[0]
odomsg.pose.pose.orientation.y = q[1]
odomsg.pose.pose.orientation.z = q[2]
odomsg.pose.pose.orientation.w = q[3]
# Assuming the covariance is Cov[x y 0 . . theta]
odomsg.pose.covariance[6*0+0] = lospose[3]; # x * x
odomsg.pose.covariance[6*0+1] = lospose[6]; # x * y
odomsg.pose.covariance[6*1+0] = lospose[6]; # y * x
odomsg.pose.covariance[6*1+1] = lospose[4]; # y * y
odomsg.pose.covariance[6*0+5] = lospose[7]; # x * theta
odomsg.pose.covariance[6*5+0] = lospose[7]; # theta * x
odomsg.pose.covariance[6*1+5] = lospose[8]; # y * theta
odomsg.pose.covariance[6*5+1] = lospose[8]; # theta * y
odomsg.pose.covariance[6*5+5] = lospose[5]; # theta * theta
pubodo.publish(odomsg)
# Publish transformation frame
br = tf.TransformBroadcaster()
br.sendTransform(
(odomsg.pose.pose.position.x,
odomsg.pose.pose.position.y,
odomsg.pose.pose.position.z),
(odomsg.pose.pose.orientation.x,
odomsg.pose.pose.orientation.y,
odomsg.pose.pose.orientation.z,
odomsg.pose.pose.orientation.w),
rospy.Time.now(),
odomsg.child_frame_id,
globalFrameName
)
#rospy.loginfo("published laser and odo")
rate.sleep()
#!/usr/bin/env python
import rospy
import numpy as np
import tf
import mil_ros_tools
from sensor_msgs.msg import PointCloud
rospy.init_node("ground_finder")
pub = rospy.Publisher("ground_est", PointCloud, queue_size=1)
listener = tf.TransformListener()
pc = PointCloud()
pc.header = mil_ros_tools.make_header(frame="map")
pc.points = []
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
try:
t = listener.waitForTransform('/map', '/ground', rospy.Time.now(), rospy.Duration(1))
(trans, rot) = listener.lookupTransform('/map', '/ground', rospy.Time(0))
except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("TF waitForTransform timeout")
continue
pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans)))
pub.publish(pc)
rate.sleep()
def callback(data):
ma = MarkerArray()
pcl = PointCloud()
pcl.header.frame_id = "world"
pcl.header.stamp = rospy.Time()
pcl.points = static_map_borders # this is obviously a cludge, the static map should be generated elsewhere
for i, name in enumerate(data.name):
marker = Marker()
marker.header.frame_id = "world"
marker.id = 0
marker.ns = name
marker.header.stamp = rospy.Time()
marker.lifetime = rospy.Time(0.3)
marker.action = Marker.ADD
marker.pose = data.pose[i]
text_marker = Marker()
text_marker.header.frame_id = "world"
text_marker.id = 1
text_marker.ns = name
text_marker.header.stamp = rospy.Time()
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.lifetime = rospy.Time(0.3)
text_marker.action = Marker.ADD
text_marker.pose = data.pose[i]
text_marker.text = name
text_marker.scale.x = 0.1
text_marker.scale.y = 0.1
text_marker.scale.z = 0.05
text_marker.color.a = 0.9
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
if 'cylinder' in name:
marker.type = Marker.CYLINDER
marker.scale.x = 0.060
marker.scale.y = 0.060
marker.scale.z = 0.070
marker.color.a = 0.5
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
ma.markers.append(marker)
ma.markers.append(text_marker)
pcl.points += gen_flatcircle(marker.scale.x / 2.0, marker.pose.position.x, marker.pose.position.y)
if 'tennis' in name:
marker.type = Marker.SPHERE
marker.scale.x = 0.064
marker.scale.y = 0.064
marker.scale.z = 0.064
marker.color.a = 0.5
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
ma.markers.append(marker)
ma.markers.append(text_marker)
pcl.points += gen_flatcircle(marker.scale.x / 2.0, marker.pose.position.x, marker.pose.position.y)
if 'pop_corn' in name:
marker.type = Marker.SPHERE
marker.scale.x = 0.040
marker.scale.y = 0.040
marker.scale.z = 0.040
marker.color.a = 0.5
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 1.0
ma.markers.append(marker)
ma.markers.append(text_marker)
pcl.points += gen_flatcircle(marker.scale.x / 2.0, marker.pose.position.x, marker.pose.position.y)
if 'cup' in name:
marker.type = Marker.CYLINDER
marker.pose.position.z += 0.075
marker.scale.x = 0.095
marker.scale.y = 0.095
marker.scale.z = 0.150
marker.color.a = 0.5
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
ma.markers.append(marker)
ma.markers.append(text_marker)
pcl.points += gen_flatcircle(marker.scale.x / 2.0, marker.pose.position.x, marker.pose.position.y)
# if 'nucobot' in name:
# br.sendTransform((data.pose[i].position.x, data.pose[i].position.y, data.pose[i].position.z),
# (data.pose[i].orientation.x, data.pose[i].orientation.y, data.pose[i].orientation.z, data.pose[i].orientation.w),
# rospy.Time.now(),
# "base_footprint",
# "world")
pub_pcl.publish(pcl)
pub.publish(ma)
from geometry_msgs.msg import Point32
pub1 = rospy.Publisher("cloud1", PointCloud)
pub2 = rospy.Publisher("cloud2", PointCloud)
rospy.init_node('send_two_clouds')
cloud = PointCloud()
while not rospy.is_shutdown():
cloud.header.frame_id = "/base_link"
cloud.header.stamp = rospy.Time.now()
cloud.points = [
Point32( 0, 0, 0 ),
Point32( .1, 0, 0 ),
Point32( 0, .1, 0 ),
Point32( .1, .1, 0 )
]
pub1.publish( cloud )
cloud.points = [
Point32( .5, 0, 0 ),
Point32( .6, 0, 0 ),
Point32( .5, .1, 0 ),
Point32( .6, .1, 0 )
]
pub2.publish( cloud )
rospy.sleep(.5)
