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 __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 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 __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 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 build_and_publish_obstacle_point_clouds(self,
reachable_workspace_points):
"""
Builds and publishes obstacles point clouds for both the left and right Baxter arms.
Publishes obstacle clouds in the base from, as they are transformed and filtered points.
:param reachable_workspace_points: the overall environment points list, filtered and transformed to base frame,
in reachable workspace
"""
obstacle_cloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base'
obstacle_cloud.header = header
left_filtered_pts = self.filter_out_arm(reachable_workspace_points,
"left")
# update collision checker obstacle list
self.left_cc.update_obstacles(left_filtered_pts)
for point in left_filtered_pts:
obstacle_cloud.points.append(Point32(point[0], point[1], point[2]))
print "publishing new left obstacle cloud!"
self.left_obs_pub.publish(obstacle_cloud)
obstacle_cloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base'
obstacle_cloud.header = header
right_filtered_pts = self.filter_out_arm(reachable_workspace_points,
"right")
# update collision checker obstacle list
self.right_cc.update_obstacles(right_filtered_pts)
for point in right_filtered_pts:
obstacle_cloud.points.append(Point32(point[0], point[1], point[2]))
print "publishing new right obstacle cloud!"
self.right_obs_pub.publish(obstacle_cloud)
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 __init__(self):
self.joint_state = np.zeros((3), dtype=np.float32)
# print self.joint_state
self.action_num = 0
self.reward = 0.0
self.target_point = PointCloud()
self.target_init_x = self.L + 0.270
self.target_init_y = -0.080
self.target_init_z = 0.960
self.arm_end = np.zeros((3), dtype=np.float32)
self.arm_end_com = np.zeros((3), dtype=np.float32)
self.num_step = 0
self.num_episode = 0
# self.model = chainer.FunctionSet(
# l1 = F.Linear(6, 2048),
# l2 = F.Linear(2048, 1024),
# l3 = F.Linear(1024, 512),
# l4 = F.Linear(512, 256),
# l5 = F.Linear(256, 128),
# l6 = F.Linear(128, 64),
# l7 = F.Linear(64, 27, initialW=np.zeros((27, 64), dtype=np.float32)),
# )
f = open(
'/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_model/dqn_test16_angeal/dqn_arm_model_30000.dat',
'rb')
self.model = pickle.load(f)
if args.gpu >= 0:
self.model.to_gpu()
self.optimizer = optimizers.SGD()
self.optimizer.setup(self.model)
self.q_list = chainer.Variable(xp.zeros((1, 27), dtype=xp.float32))
self.action = 0
self.joint1 = self.init_state_joint1
self.joint2 = -86.51
self.joint3 = self.init_state_joint3
self.joint5 = self.init_state_joint5
self.next_joint1 = self.init_state_joint1
self.next_joint3 = self.init_state_joint3
self.next_joint5 = self.init_state_joint5
self.joint1_com = self.init_state_joint1
self.joint3_com = self.init_state_joint3
self.joint5_com = self.init_state_joint5
self.next_arm_end_point = PointCloud()
self.optimal_path_x = np.array([])
self.optimal_path_y = np.array([])
self.optimal_path_z = np.array([])
def callback2(pc=PointCloud()):
avg_height = 0
pc_new = PointCloud()
for p in pc.points:
avg_height += (p.z + 2.314)
if (p.z + 2.314) > height:
pc_new.points.append(p)
pc_new.header.stamp = rospy.Time.now()
pc_new.header.frame_id = pc.header.frame_id
pub.publish(pc_new)
print 'Recived ', len(
pc.points), ' points. avg height is: ', avg_height / len(
pc.points), ' published ', len(pc_new.points), ' points'
def __init__(self):
print 'status init'
self.map = OccupancyGrid()
self.sharedmap = OccupancyGrid()
self.mission = StringStamped()
self.missionext = StringStamped()
self.robotpose = PoseWithCovarianceStamped()
self.robotposeext = PoseWithCovarianceStamped()
self.robotstatus = StringStamped()
self.robotstatusext = StringStamped()
self.cloud = PointCloud()
self.cloud_ext = PointCloud()
self.image = Image()
self.image_ext = Image()
def __init__(self, node_name="pointcloud_reproject"):
self.pointcloud = PointCloud()
self.pointcloud2 = PointCloud()
self.node_name = node_name
#self.listener = tf.TransformListener()
rospy.init_node(self.node_name)
self.tf = TransformListener()
#self.tf = TransformerROS()
self.pointcloud_sub = rospy.Subscriber("/output_cloud_husky2",
PointCloud,
self.Pointcloud_callback)
self.pointcloud_pub = rospy.Publisher("~pointcloud_reproject",
PointCloud,
queue_size=2)
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 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 run(self, parameters):
result = self.fetch_result()
found_buoys = yield self.database_query("start_gate")
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
if len(buoys) == 2:
ogrid, setup, target = yield self.two_buoys(buoys, 10)
elif len(buoys) == 4:
ogrid, setup, target = yield self.four_buoys(buoys, 10)
else:
raise Exception
# Put the target into the point cloud as well
points = []
points.append(mil_tools.numpy_to_point(target.position))
pc = PointCloud(header=mil_tools.make_header(frame='/enu'),
points=np.array(points))
yield self._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield self.nh.sleep(3)
yield setup.go(move_type='skid')
pose = yield self.tx_pose
ogrid.generate_grid(pose)
msg = ogrid.get_message()
print "publishing"
self.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
print "START_GATE: Moving in 5 seconds!"
yield target.go(initial_plan_time=5)
defer.returnValue(result)
def get_prepared_pointcloud(self, pts, frame):
cloud = PointCloud()
cloud.header.frame_id = frame
cloud.header.stamp = rospy.Time.now()
for p in pts:
cloud.points.append(self.get_point_stamped(p, frame).point)
return cloud
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 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 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 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 callback(self, _data): #deep copy
ls = LaserScan()
ps = PointCloud()
ps.header.frame_id = "laser"
_r = 2
for i in range(0, 720):
_theta = 3.14 - (i * 0.01)
_z = _r * math.sin(_theta)
for j in range(0, 720):
_theta2 = 3.14 - (j * 0.01)
_x = _r * math.cos(_theta) * math.cos(_theta2)
_y = _r * math.cos(_theta) * math.sin(_theta2)
po = Point32()
po.x = _x
po.y = _y
po.z = _z
#print(po.z)
#for self.j in range(0,len(_data.ranges)):
#po.x=po.x*math.cos(_data.angle_increment*self.j)
#po.y=po.y*math.sin(_data.angle_increment*self.j)
#print(po.z)
ps.points.append(po)
self.point_pub.publish(ps)
def __init__(self):
ns = 'voxel_map/'
self._visual_threshold = rospy.get_param(ns + 'visual_threshold')
voxels_per_side = rospy.get_param(ns + 'voxels_per_side')
width_on_a_side = rospy.get_param(ns + 'width_on_a_side')
initial_prob = rospy.get_param(ns + 'initial_prob')
pD = rospy.get_param(ns + 'pD')
pFA = rospy.get_param(ns + 'pFA')
ps = rospy.get_param(ns + 'ps')
pt = rospy.get_param(ns + 'pt')
self.map = VoxelMap(voxels_per_side, width_on_a_side, initial_prob, pD,
pFA, ps, pt)
rospy.Subscriber("truth/NED",
Odometry,
callback=self.state_cb,
queue_size=1)
rospy.Subscriber("triang_points",
PointCloud,
callback=self.measurement_cb)
self.map_pub = rospy.Publisher("voxel_map", PointCloud, queue_size=1)
self.vis_pub = rospy.Publisher("voxel_map_visual",
PointCloud,
queue_size=1)
self.map_center_pub = rospy.Publisher("voxel_map_center", Odometry)
self.state = State()
self.blank_cloud = PointCloud()
zero_point = Point32(0, 0, 0)
self.blank_cloud.points = [zero_point] * self.map.N**3
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 laser_callback(self, msg):
pcd = PointCloud()
motor_msg = Float64()
pcd.header.frame_id = msg.header.frame_id
angle = 0
for r in msg.ranges:
tmp_point = Point32()
tmp_point.x = r * cos(angle)
tmp_point.y = r * sin(angle)
print(angle, tmp_point.x, tmp_point.y)
angle = angle + (1.0 / 180 * pi)
if r < 12:
pcd.points.append(tmp_point)
count = 0
for point in pcd.points:
if point.x > 0 and point.x < 1 and point.y > -1 and point.y < 1:
count = count + 1
if count > 20:
motor_msg.data = 0
else:
motor_msg.data = 2000
print(count)
self.motor_pub.publish(motor_msg)
self.pcd_pub.publish(pcd)
def __init__(self, width, frame):
"""
:param self: The self reference.
:param width: The width of the robot.
:param frame: The base coordinate frame of the robot.
"""
# We're going to assume that the robot is pointing up the x-axis, so that any points with y coordinates further
# than half a robot width from the axis are not in front of the robot.
self.extent = width / 2.0
self.frame = frame
# A subscriber and a publisher. We're going to give these generic names, and deal with changing them at runtine
# by using topic remapping.
self.sub = rospy.Subscriber('input_scan',
LaserScan,
self.filter_scan,
queue_size=1)
self.pub = rospy.Publisher('output_scan', LaserScan, queue_size=10)
# Set up a tf listener, so that we can do the frame transforms.
self.listener = tf.TransformListener()
# Make a PointCloud, which we're going use for the transform. We'll do this once to avoid the creation/destruction
# overhead in the callback.
self.cloud = PointCloud()
def __init__(self): print 'status init' self.depth = PointCloud() self.camera = Image() self.camera_info = CameraInfo() self.map = OccupancyGrid() self.pose = PoseWithCovarianceStamped()
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 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 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 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 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
