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')