def update(self): pc = PointCloud() pc.header.stamp = rospy.Time.now() pc.header.frame_id = self.frame pc.channels = [] for particle in self.pf.particles: pc.points.append(Point32(particle.pos[0], particle.pos[1], particle.pos[2])) prediction = self.pf.predict() marker = MarkerMsg() marker.header.stamp = rospy.Time.now() marker.header.frame_id = self.frame marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1 marker.color.r = 1 marker.color.g = 1 marker.color.b = 1 marker.pose.position.x = prediction[0] marker.pose.position.y = prediction[1] marker.pose.position.z = prediction[2] markerArray = MarkerArrayMsg() markerArray.markers.append(marker) id = 0 for marker in markerArray.markers: marker.id = id id = id + 1 self.pcPub.publish(pc) self.markerPub.publish(markerArray)
def odomCallback(data, tfListener):
global markerArray
horizFOV = 54.0 * math.pi / 180.0
vertFOV = 45.0 * math.pi / 180.0
depth = 1.0
horizRadsPerPixel = horizFOV / 640.0
vertRadsPerPixel = vertFOV / 480.0
angles = np.array([horizRadsPerPixel, vertRadsPerPixel])
endPixel1 = np.array([-320, -240])
endPixel2 = np.array([320, 240])
endAngs1 = angles * endPixel1
endAngs2 = angles * endPixel2
endRay1 = [math.sin(endAngs1[0]), -43]
endRay2 = [math.sin(endAngs2[0]), -43]
h1 = math.sqrt(1 + endRay1[0]**2)
h2 = math.sqrt(1 + endRay2[0]**2)
endRay1[1] = h1 * math.sin(endAngs1[1])
endRay2[1] = h2 * math.sin(endAngs2[1])
pos = data.pose.pose.position
startPoint = PointStampedMsg()
endPoint1 = PointStampedMsg()
endPoint2 = PointStampedMsg()
while True:
try:
point = PointStampedMsg()
point.point.x = 0
point.point.y = 0
point.point.z = 0
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime("/head_camera_rgb_optical_frame", "/odom")
startPoint = tfListener.transformPoint("/odom", point)
point = PointStampedMsg()
point.point.x = endRay1[0]
point.point.y = endRay1[1]
point.point.z = 1
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime("/head_camera_rgb_optical_frame", "/odom")
endPoint1 = tfListener.transformPoint("/odom", point)
point = PointStampedMsg()
point.point.x = endRay2[0]
point.point.y = endRay2[1]
point.point.z = 1
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime("/head_camera_rgb_optical_frame", "/odom")
endPoint2 = tfListener.transformPoint("/odom", point)
print "Good!"
break
except:
print "Waiting..."
markerArray = MarkerArrayMsg()
marker = MarkerMsg()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint.point.x
marker.points[0].y = startPoint.point.y
marker.points[0].z = startPoint.point.z
marker.points[1].x = endPoint1.point.x
marker.points[1].y = endPoint1.point.y
marker.points[1].z = endPoint1.point.z
marker.scale.x = 0.01
marker.scale.y = 0.02
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
markerArray.markers.append(marker)
marker = MarkerMsg()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint.point.x
marker.points[0].y = startPoint.point.y
marker.points[0].z = startPoint.point.z
marker.points[1].x = endPoint2.point.x
marker.points[1].y = endPoint2.point.y
marker.points[1].z = endPoint2.point.z
marker.scale.x = 0.01
marker.scale.y = 0.02
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1
markerArray.markers.append(marker)
print markerArray
id = 0
for marker in markerArray.markers:
marker.id = id
id = id + 1
import rospy import roslib import tf import math import numpy as np from nav_msgs.msg import Odometry as OdometryMsg from geometry_msgs.msg import PointStamped as PointStampedMsg from geometry_msgs.msg import Point as PointMsg from tf2_msgs.msg import TFMessage as TFMsg from visualization_msgs.msg import Marker as MarkerMsg from visualization_msgs.msg import MarkerArray as MarkerArrayMsg markerArray = MarkerArrayMsg() def odomCallback(data, tfListener): global markerArray horizFOV = 54.0 * math.pi / 180.0 vertFOV = 45.0 * math.pi / 180.0 depth = 1.0 horizRadsPerPixel = horizFOV / 640.0 vertRadsPerPixel = vertFOV / 480.0 angles = np.array([horizRadsPerPixel, vertRadsPerPixel]) endPixel1 = np.array([-320, -240]) endPixel2 = np.array([320, 240]) endAngs1 = angles * endPixel1
def transformImgTo3d(points, tfListener):
global markerArray, markerPublisher
print "And going back, from ", points
markerArray = MarkerArrayMsg()
horizFOV = cameraFOV[0]
vertFOV = cameraFOV[1]
depth = 1.0
horizRadsPerPixel = horizFOV / 640.0
vertRadsPerPixel = vertFOV / 480.0
angles = np.array([horizRadsPerPixel, vertRadsPerPixel])
while True:
try:
stamp = tfListener.getLatestCommonTime("/head_camera_rgb_optical_frame", "/odom")
pos, quat = tfListener.lookupTransform("/odom", "/head_camera_rgb_optical_frame", stamp)
matTransform = tfListener.fromTranslationRotation(pos, quat)
startPoint = np.array([0, 0, 0, 1])
startPoint = np.matmul(matTransform, startPoint)
for point in points:
endPixel = np.array(point)
endAngs = angles * endPixel
endRay = [math.sin(endAngs[0]), -43]
h1 = math.sqrt(1 + endRay[0]**2)
endRay[1] = h1 * math.sin(endAngs[1])
endPoint = np.array([endRay[0], endRay[1], 1, 1])
endPoint = np.matmul(matTransform, endPoint)
marker = MarkerMsg()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint[0]
marker.points[0].y = startPoint[1]
marker.points[0].z = startPoint[2]
marker.points[1].x = endPoint[0]
marker.points[1].y = endPoint[1]
marker.points[1].z = endPoint[2]
marker.scale.x = 0.005
marker.scale.y = 0.02
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
markerArray.markers.append(marker)
break
except:
print "Waiting for transform..."
# print markerArray
id = 0
for marker in markerArray.markers:
marker.id = id
id = id + 1
markerPublisher.publish(markerArray)
def update(self, current):
pc = PointCloud()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = self.frame
pc.channels = [ChannelFloat32()]
pc.channels[0].name = "weight"
for particle in self.pf.particles:
pc.points.append(
Point32(particle.pos[0], particle.pos[1], particle.pos[2]))
pc.channels[0].values.append(particle.getWeight())
prediction = self.pf.predict()
r = self.markerColor[0]
g = self.markerColor[1]
b = self.markerColor[2]
a = self.markerColor[3]
markerArray = MarkerArrayMsg()
marker = MarkerMsg()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = self.frame
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = a
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.pose.position.x = prediction[0]
marker.pose.position.y = prediction[1]
marker.pose.position.z = prediction[2]
markerArray.markers.append(marker)
startPoint = current[0]
for endPoint in current[1]:
marker = MarkerMsg()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = self.frame
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint[0]
marker.points[0].y = startPoint[1]
marker.points[0].z = startPoint[2]
marker.points[1].x = endPoint[0]
marker.points[1].y = endPoint[1]
marker.points[1].z = endPoint[2]
marker.scale.x = 0.0025
marker.scale.y = 0.0025
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 1
marker.color.b = 1
markerArray.markers.append(marker)
id = 0
for marker in markerArray.markers:
marker.id = id
id = id + 1
self.pcPub.publish(pc)
self.markerPub.publish(markerArray)
def odomCallback(data, tfListener):
global markerArray
pos = data.pose.pose.position
startPoint = PointStampedMsg()
endPoint = PointStampedMsg()
while True:
try:
point = PointStampedMsg()
point.point.x = 0
point.point.y = 0
point.point.z = 0
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime(
"/head_camera_rgb_optical_frame", "/odom")
startPoint = tfListener.transformPoint("/odom", point)
point = PointStampedMsg()
point.point.x = 0
point.point.y = 0
point.point.z = 1
point.header.frame_id = "/head_camera_rgb_optical_frame"
point.header.stamp = tfListener.getLatestCommonTime(
"/head_camera_rgb_optical_frame", "/odom")
endPoint = tfListener.transformPoint("/odom", point)
print "Good!"
break
except:
print "Waiting..."
# marker = MarkerMsg()
# marker.header.frame_id = "/odom"
# marker.type = marker.ARROW
# marker.action = marker.ADD
# marker.scale.x = 0.05
# marker.scale.y = 0.05
# marker.scale.z = 0.05
# marker.color.a = 1
# marker.color.r = 1
# marker.color.g = 1
# marker.color.b = 1
# marker.pose.position.x = endPoint.point.x
# marker.pose.position.y = endPoint.point.y
# marker.pose.position.z = endPoint.point.z
marker = MarkerMsg()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [PointMsg(), PointMsg()]
marker.points[0].x = startPoint.point.x
marker.points[0].y = startPoint.point.y
marker.points[0].z = startPoint.point.z
marker.points[1].x = endPoint.point.x
marker.points[1].y = endPoint.point.y
marker.points[1].z = endPoint.point.z
marker.scale.x = 0.01
marker.scale.y = 0.02
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 1
marker.color.g = 1
marker.color.b = 1
markerArray = MarkerArrayMsg()
markerArray.markers.append(marker)
id = 0
for marker in markerArray.markers:
marker.id = id
id = id + 1