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 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 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 cloud_build(self):
# add sonar readings (robot-local coordinate frame) to cloud
pl = Point32()
pm = Point32()
pr = Point32()
# Instanziiere leere PointCloud
cloud = PointCloud()
# filling pointcloud header
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base_link'
cloud.header = header
# Linke Seite
if (self.dist_left < 0.95 and self.dist_left > 0.05):
pl.x = self.dist_left + 0.05
pl.y = 0.03
pl.z = 0.0
cloud.points.append(pl)
pm.x = (self.dist_left + self.dist_right) / 2 + 0.05
pm.y = 0.0
pm.z = 0.0
cloud.points.append(pm)
# Rechte Seite punkt einfügen (x,y,z)
if (self.dist_right < 0.95 and self.dist_right > 0.05):
pr.x = self.dist_right + 0.05
pr.y = -0.03
pr.z = 0.0
cloud.points.append(pr)
# Senden
self.cloud_pub.publish(cloud)
def match_detections():
distances = []
global legs
global faces
global pub
pc = PointCloud()
pc.header = Header(0, rospy.get_rostime(), "/map")
for m in range(len(faces)):
pc.points.append(Point32(faces[m].x, faces[m].y, 1.5))
for l in range(len(legs)):
pc.points.append(Point32(legs[l].x, legs[l].y, 1.0))
distancecol = []
for m in range(len(faces)):
# measure the distance between the detections and store them
dist = sqrt((faces[m].x - legs[l].x) ** 2 + (faces[m].y - legs[l].y) ** 2)
distancecol.append(dist)
distances.append(distancecol)
print "distances"
print distances
pub.publish(pc)
def match_detections():
distances = []
global legs
global faces
global pub
pc = PointCloud()
pc.header = Header(0, rospy.get_rostime(), '/map')
for m in range(len(faces)):
pc.points.append(Point32(faces[m].x, faces[m].y, 1.5))
for l in range(len(legs)):
pc.points.append(Point32(legs[l].x, legs[l].y, 1.0))
distancecol = []
for m in range(len(faces)):
# measure the distance between the detections and store them
dist = (sqrt((faces[m].x - legs[l].x)**2 +
(faces[m].y - legs[l].y)**2))
distancecol.append(dist)
distances.append(distancecol)
print "distances"
print distances
pub.publish(pc)
def cloudCallback(point_cloud_2):
point_cloud = PointCloud()
np_point_cloud = []
#the format coming into this function is a sensor_msgs::PointCloud2
#the data in this pc2 needs to be decoded in order to get x,y,z points
gen = pc2Functions.read_points(point_cloud_2,
skip_nans=True,
field_names=("x", "y", "z", "intensity",
"ring"))
#now that we have the x,y,z points, lets turn it into our numpy array
testingPC = PointCloud()
testingPC.header.frame_id = "lidar_nwu"
for p in gen:
#only take elements above the water plane
#this number is 1.5, and not 0, because the points are in the lidar frame of reference, which is 1.5 meters above the water surface
if (math.sqrt(p[0] * p[0] + p[1] * p[1]) > 2
and math.sqrt(p[0] * p[0] + p[1] * p[1]) < 25 and p[2] > -1.3):
np_point_cloud.append([p[0], p[1], p[2]])
tempPoint = Point32()
tempPoint.x = p[0]
tempPoint.y = p[1]
tempPoint.z = p[2]
testingPC.points.append(tempPoint)
#print p
#this publisher was used to publish to rviz to visually validate that the data is doing what we want
#pubTest.publish(testingPC)
thresh = 1.25
if (not np.size(np_point_cloud) == 0):
Y = pdist(np_point_cloud, metric='euclidean')
Z = linkage(Y, method='average')
clusterPub(fcluster(Z, thresh, criterion='distance'), np_point_cloud)
def aplicarFiltro(data):
global taps
global N
scan = PointCloud()
scan.header = data.header
for i in range(0, len(data.points)):
scan.points.append(
Point(data.points[i].x, data.points[i].y, data.points[i].z))
signal = [0.0] * 2 * len(data.points)
#x = [0.0]*(kFin-kIni+1)
# j = 0
for i in range(0, len(data.points)):
signal[i] = data.points[i].z
signal[i + len(data.points)] = data.points[len(data.points) - 1].z
# x[j+2*(kFin-kIni+1)] = data.ranges[i];
# j = j + 1
filtered_signal = lfilter(taps, 1.0, signal)
delay = int(0.5 * (N - 1))
for i in range(len(data.points)):
scan.points[i].z = filtered_signal[delay + i]
return scan
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 __init__(self, robot):
self.robot = robot
self.sprayed = [] # Keep a list of sprayed points for rviz
self.real_sprayed = []
self.real_sprayed2 = []
self.last_spray_pos = 0
self.y_previous = 0
self.radius = 0.3 # killbox radius
self.spray_srv = rospy.ServiceProxy(
"{}/dynamic_sprayer".format(self.robot), y_axes_diff)
self.sub = rospy.Subscriber("thorvald_001/complete_weed_pointcloud",
PointCloud, self.spray_weed_callback)
self.tflistener = tf.listener.TransformListener()
self.sprayed_points_msg = PointCloud()
self.sprayed_points_pub = rospy.Publisher(
"{}/weed/sprayed_points/".format(self.robot),
PointCloud,
queue_size=5)
self.sprayed_points_indirect_msg = PointCloud()
self.sprayed_points_indirect_pub = rospy.Publisher(
"{}/weed/sprayed_indirect_points/".format(self.robot),
PointCloud,
queue_size=5)
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 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 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 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 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 scan_callback(scan):
lidar_data = np.array([np.array(scan.ranges), scan.intensities]).T
landmarks = filter(lidar_data)
if visualization == True:
# create and pub PointArray of detected beacon points
points = [Point(x=landmarks[i, 0], y=landmarks[i, 1], z=0) for i in range(len(landmarks))]
array = PointCloud(points=points)
array.header.frame_id = "map"
pub_landmarks.publish(array)
centers = []
if landmarks.shape[0] > 0:
# clustering
db = DBSCAN(eps=eps, min_samples=min_samples).fit(landmarks)
labels = db.labels_
unique_labels = set(labels)
for l in unique_labels:
if l == -1:
# noise
continue
class_member_mask = (labels == l)
center = get_center(landmarks[class_member_mask])
centers.append(Point(x=center.x[0], y=center.x[1], z=0))
# create and pub PointArray of detected centers
array = PointCloud(points=centers)
array.header.frame_id = "map"
array.header.stamp = rospy.Time.now()
pub_center.publish(array)
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 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 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 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 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 publish_pc2(event=None):
global pc2data, ppl, p_angles, p_segs, p_segindex, laserpub, shiftscan
segid = rospy.get_param("/segid")
if pc2data == None:
pass
else:
ppl_seg = ppl[p_segindex[segid][0]:p_segindex[segid][1]]
pangle_seg = p_angles[p_segindex[segid][0]:p_segindex[segid][1]]
#print('segid ', segid, p_segindex[segid], ppl_seg)
pc2pub.publish(pc2data)
my_pc = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = pc2data.header.frame_id
my_pc.header = header
for pt in ppl_seg:
my_pc.points.append(pt)
print "happily publishing pointcloud"
pc_pub.publish(my_pc)
#/scan
#laserscan = laser_dummy(3)
laserscan, slopesample = convert_to_scan(ppl_seg, pangle_seg,
my_pc.header, shiftscan)
laserpub.publish(laserscan)
if slopesample > 20 or slopesample < -20:
print(' slope exceed 20 abnormal')
def calculation_path(self):
target_x = self.target_point.points[0].x - 0.270
target_y = self.target_point.points[0].y - 0.000
target_z = self.target_point.points[0].z - 0.935
path_point = PointCloud()
path_point.header.frame_id = "/base_link"
vis_path_point = PointCloud()
vis_path_point.header.frame_id = "/base_link"
for x in drange(self.init_arm_end_x, target_x, self.dx):
y = self.calculation_linear(x, self.init_arm_end_x, target_x,
self.init_arm_end_y, target_y)
z = self.calculation_linear(x, self.init_arm_end_x, target_x,
self.init_arm_end_z, target_z)
# print "x : ", x, ", y : ", y, ", z : ", z
self.optimal_path_x = np.append(self.optimal_path_x,
np.array([x + 0.270]))
self.optimal_path_y = np.append(self.optimal_path_y,
np.array([y + 0.000]))
self.optimal_path_z = np.append(self.optimal_path_z,
np.array([z + 0.935]))
path_point.header.stamp = rospy.Time.now()
path_point.points.append(Point32(x, y, z))
vis_path_point.header.stamp = rospy.Time.now()
vis_path_point.points.append(
Point32(x + 0.270, y + 0.000, z + 0.935))
vis_path_point_size = len(path_point.points)
# print "path_point_size : ", vis_path_point_size
self.pub_path_point.publish(vis_path_point)
self.pub_path_point_size.publish(vis_path_point_size)
return path_point, vis_path_point
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 __init__(self, robot):
self.robot = robot
self.weedkeeplist = []
self.plantkeeplist = []
self.notsprayed = []
self.spr = rospy.ServiceProxy("{}/spray".format(self.robot), Empty)
rospy.Subscriber("/plant/points/{}".format(self.robot), PointCloud,
self.plantpoints_callback)
rospy.Subscriber("/weed/points/{}".format(self.robot), PointCloud,
self.weedpoints_callback)
rospy.Subscriber("/{}/odometry/base_raw".format(self.robot), Odometry,
self.odometry_callback)
self.weedpoints_pub = rospy.Publisher("/weed/allpoints/{}".format(
self.robot),
PointCloud,
queue_size=5)
self.plantpoints_pub = rospy.Publisher("/plant/allpoints/{}".format(
self.robot),
PointCloud,
queue_size=5)
self.weedpoints_msg = PointCloud()
self.plantpoints_msg = PointCloud()
self.tflistener = tf.listener.TransformListener()
def laser_callback(current_laser_scan):
real_angles = np.array(
[-90.0, -50.0, -30.0, -10.0, 10.0, 30.0, 50.0, 90.0])
real_angles = real_angles / 360.0 * 2 * np.pi
opening_angle = 40.0 / 360.0 * 2 * np.pi
sensor_angles = np.arange(
current_laser_scan.angle_min,
current_laser_scan.angle_max + current_laser_scan.angle_increment,
current_laser_scan.angle_increment)
sensor_ranges = np.array(current_laser_scan.ranges)
sonar_readings = np.zeros(len(real_angles))
for i in range(len(real_angles)):
sonar_readings[i] = np.min(sensor_ranges[np.where(
np.logical_and(
sensor_angles >= real_angles[i] - opening_angle / 2,
sensor_angles <= real_angles[i] + opening_angle / 2))])
pub = rospy.Publisher("robotic_games/sonar", PointCloud, queue_size=1)
output = PointCloud()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "robot"
output.header = header
for i in range(8):
#TODO add position of Sensor on Pioneer!
output.points.append((Point32(
np.sin(real_angles[i]) * sonar_readings[i],
np.cos(real_angles[i]) * sonar_readings[i], 0)))
pub.publish(output)
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 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 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 update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg
Args:
msg (LaserScan): incoming message
"""
# Transform to cartesian coordinates
scan_points = PointCloud()
scan_points.header = msg.header
for i, range in enumerate(msg.ranges):
if range == 0:
continue
# Calculate point in laser coordinate frame
angle = msg.angle_min + i * msg.angle_increment
x = range * np.cos(angle)
y = range * np.sin(angle)
scan_points.points.append(Point32(x=x, y=y))
# Transform into base_link coordinates
scan_points = self.tf_listener.transformPointCloud('base_link', scan_points)
# For each particle...
for particle in self.particle_cloud:
# Create a 3x3 matrix that transforms points from the origin to the particle
rotmatrix = np.matrix([[np.cos(particle.theta), -np.sin(particle.theta), 0],
[np.sin(particle.theta), np.cos(particle.theta), 0],
[0, 0, 1]])
transmatrix = np.matrix([[1, 0, particle.x],
[0, 1, particle.y],
[0, 0, 1]])
mat33 = np.dot(transmatrix, rotmatrix)
# Iterate through the points in the laser scan
probabilities = []
for point in scan_points.points:
# Move the point onto the particle
xy = np.dot(mat33, np.array([point.x, point.y, 1]))
# Figure out the probability of that point
distToWall = self.occupancy_field.get_closest_obstacle_distance(xy.item(0), xy.item(1))
if np.isnan(distToWall):
continue
probabilities.append(self.laser_uncertainty_model(distToWall))
# Combine those into probability of this scan given hypothesized location
# This is the bullshit thing Paul showed
# TODO: exponent should be a rosparam
totalProb = np.sum([p ** 3 for p in probabilities]) / len(probabilities)
# Update the particle's probability with new info
particle.w *= totalProb
# Normalize particles
self.normalize_particles()
def determinarSinal(scan,tanque):
dado = PointCloud()
dado.header = scan.header
for i in range(0,len(scan.points)):
ponto_x = scan.points[i].x
ponto_z = (scan.points[i].z - tanque.points[i].z)
dado.points.append(Point(ponto_x,0,ponto_z))
return dado
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 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 testecalcularCentroSolda(scan):
iMin,iMax,estado = calcularLimitesSolda(scan)
if estado != 3:
return -1
centro = PointCloud()
centro.header = scan.header
centro.points.append(Point(scan.points[int(round((iMin+iMax)/2))].x,0,0))
return centro
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 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 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 sub_cb(self, msg):
pnt = PointCloud()
pnt.header = msg.header
angle_step = 1.0/(msg.range*100.0)
angle = -msg.spread_angle
if msg.range < msg.range_max and msg.range > msg.range_min:
while angle < msg.spread_angle:
pnt.points.append(Point32(msg.range*cos(angle), msg.range*sin(angle), 0))
angle += angle_step
self.pub.publish(pnt)
def sub_cb(self, msg):
pnt = PointCloud()
pnt.header = msg.header
angle_step = 1.0 / (msg.range * 100.0)
angle = -msg.field_of_view
if msg.range < msg.max_range and msg.range > msg.min_range:
while angle < msg.field_of_view:
pnt.points.append(Point32(msg.range * cos(angle), msg.range * sin(angle), 0))
angle += angle_step
self.pub.publish(pnt)
def get_bounding_box(self, points):
""" simple axis-aligned bounding box"""
world_frame = "odom_combined"
# get pointcloud in world frame
if not self.listener.frameExists(points.header.frame_id):
rospy.logerr("point cloud header frame %s does not exist!", points.header.frame_id)
return
if not self.listener.frameExists(world_frame):
rospy.logerr("world frame %s does not exist!", world_frame)
return
pc = PointCloud()
pc.header = points.header
iter_pt = pts.read_points(points)
for pt in iter_pt:
pc.points.append(Point(pt[0], pt[1], pt[2]))
# TODO: technically, want to transform at time image was taken, but thanks
# to waiting for human input, will be arbitrary delay ...
pc.header.stamp = self.listener.getLatestCommonTime(world_frame, pc.header.frame_id)
# TODO: should be wrapped in try/except block
pc_world = self.listener.transformPointCloud(world_frame, pc)
# get world-axis-aligned bounding box
min_x = sys.float_info.max
max_x = sys.float_info.min
min_y = sys.float_info.max
max_y = sys.float_info.min
min_z = sys.float_info.max
max_z = sys.float_info.min
for pt in pc_world.points:
min_x = min(min_x, pt.x)
max_x = max(max_x, pt.x)
min_y = min(min_y, pt.y)
max_y = max(max_y, pt.y)
min_z = min(min_z, pt.z)
max_z = max(max_z, pt.z)
# testing whether fudging it helps
min_z = min_z + 0.05
# add bounding box to the planning scene
obj_pose = PoseStamped()
obj_pose.pose.position.x = (min_x + max_x)/2
obj_pose.pose.position.y = (min_y + max_y)/2
obj_pose.pose.position.z = (min_z + max_z)/2
obj_pose.pose.orientation.z = 1.0
obj_pose.header.frame_id = world_frame
obj_dims = Vector3()
obj_dims.x = max_x - min_x
obj_dims.y = max_y - min_y
obj_dims.z = max_z - min_z
return (obj_pose, obj_dims)
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 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 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 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 publish_points(self, pcloud, seq):
pc = PointCloud()
pc.header = Header(seq, rospy.Time.now(), 'points')
for i in range(len(pcloud[:, 0])):
# Publishing as (z, x, y) b/c of the way that Z is forward in
# triangulation
pc.points.append(Point32(pcloud[i, 2],
pcloud[i, 0],
pcloud[i, 1]))
self.point_pub.publish(pc)
self.tf_broadcaster.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
'points',
'map')
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 main():
viz_pub = rospy.Publisher('/cluster_boxes', Marker)
cloud_pub = rospy.Publisher('/cluster_points', PointCloud)
segmentation_service = rospy.ServiceProxy(SEGMENTATION_SERVICE, TabletopSegmentation)
rospy.loginfo('Waiting for object recognition service')
segmentation_service.wait_for_service()
rospy.loginfo('Found segmentation service!')
box_service = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox)
rospy.loginfo('Waiting for find cluster bounding box service')
box_service.wait_for_service()
rospy.loginfo('Found cluster bounding box service!')
req = TabletopSegmentationRequest()
resp = segmentation_service(req)
rospy.loginfo('Found %d clusters', len(resp.clusters))
rospy.loginfo('Table pose is:\n' + str(resp.table.pose))
table = marker_at(resp.table.pose, mtype = Marker.TRIANGLE_LIST, ns = 'table', sx = 1.0,
sy = 1.0, sz = 1.0, r = 0.0, g = 1.0, b = 0.0, a = 0.4)
for tri in resp.table.convex_hull.triangles:
for ind in tri.vertex_indices:
table.points.append(resp.table.convex_hull.vertices[ind])
viz_pub.publish(table)
pc = PointCloud()
pc.header = resp.table.pose.header
for (mid, cluster) in enumerate(resp.clusters):
box_resp = box_service.call(FindClusterBoundingBoxRequest(cluster = cluster))
marker = marker_at(
box_resp.pose, ns = str(mid), mid = mid, mtype = Marker.CUBE, sx = box_resp.box_dims.x, sy = box_resp.box_dims.y,
sz = box_resp.box_dims.z, a = 0.5)
viz_pub.publish(marker)
pose = PoseStamped()
pose.header = cluster.header
pose.pose.orientation.w = 1.0
marker = marker_at(pose, ns = 'cluster', mid = mid, mtype = Marker.POINTS, sx = 1.0,
sy = 1.0, sz = 1.0, r = 1.0, g = 0.0, b = 0.0, a = 1.0)
marker.points = cluster.points
#viz_pub.publish(marker)
pc.points += cluster.points
pc.channels += cluster.channels
rospy.loginfo('Cluster frame is ' + cluster.header.frame_id)
cloud_pub.publish(pc)
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
def sonarCallback(self, data):
sonarpc = PointCloud()
sonarpc.header.stamp = data.header.stamp
sonarpc.header.frame_id = 'base_link'
sonarpc.channels = [ChannelFloat32()]
sonarpc.channels[0].values = []
counter = 0
for r in data.ranges:
#transform the pt
tpc = Point32()
r = r + 0.24
tpc.x = cos(self.sonar_positions[counter])*r
tpc.y = sin(self.sonar_positions[counter])*r
tpc.z = 0.23
counter += 1
#add this to the cloud
sonarpc.points.append(tpc)
sonarpc.channels[0].values.append(100)
# Publish the new cmd message using publish
self.pub.publish(sonarpc)
#!/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()
# POSSIBILITY OF SUCH DAMAGE.
#!/usr/bin/env python
import roslib
roslib.load_manifest('rviz')
import rospy
from sensor_msgs.msg import PointCloud
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 ),
while not rospy.is_shutdown():
PC.header.seq = counter
PC.header.stamp = rospy.Time.now()
counter += 1
pub.publish(PC)
br.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
'points',
'map')
r.sleep()
if __name__ == '__main__':
rospy.init_node('talker', anonymous=True)
P = PointCloud()
P.header = Header(0, 0, 'points')
P.points.append(Point32(2, 0, 0)); P.points.append(Point32(3, 0, 0))
P.points.append(Point32(4, 0, 0)); P.points.append(Point32(5, 0, 0))
P.points.append(Point32(0, 3, 0)); P.points.append(Point32(0, 6, 0))
P.points.append(Point32(0, 6.5, 0)); P.points.append(Point32(0, 7, 0))
P.points.append(Point32(0, 0, 2)); P.points.append(Point32(0, 0, -2))
P.points.append(Point32(0, 0, 1)); P.points.append(Point32(0, 0, -1))
# for i in range(2):
# P.points.append(Point32(i, i, i))
talker(P)
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()
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)
else:
#print "false"
return False
#######################################
if __name__=="__main__":
rospy.init_node('wp_set')
position_sub = rospy.Subscriber('gazebo/model_states', ModelStates, positionCallback)
odom_sub = rospy.Subscriber('roomba_test/mobile_base_controller/odom', Odometry, odomCallback)
set_flag_sub = rospy.Subscriber('roomba_test/goal_set_flag', Bool, flagCallback)
pub = rospy.Publisher('roomba_test/wp', PointCloud, queue_size=2)
gp_pub = rospy.Publisher('roomba_test/wp/odom', PointCloud,queue_size=2)
pc = PointCloud()
pc.header.frame_id = "odom"
pt = Point()
pt.x = 0.0
pt.y = 0.0
pc.points.append(pt)
pc.points.append(pt)
pc.points.append(pt)
#pc.points.append(pt)
#pt.points[0].x = 0.2
#pt.points[0].y = 1.0
pc_odom = PointCloud()
pc_odom.header.frame_id = "roomba_base"
pc_odom.points.append(pt)
