def run(self):
while not rospy.is_shutdown():
rospy.loginfo("$gt: %d" % (rospy.Time.now().secs - self.duration * 60 * 60 * 24))
if self.use_ros_time or self.use_sim_time:
rospy.loginfo("fetching data")
trans = self.msg_store.query(type=TransformStamped._type,
message_query={"header.stamp.secs": {
"$lte": rospy.Time.now().secs,
"$gt": rospy.Time.now().secs - self.duration * 60 * 60 * 24
}},
sort_query=[("$natural", 1)],
limit=self.limit)
else:
trans = self.msg_store.query(type=TransformStamped._type,
meta_query={"inserted_at": {
"$gt": datetime.utcnow() - timedelta(days=self.duration)
}},
sort_query=[("$natural", 1)])
rospy.loginfo("found %d msgs" % len(trans))
m_arr = MarkerArray()
m_arr.markers = V.transformStampedArrayToLabeledLineStripMarker(trans[::self.downsample], label_downsample=self.label_downsample, discrete=True)
m_arr.markers = V.transformStampedArrayToLabeledArrayMarker(trans[::self.downsample], label_downsample=self.label_downsample, discrete=True)
self.pub.publish(m_arr)
rospy.sleep(1.0)
rospy.loginfo("publishing move_base_marker_array")
def people_locations_cb(self, data):
## Handles an array of personlocation2d
## creates a marker for each and publishes an
## array of the markers
## holds the objects representing people
marker_array = MarkerArray()
marker_array.markers = []
## contains labels for people
marker_label_array = MarkerArray()
marker_label_array.markers = []
for person in data.people_location:
## in future have a heat map color marker
# color = self.contamination_color(person.contamination)
color = ColorRGBA(0.2, 0.5, 0.7, 1.0)
marker = self.create_cylindrical_person_marker(person, color)
marker_label = self.create_person_label_marker(person, color)
marker_array.markers.append(marker)
marker_label_array.markers.append(marker_label)
self.people_marker_pub.publish(marker_array)
self.people_label_marker_pub.publish(marker_label_array)
def publishLines(self, markerLines, lines, stamp):
markerArray = MarkerArray()
markerArray.markers = markerLines
self.pubMarker.publish(markerArray);
linesMsg = visionTests.msg.Lines()
linesMsg.header.stamp = stamp
linesMsg.lines = lines
self.pubLines.publish(linesMsg)
def calcAffordances(self, lines):
# Calculate affordances by intersecting agains one triangle per affordance
# make initial triangle
o = Point(0, 0, 0)
# The triangle is a bit behind
v1 = Point(cos(self.aperture / 2) * self.lenght, sin(self.aperture / 2) * self.lenght, 0)
v2 = Point(cos(self.aperture / 2) * self.lenght, -sin(self.aperture / 2) * self.lenght, 0)
t = Triangle([o, v1, v2])
# for each angle: transform the triangle and intersect it with all lines
lBase = Line(o, Point(1, 0, 0));
affordances = []
markers = []
i = 0
for angle in self.possibleAngles:
# reference transformation
lTransform = Line(o, Point(cos(angle), sin(angle), 0))
mov = Movement(lBase, lTransform)
tMoved = t.moved(mov);
#canMove = True
# Empty lines means no affordance (too close to the wall to see a line
canMove = len(lines) > 0
j = 0
while j < len(lines) and canMove:
l = lines[j]
p1 = l.points[0]
p2 = l.points[1]
canMove = canMove and not tMoved.containedOrIntersect(Point(p1.x, p1.y, p1.z), Point(p2.x, p2.y, p2.z))
# if not canMove:
# print "cant move to", angle
# print tMoved.vertices
# print p1
# print p2
j += 1
affordances.append(canMove)
if canMove:
color = 'b'
else:
color = 'r'
geomPoints = [geometry_msgs.msg.Point(p.r[0], p.r[1], p.r[2]) for p in tMoved.vertices]
geomPoints.append(geomPoints[0])
markers.append(self.getLineStrip(geomPoints, i, rospy.Time.now(), color))
i += 1
for l in lines:
# print "line", a
markers.append(self.getLineStrip([l.points[0], l.points[1]], i, rospy.Time.now(), 'g'))
i = i + 1
mArray = MarkerArray()
mArray.markers = markers
self.pubMarker.publish(mArray)
return affordances
def table_marker_publisher():
rospy.init_node('table_marker_publisher', anonymous=True)
pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1)
rospy.sleep(.3)
if not rospy.is_shutdown():
msg = MarkerArray()
msg.markers = make_table_markers()
msg.markers.append(make_mesh_marker("maker_frame", len(msg.markers) + 1, Pose(Point(z=0.04), Quaternion(z=1.0)), Vector3(1, 1, 1), opaque(), "package://giskard_examples/models/electrical-devices/pancake-maker.dae"))
msg.markers.append(make_cylinder_marker("cup_bottom_frame", len(msg.markers) +1, Pose(position=Point(z=0.07)), Vector3(0.085, 0.085, 0.14), transparent_red(), True))
pub.publish(msg)
def clear_markers(self):
ma = MarkerArray()
ma.markers = [ Marker() for tid in self.tracks3d.keys() ]
for tid,i in zip(self.tracks3d.keys(), range(len(self.tracks3d))):
ma.markers[i].header.frame_id = "camera_rgb_optical_frame"
ma.markers[i].ns = "line"
ma.markers[i].id = tid
ma.markers[i].action = 2
self.pub_marker.publish(ma)
for i in range(len(self.tracks3d)):
ma.markers[i].ns = "point"
self.pub_marker.publish(ma)
def place_kin_tree_from_link(self, ps, linkname, valuedict=None, ns = "default_ns"):
markers = make_kin_tree_from_link(ps, linkname, valuedict=valuedict)
marker_array = MarkerArray()
marker_array.markers = markers
handles = []
for marker in markers:
id = self.get_unused_id()
self.ids.add(id)
marker.id = id
marker.ns = ns
handles.append(MarkerHandle(marker, self.pub))
self.pub.publish(marker)
return handles
def table_marker_publisher():
rospy.init_node('table_marker_publisher')
cup_on_table = rospy.get_param("~cup_on_table")
pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1)
rospy.sleep(.3)
if not rospy.is_shutdown():
msg = MarkerArray()
msg.markers = make_table_markers()
if cup_on_table:
msg.markers.append(make_mesh_marker("base_link", len(msg.markers) + 1, Pose(Point(x=0.4, z=0.71), Quaternion(z=1.0)), Vector3(1, 1, 1), white(), "package://giskard_ros/object_meshes/cup_eco_orange.dae", frame_locked=True))
else:
msg.markers.append(make_mesh_marker("l_gripper_tool_frame", len(msg.markers) + 1, Pose(Point(x=-0.01, z=-0.02), Quaternion(z=1.0)), Vector3(1, 1, 1), white(), "package://giskard_ros/object_meshes/cup_eco_orange.dae", frame_locked=True))
msg.markers.append(make_mesh_marker("r_gripper_tool_frame", len(msg.markers) + 1, Pose(Point(x=-0.01, z=0.02), Quaternion(z=1.0)), Vector3(1, 1, 1), white(), "package://giskard_ros/object_meshes/sigg_bottle.dae", frame_locked=True))
# msg.markers.append(make_cylinder_marker("cup_bottom_frame", len(msg.markers) +1, Pose(position=Point(z=0.07)), Vector3(0.085, 0.085, 0.14), transparent_red(), True))
pub.publish(msg)
def get_many_vectors(self):
arrows = MarkerArray()
coords = []
i = 0
color = ColorRGBA(0.0, 1.0, 0.0, 1.0)
for lat in np.linspace(0, np.pi, 10):
color.g += 0.1
color.b = 0
for lon in np.linspace(0, 2 * np.pi, 10):
color.b += 0.1
coords.append((lat, lon, 1, i, copy.copy(color)))
i += 1
arrows.markers = [
create_arrow_marker(self.ell_space.ellipsoidal_to_pose(lat, lon, height), i, clr)
for lat, lon, height, i, clr in coords
]
return arrows
def callback(data):
global marker_ests
#Publish it as a marker in rviz
marker_ests = MarkerArray()
marker_ests.markers = []
print len(data.poses)
i = 0
for pose in data.poses:
marker_est = Marker()
marker_est.header.frame_id = "base"
marker_est.ns = "est_pose_"+str(i)
marker_est.id = 42+i
marker_est.type = Marker.CUBE
marker_est.action = Marker.ADD
marker_est.pose = pose
marker_est.color.r, marker_est.color.g, marker_est.color.b = (0, 255, 0)
marker_est.color.a = 0.5
marker_est.scale.x, marker_est.scale.y, marker_est.scale.z = (0.06, 0.06, 0.06)
marker_ests.markers.append(marker_est)
i+=1
def publish_frame_marker(pose_stamped, id_=1, length=0.1):
"""
:type pose_stamped: PoseStamped
:type id_: int
"""
kdl_pose = msg_to_kdl(pose_stamped.pose.orientation)
ma = MarkerArray()
x = Marker()
x.action = x.ADD
x.ns = u'debug'
x.id = id_
x.type = x.CUBE
x.header.frame_id = pose_stamped.header.frame_id
x.pose.position = copy(pose_stamped.pose.position)
x.pose.orientation = pose_stamped.pose.orientation
v = PyKDL.Vector(length / 2., 0, 0)
v = kdl_pose * v
x.pose.position.x += v[0]
x.pose.position.y += v[1]
x.pose.position.z += v[2]
x.color = ColorRGBA(1, 0, 0, 1)
x.scale.x = length
x.scale.y = length / 10.
x.scale.z = length / 10.
ma.markers.append(x)
y = Marker()
y.action = y.ADD
y.ns = u'debug'
y.id = id_ + 1
y.type = y.CUBE
y.header.frame_id = pose_stamped.header.frame_id
y.pose.position = copy(pose_stamped.pose.position)
y.pose.orientation = pose_stamped.pose.orientation
v = PyKDL.Vector(0, length / 2., 0)
v = kdl_pose * v
y.pose.position.x += v[0]
y.pose.position.y += v[1]
y.pose.position.z += v[2]
y.color = ColorRGBA(0, 1, 0, 1)
y.scale.x = length / 10.
y.scale.y = length
y.scale.z = length / 10.
ma.markers.append(y)
z = Marker()
z.action = z.ADD
z.ns = u'debug'
z.id = id_ + 2
z.type = z.CUBE
z.header.frame_id = pose_stamped.header.frame_id
z.pose.position = copy(pose_stamped.pose.position)
z.pose.orientation = pose_stamped.pose.orientation
v = PyKDL.Vector(0, 0, length / 2.)
v = kdl_pose * v
z.pose.position.x += v[0]
z.pose.position.y += v[1]
z.pose.position.z += v[2]
z.color = ColorRGBA(0, 0, 1, 1)
z.scale.x = length / 10.
z.scale.y = length / 10.
z.scale.z = length
ma.markers.append(z)
pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=1)
while pub.get_num_connections() < 1:
# wait for a connection to publisher
# you can do whatever you like here or simply do nothing
pass
pub.publish(ma)
def scan_callback(data):
global angles, current_state, min_index, max_index, mid_poses, distance_thresh, pub_marker, min_opening_width, door_poses
# print "scan"
my_angles = angles[min_index:max_index]
ranges = list(data.ranges[min_index:max_index])
# ranges = map(lambda y: y>distance_thresh and distance_thresh or y, ranges)
ranges = map(lambda y: y > min_dist and distance_thresh or y, ranges)
cart_points = map(polar2cart, ranges, my_angles)
# print cart_points
# opening = [x > distance_thresh for x in ranges]
start_border_index = []
stop_border_index = []
opening_IDs = []
started = False
for i in range(1, len(ranges)): # also supposed to work for the start and stop ends
if math.fabs(ranges[i] - ranges[i - 1]) > delta_dist:
opening_IDs.append(i)
# print opening_IDs
# print start_border_index
# langles = [None]
# for i in range(1, len(cart_points)-2):
# b = math.sqrt( (cart_points[i+1][0] - cart_points[i-1][0])**2 + (cart_points[i+1][1] - cart_points[i-1][1])**2 )
# c = math.sqrt( (cart_points[i][0] - cart_points[i-1][0])**2 + (cart_points[i][1] - cart_points[i-1][1])**2 )
# a = math.sqrt( (cart_points[i+1][0] - cart_points[i][0])**2 + (cart_points[i+1][1] - cart_points[i][1])**2 )
# langle = math.acos( (a**2 + c**2 - b**2)/(2*a*c) )
# langles.append(langle)
# langles.append(None)
# print langles
# print cart_points
mm = MarkerArray()
mm.markers = []
# marker = Marker() # circle of maximal distance (green)
# marker.header.frame_id = "/base_laser_rear_link"
# marker.header.stamp = rospy.Time.now()
# marker.lifetime = rospy.Duration(1.5)
# marker.ns = "edges"
# marker.id = 11000
# marker.type = 2 # SPHERE
# marker.action = 0 # ADD
# marker.pose.position.x = 0.
# marker.pose.position.y = 0.
# marker.pose.position.z = 0.
# marker.pose.orientation.x = 0.0
# marker.pose.orientation.y = 0.0
# marker.pose.orientation.z = 1.0
# marker.pose.orientation.w = 1.0
# marker.scale.x = 2*distance_thresh
# marker.scale.y = 2*distance_thresh
# marker.scale.z = 0.25
# marker.color.a = 0.3
# marker.color.r = 0. # self.color.r
# marker.color.g = 1. # self.color.g
# marker.color.b = 0. # self.color.b
# mm.markers.append(marker)
# pub_marker.publish(mm)
marker = Marker() # circle of minimal distance (yellow)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
# marker.lifetime = rospy.Duration(1.5)
marker.ns = "edges"
marker.id = 8746
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 2 * min_dist
marker.scale.y = 2 * min_dist
marker.scale.z = 0.30
marker.color.a = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 1.0 # self.color.g
marker.color.b = 0.0 # self.color.b
mm.markers.append(marker)
## special cases: open to right or left
iStart = []
iStop = []
lengths = []
if ranges[0] >= min_dist:
ranges[0] = min_dist
cart_points[0] = polar2cart(ranges[0], my_angles[0])
if ranges[len(ranges) - 1] >= min_dist:
ranges[len(ranges) - 1] = min_dist
cart_points[len(ranges) - 1] = polar2cart(ranges[len(ranges) - 1], my_angles[len(ranges) - 1])
started = False
for i in range(1, len(ranges)):
if not started and ranges[i] >= min_dist:
iStart.append(i - 1)
started = True
elif started and ranges[i] <= min_dist:
iStop.append(i)
started = False
for i in range(0, len(ranges)): # all ranges (blue):
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 3000 + i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = cart_points[i][0]
marker.pose.position.y = cart_points[i][1]
marker.pose.position.z = 0.5
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.04
marker.scale.y = 0.04
marker.scale.z = 0.25
marker.color.a = 0.7
marker.color.r = 0.6 # self.color.r
marker.color.g = 0.0 # self.color.g
marker.color.b = 0.8 # self.color.b
if i == 0:
marker.color.g = 0.0
marker.color.a = 1.0
marker.color.b = 1.0
marker.scale.x = 0.18
marker.scale.y = 0.28
marker.scale.z = 0.3
mm.markers.append(marker)
for i in range(0, len(iStart)):
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 17101 + 10 * i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = cart_points[iStart[i]][0]
marker.pose.position.y = cart_points[iStart[i]][1]
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 0.0 # self.color.g
marker.color.b = 0.0 # self.color.g
mm.markers.append(marker)
for i in range(0, len(iStop)):
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 17102 + 10 * i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = cart_points[iStop[i]][0]
marker.pose.position.y = cart_points[iStop[i]][1]
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 0.0 # self.color.g
marker.color.b = 1.0 # self.color.g
mm.markers.append(marker)
pub_marker.publish(mm)
print "iStart:"
print iStart
print "iStop:"
print iStop
for i in range(0, len(iStart)):
lengths.append(
math.sqrt(
(cart_points[iStart[i]][0] - cart_points[iStop[i]][0]) ** 2
+ (cart_points[iStart[i]][1] - cart_points[iStop[i]][1]) ** 2
)
)
toKeep = []
for i in range(0, len(iStart)):
if lengths[i] > min_opening_width:
toKeep.append(i)
iStart = [iStart[i] for i in toKeep]
iStop = [iStop[i] for i in toKeep]
lengths = [lengths[i] for i in toKeep]
centerpoints = []
tangents = []
normals = []
phis = []
for i in range(0, len(iStart)):
centerpoints.append(
[
(cart_points[iStart[i]][0] + cart_points[iStop[i]][0]) * 0.5,
(cart_points[iStart[i]][1] + cart_points[iStop[i]][1]) * 0.5,
]
)
tangents.append(
[cart_points[iStart[i]][0] - cart_points[iStop[i]][0], cart_points[iStart[i]][1] - cart_points[iStop[i]][1]]
)
normals.append([-tangents[len(tangents) - 1][1], tangents[len(tangents) - 1][0]])
phis.append(math.atan(normals[len(tangents) - 1][1] / normals[len(tangents) - 1][0]))
door_poses = []
for i in range(0, len(iStart)):
p = Pose2D()
p.x = centerpoints[i][0]
p.y = centerpoints[i][1]
p.theta = phis[i]
door_poses.append(p)
print len(iStart)
print door_poses
for i in range(0, len(iStart)):
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 17103 + 10 * i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = centerpoints[i][0]
marker.pose.position.y = centerpoints[i][1]
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 1.0 # self.color.g
marker.color.b = 0.0 # self.color.g
mm.markers.append(marker)
marker = Marker()
marker.lifetime = rospy.Duration(1.5)
marker.header.frame_id = "/base_laser_rear_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "edges"
marker.id = 17104 + 10 * i
marker.type = 2 # SPHERE
marker.action = 0 # ADD
marker.pose.position.x = centerpoints[i][0] + normals[i][0]
marker.pose.position.y = centerpoints[i][1] + normals[i][1]
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 1.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
marker.color.a = 0.7
marker.color.r = 1.0 # self.color.r
marker.color.g = 0.0 # self.color.g
marker.color.b = 1.0 # self.color.g
mm.markers.append(marker)
pub_marker.publish(mm)
x = random.randint(1, 10)
if x < 5:
current_state = False
else:
current_state = True
def particle_filter_callback(self, poses, weights):
# Construct mean pose
how_many = len(poses.poses)
mean_pose = Pose()
RZ = 0
for pose in poses.poses:
mean_pose.position.x += pose.position.x / how_many
mean_pose.position.y += pose.position.y / how_many
mean_pose.position.z += pose.position.z / how_many
q = [pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w]
rx, ry, rz = euler_from_quaternion(q)
RZ += rz / how_many
[mean_pose.orientation.x,
mean_pose.orientation.y,
mean_pose.orientation.z,
mean_pose.orientation.w] = quaternion_about_axis(RZ, (0,0,1))
# Use Decorate-Sort-Undecorate idiom
weights_sum = sum([weight.data for weight in weights.weights])
#print weights_sum
weights_enumerated = [[weight.data / weights_sum, i] for i, weight in enumerate(weights.weights)]
weights_enumerated.sort(reverse=True)
#pyplot.plot([weight[0] for weight in weights_enumerated], '.')
#pyplot.show()
self.confidence = 1.0
if self.confidence == -1: # just the mean pose
poses.poses = [mean_pose]
else:
for i in range(len(weights_enumerated)):
if i > 0:
weights_enumerated[i][0] += weights_enumerated[i-1][0] # add previous weight
if weights_enumerated[i][0] > self.confidence:
rospy.loginfo('Selected {0} poses.'.format(i+1))
break
indices = [el[1] for el in weights_enumerated]
poses.poses = [poses.poses[index] for index in indices[:i+1]]
poses.poses.append(mean_pose) # append mean pose!
weights.weights = [weights.weights[index] for index in indices[:i+1]]
markers = MarkerArray() # clear markers
for i in range(500): # HACK assumes 500 poses
marker = Marker()
marker.header = poses.header
marker.id = i
marker.type = marker.ARROW
marker.action = 0 # add/modify marker
marker.pose = poses.poses[i]
marker.scale.x = 0.15 # constant length
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.a = 1.0
#if weights_enumerated[i][0] < 0.20:
if i < 0:
marker.color.r = 0.2
marker.color.g = 0.2
marker.color.b = 0.2
#elif weights_enumerated[i][0] < 0.50:
elif i < 5:
marker.pose.position.z +=0.03
marker.color.r = 0
marker.color.g = 1
marker.color.b = 0
#elif weights_enumerated[i][0] < 1.00:
elif i < 505:
marker.pose.position.z +=0.03
marker.color.r = 0
marker.color.g = 1
marker.color.b = 0
if len(markers.markers) >= 501:
markers.markers[i] = marker
else:
markers.markers.append(marker)
# Add mean pose
marker = Marker()
marker.header = poses.header
marker.id = i
marker.type = marker.ARROW
marker.action = 0 # add/modify marker
marker.pose = mean_pose
marker.pose.position.z += 0.07
marker.scale.x = 0.15 # constant length
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.a = 1.0
marker.color.r = 1
marker.color.g = 1
marker.color.b = 1
if len(markers.markers) >= 501:
markers.markers[-1] = marker
else:
markers.markers.append(marker)
self.pub_markers.publish(markers)
import roslib
import rospy
roslib.load_manifest('path_planner')
from std_msgs.msg import Header
import math, time
from visualization_msgs.msg import Marker, MarkerArray
import random
from sim.vector import v, V
rospy.init_node('buoy_generator')
global buoy_array
buoy_publisher = rospy.Publisher('buoys', MarkerArray)
buoy_array = MarkerArray()
def append_marker(pos, color):
marker = Marker()
marker.header.frame_id = "/simmap"
marker.type = marker.SPHERE
marker.id = pos[0] * pos[1]
marker.lifetime = rospy.Duration(0)
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
def visualize_wrist_pose(p_wrist, q_wrist, tri_center, obj_center, pt0, pt1,
pt2, tri_idx, vis_arr_pub):
# Marker code copied from geo_ellipsoid.py, where I also visualize a
# quaternion with an arrow! That has the correct code.
marker_arr = MarkerArray()
frame_id = '/base'
alpha = 0.8
duration = 0
# Plot a cyan square at wrist position
marker_p = Marker()
create_marker(Marker.POINTS, 'wrist_pos', frame_id, 0, 0, 0, 0, 0, 1, 1,
alpha, 0.005, 0.005, 0.005, marker_p,
duration) # Use 0 duration for forever
marker_p.points.append(Point(p_wrist[0], p_wrist[1], p_wrist[2]))
marker_arr.markers.append(marker_p)
# Add a copy to cumulative markers
marker_p = Marker()
create_marker(Marker.POINTS, 'wrist_pos_cumu', frame_id, tri_idx, 0, 0, 0,
0, 1, 1, alpha, 0.002, 0.002, 0.002, marker_p,
duration) # Use 0 duration for forever
marker_p.points.append(Point(p_wrist[0], p_wrist[1], p_wrist[2]))
marker_arr.markers.append(marker_p)
# Plot a cyan arrow for orientation. Arrow starts at wrist orientation, ends
# at average center of current triangle.
marker_q = Marker()
create_marker(
Marker.ARROW,
'wrist_quat',
frame_id,
0,
p_wrist[0],
p_wrist[1],
p_wrist[2],
0,
1,
1,
alpha,
# scale.x is length, scale.y is arrow width, scale.z is arrow height
np.linalg.norm(tri_center - p_wrist),
0.002,
0,
marker_q,
duration, # Use 0 duration for forever
qw=q_wrist[3],
qx=q_wrist[0],
qy=q_wrist[1],
qz=q_wrist[2])
marker_arr.markers.append(marker_q)
# Plot a cyan arrow from average center of current triangle, to average
# center of object.
marker_out = Marker()
create_marker(
Marker.ARROW,
'tri_normal',
frame_id,
0,
0,
0,
0,
0,
1,
1,
alpha,
# scale.x is shaft diameter, scale.y is arrowhead diameter, scale.z is
# arrowhead length if non-zero
0.001,
0.002,
0,
marker_out,
duration) # Use 0 duration for forever
marker_out.points.append(Point(obj_center[0], obj_center[1],
obj_center[2]))
marker_out.points.append(Point(tri_center[0], tri_center[1],
tri_center[2]))
marker_arr.markers.append(marker_out)
# Plot current triangle, red
marker_tri_l = Marker()
create_marker(
Marker.LINE_STRIP,
'sample_tri',
frame_id,
0,
# scale.x is width of line
0,
0,
0,
1,
0,
0,
alpha,
0.001,
0,
0,
marker_tri_l,
duration) # Use 0 duration for forever
marker_tri_l.points.append(Point(pt0[0], pt0[1], pt0[2]))
marker_tri_l.points.append(Point(pt1[0], pt1[1], pt1[2]))
marker_tri_l.points.append(Point(pt2[0], pt2[1], pt2[2]))
marker_tri_l.points.append(Point(pt0[0], pt0[1], pt0[2]))
marker_arr.markers.append(marker_tri_l)
marker_tri_p = Marker()
create_marker(Marker.POINTS, 'sample_pts', frame_id, 0, 0, 0, 0, 1, 0, 0,
alpha, 0.005, 0.005, 0.005, marker_tri_p,
duration) # Use 0 duration for forever
marker_tri_p.points.append(Point(pt0[0], pt0[1], pt0[2]))
marker_tri_p.points.append(Point(pt1[0], pt1[1], pt1[2]))
marker_tri_p.points.append(Point(pt2[0], pt2[1], pt2[2]))
marker_arr.markers.append(marker_tri_p)
vis_arr_pub.publish(marker_arr)
# Pause a bit, to let msg get pushed through
rospy.sleep(0.1)
def cbLanePose(self, lane_pose_msg):
marker_array = MarkerArray()
# rospy.loginfo("[%s] cbLanePose." %(self.node_name))
marker_array.markers.append(self.lanePose2Marker(lane_pose_msg))
self.pub_markers.publish(marker_array)
def pubMarker(self):
navigation_goals = rospy.get_param("/script_server/base", {})
markerarray = MarkerArray()
i = 0
for name, pose in list(navigation_goals.items()):
# check if pose is valid
if len(pose) != 3:
continue
# arrow
marker_arrow = Marker()
marker_arrow.header.stamp = rospy.Time.now()
marker_arrow.header.frame_id = "map"
marker_arrow.ns = "/" + name
marker_arrow.id = i
marker_arrow.type = Marker.ARROW
marker_arrow.action = Marker.ADD
marker_arrow.scale.x = 1.0
marker_arrow.scale.y = 0.1
marker_arrow.scale.z = 0.1
marker_arrow.color.r = 0.0
marker_arrow.color.g = 0.0
marker_arrow.color.b = 1.0
marker_arrow.color.a = 1.0
marker_arrow.pose.position.x = pose[0]
marker_arrow.pose.position.y = pose[1]
marker_arrow.pose.position.z = 0.2
quaternion = tf.transformations.quaternion_from_euler(
0, 0, pose[2])
marker_arrow.pose.orientation.x = quaternion[0]
marker_arrow.pose.orientation.y = quaternion[1]
marker_arrow.pose.orientation.z = quaternion[2]
marker_arrow.pose.orientation.w = quaternion[3]
markerarray.markers.append(marker_arrow)
# text
marker_text = Marker()
marker_text.header.stamp = rospy.Time.now()
marker_text.header.frame_id = "map"
marker_text.ns = "/" + name
marker_text.id = i + 1000000
marker_text.type = Marker.TEXT_VIEW_FACING
marker_text.action = Marker.ADD
marker_text.scale.z = self.text_size
marker_text.color.r = 0.0
marker_text.color.g = 0.0
marker_text.color.b = 1.0
marker_text.color.a = 1.0
marker_text.pose.position.x = pose[0]
marker_text.pose.position.y = pose[1] + 0.2
marker_text.pose.position.z = 0.2
quaternion = tf.transformations.quaternion_from_euler(
0, 0, pose[2])
marker_text.pose.orientation.x = quaternion[0]
marker_text.pose.orientation.y = quaternion[1]
marker_text.pose.orientation.z = quaternion[2]
marker_text.pose.orientation.w = quaternion[3]
marker_text.text = name
markerarray.markers.append(marker_text)
i = i + 1
self.pubGoals.publish(markerarray)
def find_blocks(self):
cv_image = self.cv_image
block_marker_list = MarkerArray()
ray_marker_list = MarkerArray()
inv_block_obs_list = []
ws_block_obs_list = []
block_pixel_locs_list = []
# Enables tuning of HSV thresholds for different lighting conditions
height, width, depth = cv_image.shape
pad_size = 5
if (self.camera == "top"):
# Remove table from hsv image
hsv = remove_table(cv_image)
else:
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
images = [hsv, hsv, hsv, hsv, hsv]
i = 0
ray_id = 0
# Find table
if (self.camera == "right_hand"):
if (self.ir_reading is not None):
area_min_threshold = 400 / self.ir_reading
area_max_threshold = 100000 # TODO: tune
else:
area_min_threshold = 400 / 0.4
area_max_threshold = 100000 # TODO: tune
elif (self.camera == "top"):
area_min_threshold = 40
area_max_threshold = 1000
for color in colors:
low_h = colors[color]["low_h"]
high_h = colors[color]["high_h"]
low_s = colors[color]["low_s"]
high_s = colors[color]["high_s"]
low_v = colors[color]["low_v"]
high_v = colors[color]["high_v"]
# Converting image to HSV format
if color == "red":
hsv_mask_1 = cv2.inRange(hsv,
np.array([low_h[0], low_s, low_v]),
np.array([high_h[0], high_s, high_v]))
hsv_mask_2 = cv2.inRange(hsv,
np.array([low_h[1], low_s, low_v]),
np.array([high_h[1], high_s, high_v]))
hsv_mask = hsv_mask_1 | hsv_mask_2
else:
hsv_mask = cv2.inRange(hsv, np.array([low_h, low_s, low_v]),
np.array([high_h, high_s, high_v]))
# Apply mask to original image
masked_img = cv2.bitwise_and(cv_image, cv_image, mask=hsv_mask)
# Store HSV masked image for the current color
self.seg_img[color] = masked_img.copy()
# cv2.imshow(color, masked_img)
# Morphological opening (remove small objects from the foreground)
erode_1 = cv2.erode(hsv_mask,
np.ones((5, 5), np.uint8),
iterations=1)
dilate_1 = cv2.dilate(erode_1,
np.ones((5, 5), np.uint8),
iterations=1)
# Morphological closing (fill small holes in the foreground)
dilate_2 = cv2.dilate(dilate_1,
np.ones((10, 10), np.uint8),
iterations=1)
erode_2 = cv2.erode(dilate_2,
np.ones((10, 10), np.uint8),
iterations=1)
images[i] = erode_2.copy()
ret, thresh = cv2.threshold(erode_2, 157, 255, 0)
if (OPENCV3):
im2, contours, hierarchy = cv2.findContours(
thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
else:
contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_NONE)
# Draw the countours.
# cv2.drawContours(cv_image, contours, -1, (0,255,0), 3)
if (self.camera == "right_hand"):
if (RES_640x400):
# At 0 Meters
cv2.circle(cv_image, (325, 129), 5, (255, 0, 255), 1)
# At -0.15 Meters
cv2.circle(cv_image, (330, 94), 5, (255, 255, 0), 1)
elif (RES_1280x800):
# At 0 Meters
cv2.circle(cv_image, (645, 329), 5, (255, 0, 255), 1)
# At -0.15 Meters
cv2.circle(cv_image, (650, 294), 5, (255, 255, 0), 1)
# At -0.16 Meters
cv2.circle(cv_image, (660, 285), 5, (0, 255, 255), 1)
# rospy.loginfo("Found %d %s objects", num_obj, color)
for contour in contours:
area = cv2.contourArea(contour)
if (area > area_min_threshold and area < area_max_threshold):
x, y, w, h = cv2.boundingRect(contour)
rect = cv2.minAreaRect(contour)
# rospy.loginfo("AREA: %f", area)
if (OPENCV3):
box = cv2.boxPoints(rect)
else:
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
# Pad the outside of cropped image so that images edges
# can still effect PCA
x_padded_min = x - pad_size
x_padded_max = x + w + pad_size
y_padded_min = y - pad_size
y_padded_max = y + h + pad_size
if x_padded_min < 0:
x_padded_min = 0
if y_padded_min < 0:
y_padded_min = 0
if x_padded_max > width:
x_padded_max = width
if y_padded_max > height:
y_padded_max = height
cropped_img = masked_img[y_padded_min:y_padded_max,
x_padded_min:x_padded_max]
if (self.camera == "top"):
#cropped_img = np.flip(cropped_img, 0)
#block_angle = 0
block_angle = calc_angle(cropped_img) + math.pi / 2
else:
block_angle = calc_angle(cropped_img)
# block_angle = rect[2]
rospy.loginfo("Block angle is %f", block_angle)
# TODO: Is this the same?
# block_angle = rect[2]
rospy.loginfo("Block angle is %f", block_angle)
# block_ratio = calc_ratio(rect[1][1], rect[1][0])
block_length, block_width = calc_block_type(
rect[1][1], rect[1][0], self.camera)
moms = cv2.moments(contour)
if (moms['m00'] > area_min_threshold
and moms['m00'] < area_max_threshold):
cx = int(moms['m10'] / moms['m00'])
cy = int(moms['m01'] / moms['m00'])
if (self.camera == "top"):
# Block should not be outside of circle centered at 319,255 with radius 200
d = math.sqrt((cx - 319)**2 + (cy - 255)**2)
if (d > 198):
continue
# cx = cx - self.camera_model.cx()
# cy = cy - self.camera_model.cy()
print("Color: ", color, " Box: ", box)
cv2.drawContours(cv_image, [box], 0, color_vals[color],
1)
# Outlines for Barbell Task
"""
cv2.rectangle(cv_image, (216 - 5, 251 + 5), (244 + 5,238 - 5), (0,0,0), 1)
cv2.rectangle(cv_image, (224 - 5, 301 + 5), (238 + 5,253 - 5), (0,0,0), 1)
cv2.rectangle(cv_image, (218 - 5, 316 + 5), (244 + 5,303 - 5), (0,0,0), 1)
"""
cv2.circle(cv_image, (cx, cy), 3, color_vals[color], 1)
# Write the block tshape
font = cv2.FONT_HERSHEY_SIMPLEX
if (self.camera == "top"):
font_size = 0.5
font_thickness = 1
else:
font_size = 3.0
font_thickness = 2
cv2.putText(
cv_image,
block_type_string(block_length,
block_width), (cx, cy), font,
font_size, color_vals[color], font_thickness)
self.pixel_loc = [cx, cy]
if (self.camera == "right_hand"):
vec = np.array(
self.hand_camera_model.projectPixelTo3dRay(
(cx, cy)))
elif (self.camera == "top"):
vec = np.array(
self.top_camera_model.projectPixelTo3dRay(
(cx, cy)))
new_vec = vec.copy()
new_vec[0] = vec[2]
new_vec[1] = -vec[0]
new_vec[2] = -vec[1]
vec = new_vec.copy()
if (in_workspace((cx, cy))):
cv2.putText(cv_image, "WS", (cx + 10, cy + 10),
font, font_size, color_vals[color],
font_thickness)
else:
cv2.putText(cv_image, "INV",
(cx + 10, cy + 10), font,
font_size, color_vals[color],
font_thickness)
else:
# Invalid camera name
rospy.loginfo("%s is an invalid camera!",
self.camera)
return
# If we're using the hand camera, make sure we have a valid IR reading...
if (self.camera == "top"
or (self.camera == "right_hand"
and self.ir_reading is not None)):
if (self.camera == "right_hand"):
d = self.ir_reading + 0.15
elif (self.camera == "top"):
d = self.top_cam_table_dist
ray_pt_1 = np.array([0, 0, 0])
ray_pt_2 = 2 * vec
# rospy.loginfo("Vec: %f, %f, %f", vec[0], vec[1], vec[2])
d_cam = d * vec
homog_d_cam = np.concatenate(
(d_cam, np.ones(1))).reshape((4, 1))
homog_ray_pt_1 = np.concatenate(
(ray_pt_1, np.ones(1))).reshape((4, 1))
homog_ray_pt_2 = np.concatenate(
(ray_pt_2, np.ones(1))).reshape((4, 1))
if (self.camera == "top"):
camera_to_base = self.top_to_base_mat
"""
self.tf_listener.waitForTransform(
'/base', self.camera + "_camera", rospy.Time(), rospy.Duration(4))
(trans, rot) = self.tf_listener.lookupTransform(
'/base', self.camera + "_camera", rospy.Time())
"""
else:
# Wait for transformation from base to camera as this change as the hand_camera moves
self.tf_listener.waitForTransform(
'/base', self.camera + "_camera",
rospy.Time(), rospy.Duration(4))
(trans,
rot) = self.tf_listener.lookupTransform(
'/base', self.camera + "_camera",
rospy.Time())
camera_to_base = tf.transformations.compose_matrix(
translate=trans,
angles=tf.transformations.
euler_from_quaternion(rot))
block_position_arr = np.dot(
camera_to_base, homog_d_cam)
# Create a ray from the camera to the detected block
# Transform ray to base frame
ray_pt_1_tf = np.dot(camera_to_base,
homog_ray_pt_1)
ray_pt_2_tf = np.dot(camera_to_base,
homog_ray_pt_2)
ray_marker_list.markers.append(
create_ray_marker(
frame="base",
id=ray_id,
point1=Point(ray_pt_1_tf[0],
ray_pt_1_tf[1],
ray_pt_1_tf[2]),
point2=Point(ray_pt_2_tf[0],
ray_pt_2_tf[1],
ray_pt_2_tf[2]),
ray_color=color,
transparency=self.transparency))
ray_id += 1
# rospy.loginfo("Block position: %f, %f, %f", block_position_arr[0], block_position_arr[1], block_position_arr[2])
# rospy.loginfo("Block angle: %f", math.degrees(block_angle))
block_position_p = Point()
block_position_arr_copy = block_position_arr.copy()
block_position_p.x = block_position_arr_copy[0]
block_position_p.y = block_position_arr_copy[1]
block_position_p.z = -.1
# TODO: double check that the angle is correct (What if camera rotates?)
# Rotation about z-axis
block_orientation_arr = tf.transformations.quaternion_from_euler(
0, 0, block_angle)
block_orientation = Quaternion()
block_orientation.x = block_orientation_arr[0]
block_orientation.y = block_orientation_arr[1]
block_orientation.z = block_orientation_arr[2]
block_orientation.w = block_orientation_arr[3]
"""
if(self.camera == "top"):
block_angle += (1.34 - math.pi/2)
rot_quat = tf.transformations.quaternion_multiply(
rot, block_orientation)
block_angle = tf.transformations.quaternion_from_euler(
rot_quat)
"""
# Create a marker to visualize in RVIZ
curr_marker = create_block_marker(
frame="base",
id=len(block_marker_list.markers),
position=block_position_p,
orientation=block_orientation,
length=block_length,
width=block_width,
block_color=color,
transparency=self.transparency)
rospy.loginfo("Adding new marker")
block_marker_list.markers.append(curr_marker)
#block_pose_list.append(Pose(position=block_position_p, orientation=block_orientation))
# TODO: The block angle will still be wrong. Need to transform it from the camera coordinate to the world frame
if (in_workspace((cx, cy))):
ws_block_obs_list.append(
BlockObservation(pose=Pose2D(
x=block_position_p.x,
y=block_position_p.y,
theta=block_angle),
color=color,
length=block_length,
width=block_width))
else:
inv_block_obs_list.append(
BlockObservation(pose=Pose2D(
x=block_position_p.x,
y=block_position_p.y,
theta=block_angle),
color=color,
length=block_length,
width=block_width))
block_pixel_locs_list.append(
BlockPixelLoc(x=cx,
y=cy,
theta=block_angle,
color=color,
length=block_length,
width=block_width))
else:
# rospy.loginfo("No ir_data has been recieved yet!")
pass
else:
# rospy.loginfo("Moments aren't large enough!")
pass
else:
pass
# rospy.loginfo("Contour area is not large enough!")
self.rect_seg_img = cv_image.copy()
self.ray_markers = ray_marker_list
self.block_markers = block_marker_list
self.inv_block_obs = inv_block_obs_list
self.ws_block_obs = ws_block_obs_list
self.block_pixel_locs = block_pixel_locs_list
self.inv_detected_blocks = len(inv_block_obs_list)
self.ws_detected_blocks = len(ws_block_obs_list)
def draw(self):
markerArray = MarkerArray()
for m in self.markers:
markerArray.markers.append(m)
self.pub.publish(markerArray)
def clear(self):
markerArray = MarkerArray()
for m in self.markers:
m.action = m.DELETE
markerArray.markers.append(m)
self.pub.publish(markerArray)
p1 = Pose2D()
p1.x = footsteps_x[i] + p.x
p1.y = footsteps_y[i]
p1.theta = footsteps_theta[i]
m1 = footPoseToMarker(p1, strings.left)
m1.id = id +1
markers.markers.append(m1)
if __name__ == '__main__':
rospy.init_node('viz_footsteps', anonymous=True)
#rospy.Subscriber(strings.pose_topic, PoseWithCovarianceStamped, pose_callback)
pub = rospy.Publisher(pub_footsteps_topic, MarkerArray, queue_size=10)
Hz = 1
rate = rospy.Rate(Hz) # 10hz
size = len(footsteps_x)
print(size)
while not rospy.is_shutdown():
id = 0
markers = MarkerArray()
for i in range(0, size):
getMarker(markers,i,id)
id = id +2
pub.publish(markers)
rate.sleep()
rospy.spin()
def readSensor(data):
global graphe, markerstopub
global x, y, th, A, ccxyth, xyth, xythcuant, path
markerstopub = MarkerArray()
#markerstopub= MarkerArray()
lec = np.asarray(data.ranges)
lec[np.isinf(lec)] = 13.5
ccxyth = np.load('ccxyth.npy')
A = np.load('A.npy')
graphe = Markov_A_2_grafo(A, ccxyth)
xyth = np.asarray((x, y, th))
_, xythcuant = quantized(xyth, ccxyth)
"""if (graphe.conec[xythcuant,path[0]] == np.inf):
print("Recalculating Dijsktra")
path=dijkstra(xythcuant,xythclcuant,graphe)
"""
Fx, Fy, Fth = 0.001, 0.001, 0
deltaang = 4.7124 / len(data.ranges)
laserdegs = np.arange(-2.3562, 2.3562, deltaang)
Fx = 0
Fy = 0.001
for i, deg in enumerate(laserdegs):
if (lec[i] < 2.61):
Fx = Fx + (1 / lec[i])**2 * np.cos(deg)
Fy = Fy + (1 / lec[i])**2 * np.sin(deg)
Fth = np.arctan2(Fy, (Fx + .000000000001)) + np.pi
Fmag = np.linalg.norm((Fx, Fy))
if (len(path) > 0):
markerstopub = list_2_markers_array(path, ccxyth)
pub3.publish(markerstopub)
x_nxt, y_nxt, th_nxt = ccxyth[path[0]]
xyth_nxt = np.array((x_nxt, y_nxt, th_nxt))
_, xyth_nxt_cuant = quantized(xyth_nxt, ccxyth)
euclD = np.linalg.norm(xyth[:2] - xyth_nxt[:2])
print("path", path)
print('im in ', xyth, xythcuant)
print('nx to', x_nxt, y_nxt, xyth_nxt_cuant)
print('EuclD to dest.', euclD)
#
if (xythcuant in path[1:]):
killix = path.index(xythcuant)
print('SHortuct DETECTED', killix)
del path[:path.index(xythcuant)]
if (xythcuant == xyth_nxt_cuant or euclD <= .5):
print('path node chekced ')
markerstopub = list_2_markers_array(path, ccxyth, True)
pub3.publish(markerstopub)
path.pop(0)
nxt_pnt.point.x = ccxyth[np.int(path[0]), 0]
nxt_pnt.point.y = ccxyth[np.int(path[0]), 1]
print('Nxt X, NXt Y', nxt_pnt.point.x, nxt_pnt.point.y)
if (len(path) == 0):
x_nxt, ynxt = xcl, ycl
euclD = np.linalg.norm(xyth[:2] - xyth_nxt[:2])
Fatrx = (-x + x_nxt) / euclD
Fatry = (-y + y_nxt) / euclD
Fatrth = np.arctan2(Fatry, Fatrx)
Fatrth = Fatrth - th
Fmagat = np.linalg.norm((Fatrx, Fatry))
#print ('Fatx, Fatry, Fatrth',Fatrx,Fatry,(Fatrth)*180/np.pi )
Ftotx = Fmag * np.cos(Fth) * .0051 + Fmagat * np.cos(Fatrth)
Ftoty = Fmag * np.sin(Fth) * .0051 + Fmagat * np.sin(Fatrth)
Ftotth = np.arctan2(Ftoty, Ftotx)
if (Ftotth > np.pi):
Ftotth = -np.pi - (Ftotth - np.pi)
if (Ftotth < -np.pi):
Ftotth = (Ftotth + 2 * np.pi)
#print('Ftotxy',Ftotx,Ftoty,Ftotth*180/np.pi)
"""Fatmag=np.linalg.norm((Fatrx,Fatry))
Fmag=np.linalg.norm((Fx,Fy))
print ("theta robot",th*180/3.1416,'---------------------------')
print ('fasorFatrth',np.linalg.norm((Fatrx,Fatry)),(Fatrth)*180/3.1416 )
print ("FXATR,FYATR",Fatrx,Fatry)
print ('fasorFrepth',np.linalg.norm((Fx,Fy)),Fth*180/3.1416)
print ("Frepx,Frepy",Fx,Fy)"""
"""Fx,Fy= Fmag*np.cos(Fth) , Fmag*np.sin(Fth)
Fatrx,Fatry= Fatmag*np.cos(Fatrth) , Fatmag*np.sin(Fatrth) """
vel = 0
if (abs(Ftotth) < .33): #or (np.linalg.norm((Fx,Fy)) < 100):
speed.linear.x = 5
print('lin')
speed.angular.z = 0
else:
if Ftotth < 0:
if (abs(Ftotth) < np.pi / 2):
vel = 3
print('open curve')
print('Vang-')
speed.linear.x = vel
speed.angular.z = -2
if Ftotth > 1.57:
print('ang--')
speed.linear.x = 0
speed.angular.z = -0.5
if Ftotth > 0:
if (abs(Ftotth) < np.pi / 2):
vel = 3
print('open curve')
print('Vang+')
speed.linear.x = vel
speed.angular.z = 2
if Ftotth < -1.57:
print('ang++')
speed.linear.x = 0
speed.angular.z = 0.5
if (graphe.conec[xythcuant, path[0]] == np.inf):
print(
'Route lost Recalculate Dijsktra#####################################3',
)
print('im in ', xyth, 'cc', xythcuant)
print('going to', x_nxt, y_nxt,
'xythcuant###########################3', xyth_nxt_cuant)
if (np.linalg.norm(np.array((xcl, ycl)) - np.array((x, y))) < .7):
print("sucess")
path = []
x_nxt, ynxt = xcl, ycl
def on_new_point_cloud(data):
topic = '/pointpillars/visualization/marker_box_25m'
topic2 = '/pointpillars/visualization/marker_box_10m'
marker_publisher = rospy.Publisher(topic, MarkerArray)
markerArray = MarkerArray()
del_marker = Marker()
del_marker.action = del_marker.DELETEALL
del_marker.header.frame_id = "visualization"
del_marker.id = 1
markerArray.markers.append(del_marker)
marker2_publisher = rospy.Publisher(topic2, MarkerArray)
markerArray2 = MarkerArray()
del_marker2 = Marker()
del_marker2.action = del_marker2.DELETEALL
del_marker2.header.frame_id = "visualization"
del_marker2.id = 1
markerArray2.markers.append(del_marker2)
print(len(data.markers))
for i in range(len(data.markers)):
print(data.markers[i].pose.position.x)
#print(labels[i])
if 2.5 > math.sqrt(data.markers[i].pose.position.x *
data.markers[i].pose.position.x +
data.markers[i].pose.position.y *
data.markers[i].pose.position.y):
# if labels[i] == 0:
#result_string = str(labels[i])+','+str(scores[i])+','+str(box_preds_lidar[i][0])+ ','+ str(box_preds_lidar[i][1]) + ','+ str(math.sqrt(box_preds_lidar[i][1]*box_preds_lidar[i][1]+ box_preds_lidar[i][0]*box_preds_lidar[i][0])) +','+ str(box_preds_lidar[i][6])+'\n'
# with open('./20191216xyz16_20m.csv','a') as result_file:
# result_file.write(result_string)
marker = Marker()
marker.header.frame_id = "visualization"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.scale.x = data.markers[i].scale.x
marker.scale.y = data.markers[i].scale.y
marker.scale.z = data.markers[i].scale.z
marker.color.a = 0.75
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = data.markers[i].pose.orientation.z
marker.pose.orientation.w = data.markers[i].pose.orientation.w
marker.pose.position.x = data.markers[i].pose.position.x
marker.pose.position.y = data.markers[i].pose.position.y
marker.pose.position.z = data.markers[i].pose.position.z
marker.id = 10 + i
marker.lifetime = rospy.Duration()
markerArray.markers.append(marker)
elif 10 > math.sqrt(data.markers[i].pose.position.x *
data.markers[i].pose.position.x +
data.markers[i].pose.position.y *
data.markers[i].pose.position.y):
marker2 = Marker()
marker2.header.frame_id = "visualization"
marker2.type = marker.CUBE
marker2.action = marker.ADD
marker2.scale.x = data.markers[i].scale.x
marker2.scale.y = data.markers[i].scale.y
marker2.scale.z = data.markers[i].scale.z
marker2.color.a = 0.75
marker2.color.r = 1.0
marker2.color.g = 1.0
marker2.color.b = 0.0
marker2.pose.orientation.x = 0
marker2.pose.orientation.y = 0
marker2.pose.orientation.z = data.markers[i].pose.orientation.z
marker2.pose.orientation.w = data.markers[i].pose.orientation.w
marker2.pose.position.x = data.markers[i].pose.position.x
marker2.pose.position.y = data.markers[i].pose.position.y
marker2.pose.position.z = data.markers[i].pose.position.z
marker2.id = 10 + i
marker2.lifetime = rospy.Duration()
markerArray2.markers.append(marker2)
marker_publisher.publish(markerArray)
marker2_publisher.publish(markerArray2)
def publish_3dbox(box3d_pub, corners_3d_velos, track_ids, object_types=None, publish_id=True, publish_distance=False, log=False):
"""
Publish 3d boxes in velodyne coordinate, with color specified by object_types
If object_types is None, set all color to cyan
corners_3d_velos : list of (8, 4) 3d corners
"""
marker_array = MarkerArray()
for i, corners_3d_velo in enumerate(corners_3d_velos):
track_id = track_ids[i]
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = track_id
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_LIST
if object_types is None:
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
else:
b, g, r = DETECTION_COLOR_MAP[object_types[i]]
marker.color.r = r/255.0
marker.color.g = g/255.0
marker.color.b = b/255.0
marker.color.a = 1.0
marker.scale.x = 0.1
marker.pose.position.x = 1
marker.pose.position.y = 1
marker.pose.position.z = 1
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.points = []
for l in LINES:
p1 = corners_3d_velo[l[0]]
marker.points.append(Point(p1[0], p1[1], p1[2]))
p2 = corners_3d_velo[l[1]]
marker.points.append(Point(p2[0], p2[1], p2[2]))
marker_array.markers.append(marker)
if publish_id:
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = track_id + 1000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
p4 = corners_3d_velo[4] # upper front left corner
text_marker.pose.position.x = p4[0]
text_marker.pose.position.y = p4[1]
text_marker.pose.position.z = p4[2] + 0.5
text_marker.text = str(track_id)
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
if object_types is None:
text_marker.color.r = 0.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
else:
b, g, r = DETECTION_COLOR_MAP[object_types[i]]
text_marker.color.r = r/255.0
text_marker.color.g = g/255.0
text_marker.color.b = b/255.0
text_marker.color.a = 1.0
marker_array.markers.append(text_marker)
if publish_distance:
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = track_id + 2000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
bc = (corners_3d_velo[6] + corners_3d_velo[7] + corners_3d_velo[2] + corners_3d_velo[3])/4 # back center
text_marker.pose.position.x = bc[0]
text_marker.pose.position.y = bc[1]
text_marker.pose.position.z = bc[2]
bcu = (corners_3d_velo[6][:3]+corners_3d_velo[7][:3])/2
bcu[0] -= 3.07 # the length of the head of the car
distance = np.sqrt(np.sum(bcu**2))
text_marker.text = '%.2f'%distance
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
if distance<4:
text_marker.color.r = 1.0
text_marker.color.g = 0.0
text_marker.color.b = 0.0
else:
text_marker.color.r = 0.0
text_marker.color.g = 1.0
text_marker.color.b = 0.5
text_marker.color.a = 1.0
marker_array.markers.append(text_marker)
box3d_pub.publish(marker_array)
if log:
rospy.loginfo("%d 3d boxes published"%len(corners_3d_velos))
class FastObstacle(object):
""" For fast obstacle avoidance.
"""
K_P, K_D = 0.3, 0
SPEED = 8.1 # Maybe there is a better way to determine speed?
# Or maybe this is fine since we need to limit speed a bit
# while handling and avoiding obstacles.
MIN_OBSTACLE_DIST = 14
WHEELBASE = 3.0
TRACK = 1.6
MAX_MARKERS = 10
prev_error = 0
marker_array = MarkerArray()
def __init__(self):
self.state_lock = Lock()
self.goal_pos = (-99.5728759766, -33.2555503845) # We need a goal pose to do
# obstacle avoidance. Let's assume that this is initialized
# in the form (x, y) from some irrelevant origin.
self.odom_pose = None # This is to know which gaps/safe paths to favor
# in order to reach goal_pos. -TODO
self.lidar_sub = rospy.Subscriber("/tesse/hood_lidar/scan", LaserScan, self.lidar_callback, queue_size=1)
self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.odom_callback)
self.drive_pub = rospy.Publisher("/tesse/drive", AckermannDriveStamped, queue_size=1)
self.marker_publisher = rospy.Publisher('markers', MarkerArray, queue_size=1)
# some callbacks in here..
def odom_callback(self, data):
self.state_lock.acquire()
self.odom_pose = data.pose
self.state_lock.release()
def lidar_callback(self, data):
if self.goal_pos is None or self.odom_pose is None:
return # We can't move without a goal or car position info
# get points in the range between -90 and 90 degrees in front of the car
rt_range = np.array([np.array([data.ranges[i], data.angle_min + i * data.angle_increment])
for i in range(len(data.ranges))
if (-np.pi/2) <= (data.angle_min + i * data.angle_increment) <= (np.pi/2)])
# Obstacle dilation to eliminate narrow gaps
bit_map = [int(p[0] < self.MIN_OBSTACLE_DIST) for p in rt_range]
prev_obst = -1
for i in range(len(bit_map)):
if bit_map[i] == 1:
prev_obst = i
continue
if prev_obst == -1:
continue
d_prev_obst = 2*rt_range[prev_obst][0]*np.sin((i-prev_obst)*data.angle_increment/2)
if d_prev_obst < self.TRACK: # probably needs tweaking. We only need half of
# the car track but we do extra dilation for safety.
bit_map[i] = 1 # dilate
prev_obst = -1
for i in range(len(bit_map)-1, -1, -1):
if bit_map[i] == 1:
prev_obst = i
continue
if prev_obst == -1:
continue
d_prev_obst = 2*rt_range[prev_obst][0]*np.sin((prev_obst-i)*data.angle_increment/2)
if d_prev_obst < self.TRACK:
bit_map[i] = 1
safe = [rt_range[i] for i in range(len(rt_range)) if bit_map[i] == 1]
# for group in obstacles:
# if len(group) > 4:
# xy_range = np.array([np.array([p[0] * np.cos(p[1]), p[0] * np.sin(p[1])]) for p in group])
# self.make_marker(xy_range)
car_pos = self.odom_pose.pose.position
car_orientation = tf.transformations.euler_from_quaternion((self.odom_pose.pose.orientation.x,
self.odom_pose.pose.orientation.y,
self.odom_pose.pose.orientation.z,
self.odom_pose.pose.orientation.w))
orientation = car_orientation[2]
if orientation > 0:
orientation -= np.pi
else:
orientation += np.pi
ideal_theta = (orientation - np.arctan2(car_pos.y - self.goal_pos[1],
car_pos.x - self.goal_pos[0]))
best_gap, heuristic = [], np.inf
for gap in safe:
g_theta = ideal_theta - gap[1]
if abs(g_theta) < heuristic:
best_gap = gap
heuristic = abs(g_theta)
if best_gap == []:
steering_angle = ideal_theta
else:
# print("ideal " + str(ideal_theta))
# print("best " + str( best_gap[1]))
target_angle = best_gap[1]
derivative = target_angle - self.prev_error
self.prev_error = target_angle
steering_angle = (self.K_P * target_angle + self.K_D * derivative)
# steering_angle = ideal_theta
# steering_angle = self.wall_checker(data, steering_angle)
print(steering_angle)
# Publish drive command
ack_msg = AckermannDriveStamped()
ack_msg.header.stamp = rospy.Time.now()
ack_msg.drive.steering_angle = steering_angle
ack_msg.drive.speed = self.SPEED
self.drive_pub.publish(ack_msg)
def wall_checker(self, data, str_angle):
# Check 3 points directly in front of the robot (theta=0)
mid_index = int((len(data.ranges) - 1) / 2.0)
dist_sum = 0
for i in range(-3, 4):
dist_sum += data.ranges[mid_index + i]
avg_dist = dist_sum/3
# avg_dist_to_front_wall = (data.ranges[mid_index - 1] + data.ranges[mid_index] + data.ranges[mid_index + 1]) / 3.0
new_str_angle = str_angle
# The car needs ~0.62m to turn. Adding 0.7 for safety.
if (avg_dist < self.MIN_OBSTACLE_DIST):
print('obstacle detected')
new_str_angle = -np.pi / 2
return new_str_angle
def make_marker(self, points):
marker = Marker()
marker.header.frame_id = "hood_lidar"
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.lifetime = rospy.Duration(3)
# marker scale
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
# marker color
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
# marker orientaiton
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 0
# marker position
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
# marker line points
marker_points = []
for point in points:
marker_point = Point()
marker_point.x = point[0]
marker_point.y = point[1]
marker_points.append(marker_point)
marker.points = marker_points
# Publish the Marker
if (len(self.marker_array.markers) > self.MAX_MARKERS):
self.marker_array.markers.pop(0)
self.marker_array.markers.append(marker)
id = 0
for m in self.marker_array.markers:
m.id = id
id += 1
self.marker_publisher.publish(self.marker_array)
def avoid_obstacle(self):
pass
def on_new_point_cloud2(data):
topic = '/pointpillars/visualization/marker_box2_25m'
topic2 = '/pointpillars/visualization/marker_box2_10m'
marker_publisher = rospy.Publisher(topic, MarkerArray)
markerArray = MarkerArray()
del_marker = Marker()
del_marker.action = del_marker.DELETEALL
del_marker.header.frame_id = "visualization"
del_marker.id = 1
markerArray.markers.append(del_marker)
marker2_publisher = rospy.Publisher(topic2, MarkerArray)
markerArray2 = MarkerArray()
del_marker2 = Marker()
del_marker2.action = del_marker2.DELETEALL
del_marker2.header.frame_id = "visualization"
del_marker2.id = 1
markerArray2.markers.append(del_marker2)
print(len(data.markers))
for i in range(len(data.markers)):
#print(labels[i])
if 2.5 > math.sqrt(data.markers[i].pose.position.x *
data.markers[i].pose.position.x +
data.markers[i].pose.position.y *
data.markers[i].pose.position.y):
marker = Marker()
marker.header.frame_id = "visualization"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.scale.x = data.markers[i].scale.x
marker.scale.y = data.markers[i].scale.y
marker.scale.z = data.markers[i].scale.z
marker.color.a = 0.75
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = data.markers[i].pose.orientation.z
marker.pose.orientation.w = data.markers[i].pose.orientation.w
marker.pose.position.x = data.markers[i].pose.position.x
marker.pose.position.y = data.markers[i].pose.position.y
marker.pose.position.z = data.markers[i].pose.position.z
marker.id = 10 + i
marker.lifetime = rospy.Duration()
markerArray.markers.append(marker)
elif 10 > math.sqrt(data.markers[i].pose.position.x *
data.markers[i].pose.position.x +
data.markers[i].pose.position.y *
data.markers[i].pose.position.y):
marker2 = Marker()
marker2.header.frame_id = "visualization"
marker2.type = marker.CUBE
marker2.action = marker.ADD
marker2.scale.x = data.markers[i].scale.x
marker2.scale.y = data.markers[i].scale.y
marker2.scale.z = data.markers[i].scale.z
marker2.color.a = 0.75
marker2.color.r = 0.0
marker2.color.g = 1.0
marker2.color.b = 1.0
marker2.pose.orientation.x = 0
marker2.pose.orientation.y = 0
marker2.pose.orientation.z = data.markers[i].pose.orientation.z
marker2.pose.orientation.w = data.markers[i].pose.orientation.w
marker2.pose.position.x = data.markers[i].pose.position.x
marker2.pose.position.y = data.markers[i].pose.position.y
marker2.pose.position.z = data.markers[i].pose.position.z
marker2.id = 10 + i
marker2.lifetime = rospy.Duration()
markerArray2.markers.append(marker2)
marker_publisher.publish(markerArray)
marker2_publisher.publish(markerArray2)
def cb_timer_obstacle(self, event):
if self.is_initialized() and self.is_lane_valid():
try:
# TF
self.trans_sensor_global = self.tf_buffer.lookup_transform(
self.global_frame, self.sensor_frame, rospy.Time())
mon_state = self.obstacle_monitor_state
alive_obstacles = set()
for o in self.obstacle_monitor_state.obstacles.objects:
# To perform transformation, we define a stamped type
o_point = PointStamped()
o_point.point.x = o.pose.position.x
o_point.point.y = o.pose.position.y
o_point.point.z = o.pose.position.z
position_obstacle_base = tf2_geometry_msgs.do_transform_point(
o_point, self.trans_sensor_base).point
raw_pose = PointStamped()
raw_pose.header.frame_id = self.sensor_frame
raw_pose.point.x = o.pose.position.x
raw_pose.point.y = o.pose.position.y
raw_pose.point.z = o.pose.position.z
position_obstacle_global = tf2_geometry_msgs.do_transform_point(
raw_pose, self.trans_sensor_global).point
# STEP 0: Check if object has been already detected
match = False
for i, ob in enumerate(
self.obstacle_monitor_state.detected_obstacles):
if (ob.position_obstacle_global.x - position_obstacle_global.x) < 0.5 and \
(ob.position_obstacle_global.y - position_obstacle_global.y) < 0.5:
match = True
alive_obstacles.add(i)
ob.position_obstacle_global = position_obstacle_global
ob.ttl += 1
if ob.ttl > int(self.hz) * 2:
ob.ttl = int(self.hz * 2)
ob.position_obstacle_base = position_obstacle_base
if not match:
# STEP 1: Search for the closest waypoint of the center
mini, mind = mon_state.closest_waypoint, np.inf
# TODO: check this
for i in range(
mon_state.closest_waypoint,
len(mon_state.global_waypoints.waypoints) - 1):
# Object that are way far from a waypoint are not of interest
dx = mon_state.global_waypoints.waypoints[
i].pose.pose.position.x - position_obstacle_global.x
dy = mon_state.global_waypoints.waypoints[
i].pose.pose.position.y - position_obstacle_global.y
d = np.sqrt(dx**2 + dy**2)
if d < 2.0:
if mind > d:
mini, mind = i, d
#detected_obstacles.append((o, d, object_closest_waypoint, position_obstacle_global))
object_closest_waypoint = mini
d_lin = linear_distance(
mon_state.global_waypoints.waypoints[mini].pose.
pose.position.x, mon_state.global_waypoints.
waypoints[mini].pose.pose.position.y,
mon_state.global_waypoints.waypoints[
mini + 1].pose.pose.position.x,
mon_state.global_waypoints.waypoints[
mini + 1].pose.pose.position.y,
position_obstacle_global.x,
position_obstacle_global.y)
if d_lin < self.safe_threshold:
self.obstacle_monitor_state.detected_obstacles.append(
#[o, d_lin, object_closest_waypoint, position_obstacle_global, 1,
# mind, position_obstacle_base, 0.0, 0.0]
ObstacleProposal(o, d_lin,
object_closest_waypoint,
position_obstacle_global, 1,
mind, position_obstacle_base,
0.0, 0.0))
# Visualization TODO: migrate to visualization component
marker_visualization = MarkerArray()
delete_marker = Marker()
delete_marker.action = Marker.DELETEALL
marker_visualization.markers.append(delete_marker)
self.visualization_detector.pub_viz_detected_obstacle_marker.publish(
marker_visualization)
marker_visualization.markers = []
# STEP 1.1: Set alive obstacles
for o in self.obstacle_monitor_state.detected_obstacles:
if o.ttl > self.hz:
match = False
for alive_ob in self.obstacle_monitor_state.alive_obstacles:
#if alive_ob[3].x - o[3].x < 0.5 and alive_ob[3].y - o[3].y < 0.5:
if alive_ob.position_obstacle_global.x - o.position_obstacle_global.x < 0.5 \
and alive_ob.position_obstacle_global.y - o.position_obstacle_global.y < 0.5:
# Update
alive_ob.position_obstacle_global = o.position_obstacle_global
alive_ob.object_closest_waypoint = o.object_closest_waypoint
#
match = True
if not match:
self.obstacle_monitor_state.alive_obstacles.append(
o)
# STEP 2: get distance of points
for i, o in enumerate(
self.obstacle_monitor_state.alive_obstacles):
leftmost_point_distance = np.inf
rightmost_point_distance = -np.inf
object_closest_waypoint = o.object_closest_waypoint
# For visualization purposes
left_point = None
right_point = None
#
for cp in o.obstacle.convex_hull.polygon.points:
raw_point_position = PointStamped()
raw_point_position.point.x = cp.x
raw_point_position.point.y = cp.y
position_hull_point = tf2_geometry_msgs.do_transform_point(
raw_point_position, self.trans_sensor_global).point
d = signed_linear_distance(
mon_state.global_waypoints.waypoints[
object_closest_waypoint].pose.pose.position.x,
mon_state.global_waypoints.waypoints[
object_closest_waypoint].pose.pose.position.y,
mon_state.global_waypoints.waypoints[
object_closest_waypoint +
1].pose.pose.position.x,
mon_state.global_waypoints.waypoints[
object_closest_waypoint +
1].pose.pose.position.y, position_hull_point.x,
position_hull_point.y)
if leftmost_point_distance > d:
leftmost_point_distance = d
left_point = position_hull_point
if rightmost_point_distance < d:
rightmost_point_distance = d
right_point = position_hull_point
o.d_left = leftmost_point_distance
o.d_right = rightmost_point_distance
#
# VISUALIZATION
#
if self.visualize:
m = Marker()
m.header.frame_id = self.global_frame
m.pose.orientation.w = 1.0
m.ns = "obstacle"
m.pose.position = o.position_obstacle_global
m.type = Marker.SPHERE
m.color.r = 1.0
m.color.a = 1.0
m.scale.x = 1.0
m.scale.y = 1.0
m.scale.z = 1.0
m.id = i
marker_visualization.markers.append(m)
#
"""
m_right_point = Marker()
m_right_point.pose.orientation.w = 1.0
m_right_point.type = Marker.SPHERE
m_right_point.header.frame_id = self.global_frame
m_right_point.ns = "right_points"
m_right_point.pose.position = right_point
m_right_point.color.r = 1.0
m_right_point.color.a = 1.0
m_right_point.scale.x = 0.5
m_right_point.scale.y = 0.5
m_right_point.scale.z = 0.5
m_right_point.id = i
marker_visualization.markers.append(m_right_point)
# Visualize borders
m_left_point = Marker()
m_left_point.pose.orientation.w = 1.0
m_left_point.header.frame_id = self.global_frame
m_left_point.ns = "left_points"
m_left_point.type = Marker.SPHERE
m_left_point.pose.position = left_point
m_left_point.color.r = 1.0
m_left_point.color.a = 1.0
m_left_point.scale.x = 0.5
m_left_point.scale.y = 0.5
m_left_point.scale.z = 0.5
m_left_point.id = i
marker_visualization.markers.append(m_left_point)
"""
# Visualize closest waypoint
m_closest_waypoint = Marker()
m_closest_waypoint.pose.orientation.w = 1.0
m_closest_waypoint.header.frame_id = self.global_frame
m_closest_waypoint.ns = "closest_waypoint_1"
m_closest_waypoint.type = Marker.SPHERE
m_closest_waypoint.pose.position = self.obstacle_monitor_state.global_waypoints.waypoints[
o.object_closest_waypoint].pose.pose.position
m_closest_waypoint.color.r = 1.0
m_closest_waypoint.color.g = 0.7
m_closest_waypoint.color.a = 1.0
m_closest_waypoint.scale.x = 0.7
m_closest_waypoint.scale.y = 0.7
m_closest_waypoint.scale.z = 0.7
m_closest_waypoint.id = i
marker_visualization.markers.append(m_closest_waypoint)
# TODO: waypoint
m_closest_waypoint = Marker()
m_closest_waypoint.pose.orientation.w = 1.0
m_closest_waypoint.header.frame_id = self.global_frame
m_closest_waypoint.ns = "closest_waypoint_next"
m_closest_waypoint.type = Marker.SPHERE
m_closest_waypoint.pose.position = self.obstacle_monitor_state.global_waypoints.waypoints[
o.object_closest_waypoint + 1].pose.pose.position
m_closest_waypoint.color.r = 1.0
m_closest_waypoint.color.g = 0.7
m_closest_waypoint.color.a = 1.0
m_closest_waypoint.scale.x = 0.7
m_closest_waypoint.scale.y = 0.7
m_closest_waypoint.scale.z = 0.7
m_closest_waypoint.id = i
marker_visualization.markers.append(m_closest_waypoint)
# previous waypoint
m_closest_waypoint = Marker()
m_closest_waypoint.pose.orientation.w = 1.0
m_closest_waypoint.header.frame_id = self.global_frame
m_closest_waypoint.ns = "closest_waypoint_prev"
m_closest_waypoint.type = Marker.SPHERE
m_closest_waypoint.pose.position = self.obstacle_monitor_state.global_waypoints.waypoints[
o.object_closest_waypoint - 1].pose.pose.position
m_closest_waypoint.color.r = 1.0
m_closest_waypoint.color.g = 0.7
m_closest_waypoint.color.a = 1.0
m_closest_waypoint.scale.x = 0.7
m_closest_waypoint.scale.y = 0.7
m_closest_waypoint.scale.z = 0.7
m_closest_waypoint.id = i
marker_visualization.markers.append(m_closest_waypoint)
self.visualization_detector.pub_viz_detected_obstacle_marker.publish(
marker_visualization)
self.visualization_detector.publish_robot_safety_radius()
# STEP 3: manage obstacle list (decay, remove old)
for i, o in enumerate(
self.obstacle_monitor_state.detected_obstacles):
if o.ttl <= 0:
self.obstacle_monitor_state.detected_obstacles.remove(
o)
elif i not in alive_obstacles:
o.ttl -= 1
# STEP 4: publish alive objects
# STEP 5: check alive objects
for o in self.obstacle_monitor_state.alive_obstacles:
print(o.position_obstacle_base.x,
o.position_obstacle_base.y, o.d_min)
if self.obstacle_monitor_state.closest_waypoint > o.object_closest_waypoint + 5:
self.obstacle_monitor_state.alive_obstacles.remove(o)
print("----")
# STEP 6: publish alive objects
self.obstacle_monitor_state.msg_alive_obstacles.obstacles = []
for o in self.obstacle_monitor_state.alive_obstacles:
# Setup message
msg_ob = Obstacle()
msg_ob.closest_waypoint = o.object_closest_waypoint
msg_ob.pose.position = o.position_obstacle_base
msg_ob.global_pose.position = o.position_obstacle_global
msg_ob.obstacle_type = Obstacle.STATIC_OBSTACLE
msg_ob.linear_distance = o.d_min
msg_ob.leftmost_distance = o.d_left
msg_ob.rightmost_distance = o.d_right
self.obstacle_monitor_state.msg_alive_obstacles.obstacles.append(
msg_ob)
# Publish
self.pub_detected_obstacles.publish(
self.obstacle_monitor_state.msg_alive_obstacles)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logerr("TF error")
except IndexError:
rospy.logerr("End or beinning of line")
self.obstacle_monitor_state.detected_obstacles = []
self.obstacle_monitor_state.alive_obstacles = []
import csv
import pdb
dirname = os.path.dirname(__file__)
filename = os.path.join(dirname, '../waypoints/levine_waypoints.csv')
with open(filename) as f:
path_points = [tuple(line) for line in csv.reader(f)]
topic = 'visualization_marker_array'
publisher = rospy.Publisher(topic, MarkerArray, queue_size="1")
rospy.init_node('register')
# Visualize every other marker to save on memory and speed
while not rospy.is_shutdown():
markerArray = MarkerArray()
for i in range(len(path_points)):
if i % 2 == 0:
point = path_points[i]
x = float(point[0])
y = float(point[1])
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
obstacle_idx = self.radians_to_index(
atan2(obstacle_data.y, obstacle_data.x))
return obstacle_idx, obstacle_distance
if __name__ == "__main__":
rospy.init_node('simulated_hmi')
delta_time = 0.1
hmi_height = 2 # Meters
led_radius = 2
motor_radius = 3
motor_markers = MarkerArray()
led_markers = MarkerArray()
for direction_idx in range(6):
current_marker = Marker()
current_marker.header.frame_id = 'base_footprint'
current_marker.type = Marker.CUBE
current_marker.color = ColorRGBA(1, 1, 1, 0.1)
current_marker.scale = Vector3(0.5, 0.5, 0.5)
current_marker.ns = 'motor_markers'
current_marker.id = direction_idx
angle = direction_idx * 3.14 * 2 / 6
current_marker.pose.position.x = motor_radius * cos(angle)
current_marker.pose.position.y = motor_radius * sin(angle)
current_marker.pose.position.z = hmi_height
motor_markers.markers.append(current_marker)
def handle_periodic_publish(self, event):
req = ReadDataRequest()
req.perception_name = 'objects'
req.query = '{}'
req.data = ['~']
result = self.rd_srv(req)
if not result.result:
return
vis_msg = MarkerArray()
ret_data = json.loads(result.data)
for obj in ret_data:
br = tf2_ros.TransformBroadcaster()
transformed_msg = TransformStamped()
transformed_msg.header.stamp = rospy.Time.now()
transformed_msg.header.frame_id = obj['frame_id']
transformed_msg.child_frame_id = obj['name']
transformed_msg.transform.translation.x = obj['xyz'][0]
transformed_msg.transform.translation.y = obj['xyz'][1]
transformed_msg.transform.translation.z = obj['xyz'][2]
q = tf.transformations.quaternion_from_euler(
obj['rpy'][0], obj['rpy'][1], obj['rpy'][2])
transformed_msg.transform.rotation.x = q[0]
transformed_msg.transform.rotation.y = q[1]
transformed_msg.transform.rotation.z = q[2]
transformed_msg.transform.rotation.w = q[3]
br.sendTransform(transformed_msg)
marker = Marker()
marker.header.frame_id = obj['frame_id']
marker.header.stamp = rospy.Time.now()
marker.ns = obj['name']
marker.id = 0
obj_type = obj['geometry'].keys()[0]
if obj_type == 'box':
marker.type = Marker.CUBE
elif obj_type == 'cylinder':
marker.type = Marker.CYLINDER
elif obj_type == 'sphere':
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = obj['xyz'][0]
marker.pose.position.y = obj['xyz'][1]
marker.pose.position.z = obj['xyz'][2]
orientation = tf.transformations.quaternion_from_euler(
obj['rpy'][0], obj['rpy'][1], obj['rpy'][2])
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.scale.x = obj['geometry'][obj_type][0]
marker.scale.y = obj['geometry'][obj_type][1]
marker.scale.z = obj['geometry'][obj_type][2]
marker.color.r = obj['material']['color'][0]
marker.color.g = obj['material']['color'][1]
marker.color.b = obj['material']['color'][2]
marker.color.a = obj['material']['color'][3]
vis_msg.markers.append(marker)
self.pub_vis_msg.publish(vis_msg)
def publish_trajectory(tracker_pub, objects_to_track, publish_velocity=False, velocity_smoothing=False, log=False):
marker_array = MarkerArray()
for track_id in objects_to_track: # for each object
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = track_id + 10000
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_STRIP
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.8
marker.scale.x = 0.1
marker.points = []
for p in objects_to_track[track_id].locations:
marker.points.append(Point(p[0], p[1], 0))
marker_array.markers.append(marker)
# draw tangent
if objects_to_track[track_id].is_full() and np.asarray(objects_to_track[track_id].velocities)[:RATE].mean() > 1:
points_prediction, tangent_angle = circle_fitting(np.array(objects_to_track[track_id].locations))
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = track_id + 40000
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_STRIP
marker.color.r = 1.0
marker.color.g = 165/255.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.1
marker.points = []
for p in points_prediction:
marker.points.append(Point(p[0], p[1], 0))
marker_array.markers.append(marker)
arrow_marker = Marker()
arrow_marker.header.frame_id = FRAME_ID
arrow_marker.header.stamp = rospy.Time.now()
arrow_marker.id = track_id + 20000
arrow_marker.action = Marker.ADD
arrow_marker.lifetime = rospy.Duration(LIFETIME)
arrow_marker.type = Marker.ARROW
arrow_marker.color.r = 1.0
arrow_marker.color.g = 165/255.0
arrow_marker.color.b = 0.0
arrow_marker.color.a = 1.0
arrow_marker.pose.position.x = points_prediction[-1][0]
arrow_marker.pose.position.y = points_prediction[-1][1]
arrow_marker.pose.position.z = 0.0
q = ros_tf.transformations.quaternion_from_euler(0, 0, tangent_angle)
arrow_marker.pose.orientation.x = q[0]
arrow_marker.pose.orientation.y = q[1]
arrow_marker.pose.orientation.z = q[2]
arrow_marker.pose.orientation.w = q[3]
recent_mean_velocity = np.asarray(objects_to_track[track_id].velocities)[:RATE].mean()
arrow_marker.scale.x = recent_mean_velocity * 0.5
arrow_marker.scale.y = 0.2
arrow_marker.scale.z = 0.2
marker_array.markers.append(arrow_marker)
if publish_velocity:
if len(objects_to_track[track_id].velocities) > 0: # if it appears more than two (consecutive) frames
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = track_id + 30000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
p = objects_to_track[track_id].locations[0]
text_marker.pose.position.x = p[0]
text_marker.pose.position.y = p[1]
text_marker.pose.position.z = 0.5
if velocity_smoothing:
velocity = objects_to_track[track_id].velocities_smoothed[0]
else:
velocity = objects_to_track[track_id].velocities[0]
velocity *= 3.6 # convert to kph
text_marker.text = '%.1f'%velocity
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
text_marker.color.r = 0.0
text_marker.color.g = 1.0
text_marker.color.b = 0.8
text_marker.color.a = 1.0
marker_array.markers.append(text_marker)
tracker_pub.publish(marker_array)
if log:
rospy.loginfo("trajectories published")
def __init__(self, ns, skeleton_frame, body_id_text_size,
skeleton_line_width, file_name):
self.ns = ns
skeleton_pub = rospy.Publisher(self.ns, MarkerArray, queue_size=2)
# define the colors
colors = [
ColorRGBA(0.98, 0.30, 0.30, 1.00),
ColorRGBA(0.12, 0.63, 0.42, 1.00),
ColorRGBA(0.26, 0.09, 0.91, 1.00),
ColorRGBA(0.77, 0.44, 0.14, 1.00),
ColorRGBA(0.92, 0.73, 0.14, 1.00),
ColorRGBA(0.00, 0.61, 0.88, 1.00),
ColorRGBA(1.00, 0.65, 0.60, 1.00),
ColorRGBA(0.59, 0.00, 0.56, 1.00)
]
body_id_color = ColorRGBA(0.62, 0.93, 0.14, 1.00)
upper_body_ids = [3, 2, 20, 1, 0]
hands_ids = [21, 7, 6, 5, 4, 20, 8, 9, 10, 11, 23]
legs_ids = [15, 14, 13, 12, 0, 16, 17, 18, 19]
# define other joint ids
head_id = 3
# for the demonstration, I am just using one person recorded data
# however, it can be supplied here in real-time accquired using Kinect etc
# see 'realtime_visualization.py' for more info
skeleton_joints = read_skeleton_file(file_name)
bodies = [skeleton_joints]
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
marker_index = 0
person_index = 1
marker_array = MarkerArray()
for body in bodies:
now = rospy.Time.now()
marker_index += 1
upper_body = self.create_marker(marker_index,
colors[person_index],
Marker.LINE_STRIP,
skeleton_line_width, now,
skeleton_frame)
marker_index += 1
hands = self.create_marker(marker_index, colors[person_index],
Marker.LINE_STRIP,
skeleton_line_width, now,
skeleton_frame)
marker_index += 1
legs = self.create_marker(marker_index, colors[person_index],
Marker.LINE_STRIP,
skeleton_line_width, now,
skeleton_frame)
all_joints = body
upper_body.points = [
Point(all_joints[id][0], all_joints[id][1],
all_joints[id][2]) for id in upper_body_ids
]
hands.points = [
Point(all_joints[id][0], all_joints[id][1],
all_joints[id][2]) for id in hands_ids
]
legs.points = [
Point(all_joints[id][0], all_joints[id][1],
all_joints[id][2]) for id in legs_ids
]
marker_index += 1
head_id_marker = self.create_marker(marker_index,
body_id_color,
Marker.TEXT_VIEW_FACING,
body_id_text_size, now,
skeleton_frame)
head_id_marker.text = str(person_index)
head_id_marker.pose.position = Point(all_joints[head_id][0],
all_joints[head_id][1],
all_joints[head_id][2])
marker_array.markers.append(head_id_marker)
marker_array.markers.append(upper_body)
marker_array.markers.append(hands)
marker_array.markers.append(legs)
person_index += 1
skeleton_pub.publish(marker_array)
rate.sleep()
def goto(self, source, target, matrix, pub_marker, marker_array):
# Dictionary that holds the previous node reference
prev = {}
# Dictionary that holds node score
fscore = {}
# List that holds the nodes to process
frontier = []
INF = 9999
# Condition to stop the path finding algo
isEnd = False
print 'start processing'
# Intialisation
for i in range(len(matrix)):
for j in range(len(matrix[0])):
# all nodes receive a score of INF
fscore[str(i) + '_' + str(j)] = INF
# all nodes are added to the list to process
frontier.append({'x': i, 'y': j})
# score of the start node is set to 0
fscore[str(source['x']) + '_' + str(source['y'])] = 0
print 'end initialisation phase'
# while their is node to process or goal is reached (early exit)
while len(frontier) != 0 and not isEnd:
# get the node with the lowest score
u = self.minScore(fscore, frontier)
print 'current Node:' + str(u)
# remove the current node to the node to process list
frontier.remove(u)
# create a visual information
self.createClosedMarker(u, marker_array)
# get the list of the neighbors of the current node
currentNeighbors = self.getNeighbors(u, matrix)
# for all neighbors
for v in currentNeighbors:
# check that the current node has not already be processed
if self.inU(v, frontier):
# create a visual information
self.createFontierUnitMarker(v, marker_array)
# update the score of the current neighbor with the estimate distance between the neighbors and the target (heuristic)
fscore[str(v['x']) + '_' + str(v['y'])] = self.hn(
matrix, v, target)
print ' Neighbor:' + str(v) + ', hn:' + str(
self.hn(matrix, v, target))
# update precedence of the current neighbor
prev[str(v['x']) + '_' +
str(v['y'])] = str(u['x']) + '_' + str(u['y'])
# check if the current neighbor is the target
if str(v['x']) + '_' + str(v['y']) == str(
target['x']) + '_' + str(target['y']):
# end the path computation
isEnd = True
# publish visual information
pub_marker.publish(marker_array)
marker_array = MarkerArray()
# wait before next iteration
rospy.sleep(0.2)
print str(prev)
return prev
def frame_callback(self, data):
'''
This function will be called everytime whenever a message is received by the subscriber
'''
marker_counter = 0
person_counter = 1
marker_array = MarkerArray()
for person in data.persons:
now = rospy.Time.now()
person_counter = 1 if person_counter >= len(
self.colors) else person_counter
marker_color = self.colors[person_counter]
marker_counter += 1
upper_body = self.create_marker(marker_counter, marker_color,
Marker.LINE_STRIP,
self.skeleton_line_width, now)
marker_counter += 1
hands = self.create_marker(marker_counter, marker_color,
Marker.LINE_STRIP,
self.skeleton_line_width, now)
marker_counter += 1
legs = self.create_marker(marker_counter, marker_color,
Marker.LINE_STRIP,
self.skeleton_line_width, now)
marker_counter += 1
person_id = self.create_marker(marker_counter, marker_color,
Marker.TEXT_VIEW_FACING,
self.id_text_size, now)
# assign 3D positions. make sure to consider only valid body parts
upper_body.points = [
person.bodyParts[idx].point for idx in self.upper_body_ids
if self.isValid(person.bodyParts[idx])
]
hands.points = [
person.bodyParts[idx].point for idx in self.hands_ids
if self.isValid(person.bodyParts[idx])
]
legs.points = [
person.bodyParts[idx].point for idx in self.legs_ids
if self.isValid(person.bodyParts[idx])
]
# assign person id and 3D position
person_id.text = str(person_counter)
nose = person.bodyParts[self.nose_id]
if self.isValid(nose):
person_id.pose.position = Point(nose.point.x,
nose.point.y - 0.05,
nose.point.z)
marker_array.markers.append(person_id)
marker_array.markers.append(upper_body)
marker_array.markers.append(hands)
marker_array.markers.append(legs)
# update the counter
person_counter += 1
# publish the markers
self.skeleton_pub.publish(marker_array)
def run_network(self):
with lock:
if listener.im is None:
return
im_color = self.im.copy()
depth_cv = self.depth.copy()
rgb_frame_id = self.rgb_frame_id
rgb_frame_stamp = self.rgb_frame_stamp
input_Tbc = self.Tbc_now.copy()
input_Tbc_stamp = self.Tbc_stamp
print('===========================================')
# compute image blob
im = im_color.astype(np.float32, copy=True)
im -= cfg.PIXEL_MEANS
height = im.shape[0]
width = im.shape[1]
im = np.transpose(im / 255.0, (2, 0, 1))
im = im[np.newaxis, :, :, :]
inputs = torch.from_numpy(im).cuda()
# tranform to gpu
Tbc = torch.from_numpy(input_Tbc).cuda().float()
# backproject depth
depth = torch.from_numpy(depth_cv).cuda()
fx = self.dataset._intrinsic_matrix[0, 0]
fy = self.dataset._intrinsic_matrix[1, 1]
px = self.dataset._intrinsic_matrix[0, 2]
py = self.dataset._intrinsic_matrix[1, 2]
im_pcloud = posecnn_cuda.backproject_forward(fx, fy, px, py, depth)[0]
# compare the depth
depth_meas_roi = im_pcloud[:, :, 2]
mask_depth_meas = depth_meas_roi > 0
mask_depth_valid = torch.isfinite(depth_meas_roi)
# forward
cfg.TRAIN.POSE_REG = False
out_label, out_vertex, rois, out_pose = self.net(
inputs, self.label_blob, self.meta_data_blob, self.extents_blob,
self.gt_boxes_blob, self.poses_blob, self.points_blob,
self.symmetry_blob)
label_tensor = out_label[0]
labels = label_tensor.detach().cpu().numpy()
# filter out detections
rois = rois.detach().cpu().numpy()
index = np.where(rois[:, -1] > cfg.TEST.DET_THRESHOLD)[0]
rois = rois[index, :]
# non-maximum suppression within class
index = nms(rois, 0.2)
rois = rois[index, :]
# render output image
im_label = render_image_detection(self.dataset, im_color, rois, labels)
rgb_msg = self.cv_bridge.cv2_to_imgmsg(im_label, 'rgb8')
rgb_msg.header.stamp = rgb_frame_stamp
rgb_msg.header.frame_id = rgb_frame_id
self.posecnn_pub.publish(rgb_msg)
# publish segmentation mask
label_msg = self.cv_bridge.cv2_to_imgmsg(labels.astype(np.uint8))
label_msg.header.stamp = rgb_frame_stamp
label_msg.header.frame_id = rgb_frame_id
label_msg.encoding = 'mono8'
self.label_pub.publish(label_msg)
# visualization
if cfg.TEST.VISUALIZE:
fig = plt.figure()
ax = fig.add_subplot(2, 3, 1)
plt.imshow(im_color)
ax.set_title('input image')
ax = fig.add_subplot(2, 3, 2)
plt.imshow(im_label)
ax = fig.add_subplot(2, 3, 3)
plt.imshow(labels)
# show predicted vertex targets
vertex_pred = out_vertex.detach().cpu().numpy()
vertex_target = vertex_pred[0, :, :, :]
center = np.zeros((3, height, width), dtype=np.float32)
for j in range(1, dataset._num_classes):
index = np.where(labels == j)
if len(index[0]) > 0:
center[0, index[0],
index[1]] = vertex_target[3 * j, index[0], index[1]]
center[1, index[0],
index[1]] = vertex_target[3 * j + 1, index[0],
index[1]]
center[2, index[0],
index[1]] = np.exp(vertex_target[3 * j + 2,
index[0],
index[1]])
ax = fig.add_subplot(2, 3, 4)
plt.imshow(center[0, :, :])
ax.set_title('predicted center x')
ax = fig.add_subplot(2, 3, 5)
plt.imshow(center[1, :, :])
ax.set_title('predicted center y')
ax = fig.add_subplot(2, 3, 6)
plt.imshow(center[2, :, :])
ax.set_title('predicted z')
plt.show()
if not rois.shape[0]:
return
indexes = np.zeros((self.dataset.num_classes, ), dtype=np.int32)
index = np.argsort(rois[:, 2])
rois = rois[index, :]
now = rospy.Time.now()
markers = []
for i in range(rois.shape[0]):
roi = rois[i]
cls = int(roi[1])
cls_name = self.dataset._classes_test[cls]
if cls > 0 and roi[-1] > cfg.TEST.DET_THRESHOLD:
# compute mask translation
w = roi[4] - roi[2]
h = roi[5] - roi[3]
x1 = max(int(roi[2]), 0)
y1 = max(int(roi[3]), 0)
x2 = min(int(roi[4]), width - 1)
y2 = min(int(roi[5]), height - 1)
labels = torch.zeros_like(label_tensor)
labels[y1:y2, x1:x2] = label_tensor[y1:y2, x1:x2]
mask_label = labels == cls
mask = mask_label * mask_depth_meas * mask_depth_valid
pix_index = torch.nonzero(mask)
n = pix_index.shape[0]
print('[%s] points : %d' % (cls_name, n))
if n == 0:
'''
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1)
plt.imshow(depth.cpu().numpy())
ax.set_title('depth')
ax = fig.add_subplot(1, 2, 2)
plt.imshow(im_label.cpu().numpy())
plt.gca().add_patch(plt.Rectangle((x1, y1), x2-x1, y2-y1, fill=False, edgecolor='g', linewidth=3, clip_on=False))
ax.set_title('label')
plt.show()
'''
continue
points = im_pcloud[pix_index[:, 0], pix_index[:, 1], :]
# filter points
m = torch.mean(points, dim=0, keepdim=True)
mpoints = m.repeat(n, 1)
distance = torch.norm(points - mpoints, dim=1)
extent = np.mean(self.dataset._extents_test[cls, :])
points = points[distance < 1.5 * extent, :]
if points.shape[0] == 0:
continue
# transform points to base
ones = torch.ones((points.shape[0], 1),
dtype=torch.float32,
device=0)
points = torch.cat((points, ones), dim=1)
points = torch.mm(Tbc, points.t())
location = torch.mean(points[:3, :], dim=1).cpu().numpy()
if location[2] > 2.5:
continue
print('[%s] detection score: %f' % (cls_name, roi[-1]))
print('[%s] location mean: %f, %f, %f' %
(cls_name, location[0], location[1], location[2]))
# extend the location away from camera a bit
c = Tbc[:3, 3].cpu().numpy()
d = location - c
d = d / np.linalg.norm(d)
location = location + (extent / 2) * d
# publish tf raw
self.br.sendTransform(location, [0, 0, 0, 1], now,
cls_name + '_raw', self.target_frame)
# project location to base plane
location[2] = extent / 2
print('[%s] location mean on table: %f, %f, %f' %
(cls_name, location[0], location[1], location[2]))
print('-------------------------------------------')
# publish tf
self.br.sendTransform(location, [0, 0, 0, 1], now, cls_name,
self.target_frame)
# publish tf detection
indexes[cls] += 1
name = cls_name + '_%02d' % (indexes[cls])
tf_name = os.path.join("posecnn", name)
# send another transformation as bounding box (mis-used)
n = np.linalg.norm(roi[2:6])
x1 = roi[2] / n
y1 = roi[3] / n
x2 = roi[4] / n
y2 = roi[5] / n
self.br.sendTransform([n, now.secs, roi[6]], [x1, y1, x2, y2],
now, tf_name + '_roi', self.target_frame)
# publish marker
marker = Marker()
marker.header.frame_id = self.target_frame
marker.header.stamp = now
marker.id = cls
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = location[0]
marker.pose.position.y = location[1]
marker.pose.position.z = location[2]
marker.pose.orientation.x = 0.
marker.pose.orientation.y = 0.
marker.pose.orientation.z = 0.
marker.pose.orientation.w = 1.
marker.scale.x = .05
marker.scale.y = .05
marker.scale.z = .05
if cfg.TEST.ROS_CAMERA == 'Azure':
marker.color.a = .3
elif cfg.TEST.ROS_CAMERA == 'D415':
marker.color.a = 1.
marker.color.r = self.dataset._class_colors_test[cls][0] / 255.0
marker.color.g = self.dataset._class_colors_test[cls][1] / 255.0
marker.color.b = self.dataset._class_colors_test[cls][2] / 255.0
markers.append(marker)
self.viz_pub.publish(MarkerArray(markers))
import lib_cpp
import time
import rospy
import std_msgs.msg
from geometry_msgs.msg import Point
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import Point32
from geometry_msgs.msg import Quaternion
import sensor_msgs.point_cloud2 as pcl2
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
mnum = 0
marker_array = MarkerArray()
marker_array_text = MarkerArray()
# voxel size: Related to detection range
nh = round((cfg.RANGE['X_MAX']-cfg.RANGE['X_MIN']) / cfg.VOXEL_SIZE[0])
nw = round((cfg.RANGE['Y_MAX']-cfg.RANGE['Y_MIN']) / cfg.VOXEL_SIZE[1])
nz = round((cfg.RANGE['Z_MAX']-cfg.RANGE['Z_MIN']) / cfg.VOXEL_SIZE[2])
print(nh, nw, nz)
T1 = np.array([[0.0, -1.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0],
[1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]
)
lines = [[0, 1], [1, 2], [2, 3], [3, 0], [4, 5], [5, 6],
def callback(self,data,args):
# rospy.loginfo(rospy.get_caller_id() + "\nI heard %s", data)
if args == "gate_size":
self.gate_size = data.data
if args == "cam_ctrl":
# canvas.itemconfig(cam_text)
# canvas.coords(cam_ptr, 25 * (data.angular.z / 60 + 1), 25 * (-data.angular.y / 90 + 1))
pass
elif args == "reset":
# print("reset pressed")
pass
elif args == "arm_theta":
self.gate_arm_theta = data.data
elif args == "state":
if data.data != self.state_level:
self.gate_number = self.gate_number+1
# self.state_level = self.state_level+10
elif args == "odom":
quat = data.pose.pose.orientation
# print quat
pos = data.pose.pose.position
# quat = tf.transformations.quaternion_from_euler(quat.x,quat.y,quat.z,quat.w)
marker_array = MarkerArray()
point_marker = Marker()
point_marker.header.frame_id = "vehicle_frame"
point_marker.header.stamp = rospy.get_rostime()
point_marker.ns = "vehicle"
point_marker.id = 0
point_marker.type = 1 # prism
point_marker.action = 0
point_marker.scale.x = .2
point_marker.scale.y = .05
point_marker.scale.z = .05
point_marker.pose.orientation.w = 1
# point_marker.color.r = 0
# point_marker.color.g = 0
point_marker.color.b = 1
point_marker.color.a = 1.0
point_marker.lifetime = rospy.Duration(0)
marker_array.markers.append(point_marker)
point_marker.color.r = .5
point_marker.color.g = .5
point_marker.color.b = .5
point_marker.type = 3
point_marker.pose.position.z = .02
point_marker.pose.position.x = .1
point_marker.pose.position.y = .1
point_marker.scale.x = .02
point_marker.scale.y = .12
point_marker.scale.z = .12
marker_array.markers.append(point_marker)
point_marker.pose.position.y = -.1
marker_array.markers.append(point_marker)
point_marker.pose.position.x = -.1
marker_array.markers.append(point_marker)
point_marker.pose.position.y = .1
marker_array.markers.append(point_marker)
self.vichile_pub.publish(marker_array)
self.tbr.sendTransform((pos.x,pos.y,pos.z),(quat.x,quat.y,quat.z,quat.w),rospy.get_rostime(),'vehicle_frame', "odom")
elif args == "zed_odom":
quat = data.pose.pose.orientation
# print quat
pos = data.pose.pose.position
# quat = tf.transformations.quaternion_from_euler(quat.x,quat.y,quat.z,quat.w)
marker_array = MarkerArray()
point_marker = Marker()
point_marker.header.frame_id = "zed_frame"
point_marker.header.stamp = rospy.get_rostime()
point_marker.ns = "camera"
point_marker.id = 0
point_marker.type = 2 # sphere
point_marker.action = 0
point_marker.scale.x = .4
point_marker.scale.y = .2
point_marker.scale.z = .1
point_marker.pose.orientation.w = 1
point_marker.color.r = .5
point_marker.color.g = .5
point_marker.color.b = 1
point_marker.color.a = 1.0
point_marker.lifetime = rospy.Duration(0)
marker_array.markers.append(point_marker)
self.vichile_pub.publish(marker_array)
self.tbr.sendTransform((pos.x,pos.y,pos.z),(quat.x,quat.y,quat.z,quat.w),rospy.get_rostime(),'zed_frame', "odom")
elif args == "cam_orient":
pass
elif args == "battery":
# print ' '
# print 'Battery level: ',data.percent
# print ' '
self.battery_level = data.percent
elif args == "wifi":
if data.rssi * 2 + 160 < 100.0:
print 'R: ',(data.rssi * 2 + 160),' B: ',self.battery_level
else:
print 'R: ',(data.rssi * 2 + 160),' B: ',self.battery_level
elif args == "altitude":
# print "altitude ",data.altitude
pass
elif args == "attitude":
# print "attitude", print [data.roll, data.pitch, data.yaw]
pass
elif args == "speed":
# print "speed", [data.speedX, data.speedY, data.speedZ]
pass
elif args == "overheat":
print("overheat")
# print data
elif args == 'cmds':
marker_array = MarkerArray()
gate_marker_1 = Marker()
gate_marker_1.header.frame_id = "vehicle_frame"
gate_marker_1.header.stamp = rospy.get_rostime()
gate_marker_1.ns = "effort"
gate_marker_1.id = 0
gate_marker_1.type = 1
gate_marker_1.action = 0
gate_marker_1.pose.orientation.w = 1
gate_marker_1.pose.position.x = data.x*5/2.0
gate_marker_1.scale.x = data.x*5
gate_marker_1.scale.y = .04
gate_marker_1.scale.z = .04
gate_marker_1.color.r = 1
gate_marker_1.color.g = 0
gate_marker_1.color.b = 1
gate_marker_1.color.a = 1.0
gate_marker_1.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_1)
gate_marker_2 = Marker()
gate_marker_2.header.frame_id = "vehicle_frame"
gate_marker_2.header.stamp = rospy.get_rostime()
gate_marker_2.ns = "effort"
gate_marker_2.id = 1
gate_marker_2.type = 1
gate_marker_2.action = 0
gate_marker_2.pose.orientation.w = 1
gate_marker_2.pose.position.y = data.y*5/2.0
gate_marker_2.scale.x = .04
gate_marker_2.scale.y = data.y*5
gate_marker_2.scale.z = .04
gate_marker_2.color.r = 1
gate_marker_2.color.g = 0
gate_marker_2.color.b = 1
gate_marker_2.color.a = 1.0
gate_marker_2.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_2)
gate_marker_3 = Marker()
gate_marker_3.header.frame_id = "vehicle_frame"
gate_marker_3.header.stamp = rospy.get_rostime()
gate_marker_3.ns = "effort"
gate_marker_3.id = 2
gate_marker_3.type = 1
gate_marker_3.action = 0
gate_marker_3.pose.orientation.w = 1
gate_marker_3.pose.position.z = (data.z)
gate_marker_3.scale.x = .04
gate_marker_3.scale.y = .04
gate_marker_3.scale.z = (data.z)*2
gate_marker_3.color.r = 1
gate_marker_3.color.g = 0
gate_marker_3.color.b = 1
gate_marker_3.color.a = 1.0
gate_marker_3.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_3)
gate_marker_4 = Marker()
gate_marker_4.header.frame_id = "vehicle_frame"
gate_marker_4.header.stamp = rospy.get_rostime()
gate_marker_4.ns = "effort"
gate_marker_4.id = 3
gate_marker_4.type = 1
gate_marker_4.action = 0
gate_marker_4.pose.orientation.w = 1
gate_marker_4.pose.position.x = .5
gate_marker_4.pose.position.y = (data.r)/2
gate_marker_4.scale.x = .04
gate_marker_4.scale.y = (data.r)
gate_marker_4.scale.z = .04
gate_marker_4.color.r = .5
gate_marker_4.color.g = .5
gate_marker_4.color.b = .5
gate_marker_4.color.a = 1.0
gate_marker_4.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_4)
self.vichile_pub.publish(marker_array)
elif args == 'Image':
pass
elif args == 'wp_visual':
if data.pos.z == 0 and data.hdg == 0:
return
marker_array = MarkerArray()
temp_Size = self.gate_size
temp_normal_pos = [0,0]
if(False):
temp_Size = 2
temp_normal_pos = [0,0]
theta = math.pi/4.0
theta = self.gate_arm_theta
gate_marker_s = Marker()
gate_marker_s.header.frame_id = "gate_frame_visual"
gate_marker_s.header.stamp = rospy.get_rostime()
gate_marker_s.ns = "gate_arm"
gate_marker_s.id = 2
gate_marker_s.type = 1
gate_marker_s.action = 0
gate_marker_s.pose.position.x = -.05
gate_marker_s.pose.position.y = -.5*.7*math.cos(theta)
gate_marker_s.pose.position.z = .5*.7*math.sin(theta)
quat = tf.transformations.quaternion_from_euler(theta, 0, 0)
gate_marker_s.pose.orientation.x = quat[0]
gate_marker_s.pose.orientation.y = quat[1]
gate_marker_s.pose.orientation.z = quat[2]
gate_marker_s.pose.orientation.w = quat[3]
gate_marker_s.scale.x = .05
gate_marker_s.scale.y = .05
gate_marker_s.scale.z = .7
gate_marker_s.color.g = 1.0
gate_marker_s.color.a = 1
gate_marker_s.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_s)
gate_marker_1 = Marker()
gate_marker_1.header.frame_id = "gate_frame_visual"
gate_marker_1.header.stamp = rospy.get_rostime()
gate_marker_1.ns = "gate_1"
gate_marker_1.id = self.gate_number
gate_marker_1.type = 1
gate_marker_1.action = 0
gate_marker_1.pose.position.z = temp_Size/2
gate_marker_1.pose.orientation.w = 1
gate_marker_1.scale.x = .05
gate_marker_1.scale.y = temp_Size
gate_marker_1.scale.z = .05
gate_marker_1.color.r = 1.0
gate_marker_1.color.g = .5
gate_marker_1.color.b = 0.0
gate_marker_1.color.a = 1.0
gate_marker_1.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_1)
gate_marker_2 = Marker()
gate_marker_2.header.frame_id = "gate_frame_visual"
gate_marker_2.header.stamp = rospy.get_rostime()
gate_marker_2.ns = "gate_2"
gate_marker_2.id = self.gate_number
gate_marker_2.type = 1
gate_marker_2.action = 0
gate_marker_2.pose.position.y = temp_Size/2
gate_marker_2.pose.position.z = 0
gate_marker_2.pose.orientation.w = 1
gate_marker_2.scale.x = .05
gate_marker_2.scale.y = .04
gate_marker_2.scale.z = temp_Size
gate_marker_2.color.r = 1.0
gate_marker_2.color.g = .5
gate_marker_2.color.b = 0.0
gate_marker_2.color.a = 1.0
gate_marker_2.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_2)
gate_marker_3 = Marker()
gate_marker_3.header.frame_id = "gate_frame_visual"
gate_marker_3.header.stamp = rospy.get_rostime()
gate_marker_3.ns = "gate_3"
gate_marker_3.id = self.gate_number
gate_marker_3.type = 1
gate_marker_3.action = 0
gate_marker_3.pose.position.y = -temp_Size/2
gate_marker_3.pose.orientation.w = 1
gate_marker_3.scale.x = .05
gate_marker_3.scale.y = .05
gate_marker_3.scale.z = temp_Size
gate_marker_3.color.r = 1.0
gate_marker_3.color.g = .5
gate_marker_3.color.b = 0.0
gate_marker_3.color.a = 1.0
gate_marker_3.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_3)
gate_marker_4 = Marker()
gate_marker_4.header.frame_id = "gate_frame_visual"
gate_marker_4.header.stamp = rospy.get_rostime()
gate_marker_4.ns = "gate_4"
gate_marker_4.id = self.gate_number
gate_marker_4.type = 1
gate_marker_4.action = 0
gate_marker_4.pose.position.z = -temp_Size/2
gate_marker_4.pose.orientation.w = 1
gate_marker_4.scale.x = .05
gate_marker_4.scale.y = temp_Size
gate_marker_4.scale.z = .05
gate_marker_4.color.r = 1.0
gate_marker_4.color.g = .5
gate_marker_4.color.b = 0.0
gate_marker_4.color.a = 1.0
gate_marker_4.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_4)
gate_marker_n = Marker()
gate_marker_n.header.frame_id = "gate_frame_visual"
gate_marker_n.header.stamp = rospy.get_rostime()
gate_marker_n.ns = "gate_5n"
gate_marker_n.id = self.gate_number
gate_marker_n.type = 1
gate_marker_n.action = 0
gate_marker_n.pose.orientation.w = 1
gate_marker_n.pose.position.y = temp_normal_pos[0]
gate_marker_n.pose.position.z = temp_normal_pos[1]
gate_marker_n.scale.x = 3
gate_marker_n.scale.y = .05
gate_marker_n.scale.z = .05
gate_marker_n.color.r = 1.0
gate_marker_n.color.g = 1.0
gate_marker_n.color.b = 1.0
gate_marker_n.color.a = 1.0
gate_marker_n.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_n)
self.gate_pub.publish(marker_array)
# self.tbr.sendTransform((data.t[0],data.t[1],data.t[2]),(quat[0],quat[1],quat[2],quat[3]),rospy.get_rostime(),'gate_frame',"odom")
quat = tf.transformations.quaternion_from_euler(0, 0, data.hdg)
self.tbr.sendTransform((data.pos.x,data.pos.y,data.pos.z),(quat[0],quat[1],quat[2],quat[3]),rospy.get_rostime(),'gate_frame_visual',"odom")
elif args == 'wp_blind':
if data.pos.z == 0 and data.hdg == 0:
return
marker_array = MarkerArray()
quat = tf.transformations.quaternion_from_euler(0, 0, data.hdg)
gate_marker_1 = Marker()
gate_marker_1.header.frame_id = "gate_frame_blind"
gate_marker_1.header.stamp = rospy.get_rostime()
gate_marker_1.ns = "wp_blind_1"
gate_marker_1.id = self.gate_number
gate_marker_1.type = 2
gate_marker_1.action = 0
gate_marker_1.pose.orientation.w = 1
gate_marker_1.scale.x = .25
gate_marker_1.scale.y = .25
gate_marker_1.scale.z = .25
gate_marker_1.color.r = .5
gate_marker_1.color.g = 0
gate_marker_1.color.b = 1
gate_marker_1.color.a = 1.0
gate_marker_1.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_1)
gate_marker_4 = Marker()
gate_marker_4.header.frame_id = "gate_frame_blind"
gate_marker_4.header.stamp = rospy.get_rostime()
gate_marker_4.ns = "wp_blind_2"
gate_marker_4.id = self.gate_number
gate_marker_4.type = 1
gate_marker_4.action = 0
gate_marker_4.pose.position.x = -.5
# gate_marker_4.pose.position.y = 0
gate_marker_4.pose.orientation.w = 1
gate_marker_4.scale.x = 1
gate_marker_4.scale.y = .03
gate_marker_4.scale.z = .03
gate_marker_4.color.r = 1
gate_marker_4.color.g = 0
gate_marker_4.color.b = 1
gate_marker_4.color.a = 1.0
gate_marker_4.lifetime = rospy.Duration(0)
#marker_array.markers.append(gate_marker_4)
# print 'sending blind wp'
self.gate_pub.publish(marker_array)
self.tbr.sendTransform((data.pos.x,data.pos.y,data.pos.z),(quat[0],quat[1],quat[2],quat[3]),rospy.get_rostime(),'gate_frame_blind',"odom")
elif args == 'wp_look':
if data.pos.x == 0 and data.pos.y == 0:
return
marker_array = MarkerArray()
gate_marker_1 = Marker()
gate_marker_1.header.frame_id = 'wp_look_frame'
gate_marker_1.header.stamp = rospy.get_rostime()
gate_marker_1.ns = "wp_look"
# gate_marker_1.id = self.gate_number
gate_marker_1.id = 1
gate_marker_1.type = 2
gate_marker_1.action = 0
gate_marker_1.pose.orientation.w = 1
gate_marker_1.scale.x = .25
gate_marker_1.scale.y = .25
gate_marker_1.scale.z = .75
gate_marker_1.color.r = 0
gate_marker_1.color.g = 0
gate_marker_1.color.b = 1
gate_marker_1.color.a = 1.0
gate_marker_1.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_1)
self.gate_pub.publish(marker_array)
self.tbr.sendTransform((data.pos.x,data.pos.y,2),(0,0,0,1),rospy.get_rostime(),'wp_look_frame',"odom")
elif args == 'wp_current':
if data.pos.x == 0 and data.pos.y == 0:
return
marker_array = MarkerArray()
gate_marker_1 = Marker()
gate_marker_1.header.frame_id = 'wp_current_frame'
gate_marker_1.header.stamp = rospy.get_rostime()
gate_marker_1.ns = "wp_current_1"
gate_marker_1.id = 1
gate_marker_1.type = 1
gate_marker_1.action = 0
gate_marker_1.pose.orientation.w = 1
gate_marker_1.scale.x = .05
gate_marker_1.scale.y = .4
gate_marker_1.scale.z = .4
gate_marker_1.color.r = 1
gate_marker_1.color.g = 0
gate_marker_1.color.b = 0
gate_marker_1.color.a = 1.0
gate_marker_1.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_1)
gate_marker_4 = Marker()
gate_marker_4.header.frame_id = "wp_current_frame"
gate_marker_4.header.stamp = rospy.get_rostime()
gate_marker_4.ns = "wp_current_2"
gate_marker_4.id = 1
gate_marker_4.type = 1
gate_marker_4.action = 0
gate_marker_4.pose.orientation.w = 1
gate_marker_4.scale.x = 2
gate_marker_4.scale.y = .03
gate_marker_4.scale.z = .03
gate_marker_4.color.r = 1
gate_marker_4.color.g = .1
gate_marker_4.color.b = .1
gate_marker_4.color.a = 1.0
gate_marker_4.lifetime = rospy.Duration(0)
marker_array.markers.append(gate_marker_4)
self.gate_pub.publish(marker_array)
quat = tf.transformations.quaternion_from_euler(0, 0, data.hdg)
self.tbr.sendTransform((data.pos.x,data.pos.y,data.pos.z),(quat[0],quat[1],quat[2],quat[3]),rospy.get_rostime(),'wp_current_frame',"odom")
from visualization_msgs.msg import MarkerArray
from visualization_msgs.msg import Marker
import math
import sys
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
import cv2
br = CvBridge()
vehicle = (0, 0, 255)
pedestrian = (0, 255, 0)
cyclist = (255, 0, 0)
sign = (0, 255, 255)
front_labels = MarkerArray()
frontleft_labels = MarkerArray()
frontright_labels = MarkerArray()
sideleft_labels = MarkerArray()
sideright_labels = MarkerArray()
front_image = Image()
frontleft_image = Image()
frontright_image = Image()
sideleft_image = Image()
sideright_image = Image()
get_front_image = False
get_frontleft_image = False
get_frontright_image = False
get_sideleft_image = False
get_sideright_image = False
real_score_value = [score_value[i] for i in ind_good_grasp]
if show_bad_grasp:
real_bad_grasp = [real_grasp[i] for i in ind_bad_grasp]
# end of grasp detection
# get sorted ind by the score values
sorted_value_ind = list(
index for index, item in sorted(enumerate(real_score_value),
key=lambda item: item[1],
reverse=True))
# sort grasps using the ind
sorted_real_good_grasp = [real_good_grasp[i] for i in sorted_value_ind]
real_good_grasp = sorted_real_good_grasp
# get the sorted score value, from high to low
real_score_value = sorted(real_score_value, reverse=True)
marker_array = MarkerArray()
marker_array_single = MarkerArray()
grasp_msg_list = GraspConfigList()
for i in range(len(real_good_grasp)):
grasp_msg = get_grasp_msg(real_good_grasp[i], real_score_value[i])
grasp_msg_list.grasps.append(grasp_msg)
for i in range(len(real_good_grasp)):
show_grasp_marker(marker_array, real_good_grasp[i], gripper,
(0, 1, 0), marker_life_time)
if show_bad_grasp:
for i in range(len(real_bad_grasp)):
show_grasp_marker(marker_array, real_bad_grasp[i], gripper,
(1, 0, 0), marker_life_time)
def publishLines(self, markerLines):
markerArray = MarkerArray()
markerArray.markers = markerLines
self.pubMarker.publish(markerArray);
def show(self, topic_name, grid_map):
pub = rospy.Publisher(topic_name,
MarkerArray,
queue_size=10,
latch=True)
# Build marker for center point of rear axis.
center_rear = Marker()
center_rear.id = 0
center_rear.header.frame_id = '/map'
center_rear.type = center_rear.CUBE
center_rear.action = center_rear.ADD
center_rear.pose.position.x = self.state.x
center_rear.pose.position.y = grid_map.max_y * grid_map.resolution - self.state.y
center_rear.pose.position.z = -0.05
center_rear.scale.x = 0.1
center_rear.scale.y = 0.1
center_rear.scale.z = 0.1
center_rear.color.r = 1.0
center_rear.color.a = 1.0
# Build marker for center point of vehicle.
center = Marker()
center.id = 1
center.header.frame_id = '/map'
center.type = center.CUBE
center.action = center.ADD
center.pose.position.x = self.state.x + 0.5 * self.model.config[
'wheelbase'] * sin(radians(self.state.yaw))
center.pose.position.y = grid_map.max_y * grid_map.resolution - (
self.state.y + 0.5 * self.model.config['wheelbase'] *
cos(radians(self.state.yaw)))
center.pose.position.z = -0.05
center.scale.x = 0.1
center.scale.y = 0.1
center.scale.z = 0.1
center.color.g = 1.0
center.color.a = 1.0
# Build marker for vehicle.
vehicle = Marker()
vehicle.id = 2
vehicle.header.frame_id = '/map'
vehicle.type = vehicle.CUBE
vehicle.action = vehicle.ADD
vehicle.pose.position.x = self.state.x + 0.5 * self.model.config[
'wheelbase'] * sin(radians(self.state.yaw))
vehicle.pose.position.y = grid_map.max_y * grid_map.resolution - (
self.state.y + 0.5 * self.model.config['wheelbase'] *
cos(radians(self.state.yaw)))
vehicle.pose.position.z = 0.005
tmp_qua = Quaternion(axis=[1, 0, 0], angle=radians(self.state.yaw))
vehicle.pose.orientation.x = tmp_qua[0]
vehicle.pose.orientation.y = tmp_qua[1]
vehicle.pose.orientation.z = tmp_qua[2]
vehicle.pose.orientation.w = tmp_qua[3]
vehicle.scale.x = self.model.config['width']
vehicle.scale.y = self.model.config['length']
vehicle.scale.z = 0.01
vehicle.color.b = 1.0
vehicle.color.a = 1.0
# Build message for state.
msg = MarkerArray()
msg.markers.append(center_rear)
msg.markers.append(center)
msg.markers.append(vehicle)
pub.publish(msg)
def publish_bounding_box(self, corners):
if tf.is_tensor(corners):
corners = corners.numpy()
corner_markers = MarkerArray()
edges = [(0, 1), (1, 3), (3, 2), (2, 0), (4, 5), (5, 7), (7, 6),
(6, 4), (0, 4), (1, 5), (2, 6), (3, 7)]
for i, pt in enumerate(corners):
marker = Marker()
marker.id = i
marker.header.frame_id = "object"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.01
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = pt[0]
marker.pose.position.y = pt[1]
marker.pose.position.z = pt[2]
corner_markers.markers.append(marker)
if i == 0:
marker.color.g = 1.0
marker.color.r = 1.0
if i == 1:
marker.color.b = 1.0
marker.color.r = 1.0
for i, edge in enumerate(edges):
marker = Marker()
marker.id = i + 100
marker.header.frame_id = "object"
marker.type = marker.LINE_LIST
marker.action = marker.ADD
marker.scale.x = 0.001
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
p1 = Point()
p1.x = corners[edge[0]][0]
p1.y = corners[edge[0]][1]
p1.z = corners[edge[0]][2]
p2 = Point()
p2.x = corners[edge[1]][0]
p2.y = corners[edge[1]][1]
p2.z = corners[edge[1]][2]
marker.points.append(p1)
marker.points.append(p2)
corner_markers.markers.append(marker)
self.bb_pub.publish(corner_markers)
def construct_marker(self, locations_array):
print(locations_array.locations)
marker_array = MarkerArray()
if (len(locations_array.locations) != 0):
for point_pos in locations_array.locations:
marker = Marker()
marker.header.frame_id = 'velo_link'
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.lifetime = rospy.Time(3)
# coordinates must be reversed to suit velo_link coordinates in rviz
if point_pos.object_type == "truck":
marker.scale.x = point_pos.size.z
marker.scale.y = -point_pos.size.x
marker.scale.z = -point_pos.size.y
marker.color.a = 0.5
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 1.0
elif point_pos.object_type == "pedestrian":
marker.scale.x = point_pos.size.z
marker.scale.y = point_pos.size.y
marker.scale.z = point_pos.size.x
marker.color.a = 0.5
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
else:
marker.scale.x = point_pos.size.z
marker.scale.y = -point_pos.size.x
marker.scale.z = -point_pos.size.y
marker.color.a = 0.5
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
yaw_angle = point_pos.alpha + point_pos.theta_ray + (np.pi / 2)
q = quaternion_from_euler(0, 0, yaw_angle)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
# coordinates must be reversed to suit velo_link coordinates in rviz
marker.pose.position.x = point_pos.point.z #z
marker.pose.position.y = -point_pos.point.x #x
marker.pose.position.z = -point_pos.point.y #y
marker_array.markers.append(marker)
id = 0
for m in marker_array.markers:
m.id = id
self.tf_pub.sendTransform(
(m.pose.position.x, m.pose.position.y,
m.pose.position.z),
(m.pose.orientation.x, m.pose.orientation.y,
m.pose.orientation.z, m.pose.orientation.w),
rospy.Time.now(), 'objec nr:' + str(m.id), 'velo_link')
id += 1
self.marker_array_pub.publish(marker_array)
self.pcl_pub.publish(self.pcl)
else:
print('no objects to publish')
