def publish_debug_info(self, footprint_point, distance, scan_point): array = MarkerArray() fp = Marker() fp.id = 1 fp.type = fp.SPHERE scale = 0.1 fp.pose.position.x,fp.pose.position.y, fp.pose.position.z = footprint_point[0], footprint_point[1], 0 fp.scale.x, fp.scale.y, fp.scale.z = scale, scale, scale fp.color.r, fp.color.g, fp.color.b, fp.color.a = (0, 0, 1, 1) fp.header.frame_id = "{}/base_link".format(self.robot.robot_name) fp.frame_locked = True fp.action = fp.ADD fp.ns = "door_opening" array.markers += [fp] sp = Marker() sp.id = 2 sp.type = sp.SPHERE scale = 0.1 sp.pose.position.x,sp.pose.position.y, sp.pose.position.z = scan_point[0], scan_point[1], 0 sp.scale.x, sp.scale.y, sp.scale.z = scale, scale, scale sp.color.r, sp.color.g, sp.color.b, sp.color.a = (1, 0, 0, 1) sp.header.frame_id = "{}/base_link".format(self.robot.robot_name) sp.frame_locked = False sp.action = sp.ADD sp.ns = "door_opening" array.markers += [sp] self.debug_vizualizer.publish(array)
def add_marker(self, array, track, color, tid): m1 = Marker() m1.header.frame_id = "camera_rgb_optical_frame" m1.ns = "line" m1.id = tid m1.type = 4 #lines m1.action = 0 m1.scale.x = .002 m1.color.a = 1. m1.color.r = color[0]/255. m1.color.g = color[1]/255. m1.color.b = color[2]/255. m1.points = track array.append(m1) m2 = Marker() m2.header.frame_id = "camera_rgb_optical_frame" m2.ns = "point" m2.id = tid m2.type = 8 #points m2.action = 0 m2.scale.x = .008 m2.scale.y = .008 m2.color.a = 1. m2.color.r = color[0]/255. m2.color.g = color[1]/255. m2.color.b = color[2]/255. m2.points = [ track[-1] ] array.append(m2)
def draw_rod(self, rod): marker = Marker() marker.header.frame_id = "/pl_base" marker.ns = "basic_shapes" marker.id = 10 + rod marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.position.x = self.rod_position[rod][0] marker.pose.position.y = self.rod_position[rod][1] marker.pose.position.z = self.position_z marker.pose.orientation.w = 1.0 marker.scale.x = ROD_SCALE_XYZ[0] marker.scale.y = ROD_SCALE_XYZ[1] marker.scale.z = ROD_SCALE_XYZ[2] marker.color.r = ROD_COLOR_RGB[0] marker.color.g = ROD_COLOR_RGB[1] marker.color.b = ROD_COLOR_RGB[2] marker.color.a = 1.0 self.rviz_pub.publish(marker) # pink top of the rod marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z = marker.pose.position.z + 0.0475 marker.scale.z = TOP_SCALE_Z marker.color.r = TOP_COLOR_RGB[0] marker.color.g = TOP_COLOR_RGB[1] marker.color.b = TOP_COLOR_RGB[2] self.rviz_pub.publish(marker)
def make_marker(self, robot_id, x, y, ns): """ Create a Marker message with the given x,y coordinates """ m = Marker() m.header.stamp = rospy.Time.now() m.header.frame_id = 'map' m.ns = ns m.action = m.ADD m.pose.position.x = x m.pose.position.y = y if robot_id == 0: m.color.r = 255 m.color.g = 0 m.color.b = 0 else: m.color.r = 0 m.color.g = 0 m.color.b = 255 m.color.a = 1.0 if ns == 'mess': m.type = m.CUBE m.id = len(self.messes) m.scale.x = m.scale.y = m.scale.z = .15 self.messes.append(m) else: m.type = m.SPHERE m.id = robot_id m.scale.x = m.scale.y = m.scale.z = .35 return m
def publishPositions(self): if not self.initialized: return nr = 0 markerArray = MarkerArray() marker = Marker() marker.header.frame_id = "/map" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.mesh_use_embedded_materials = True marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae" marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation = self.pacman["orientation"] marker.pose.position.x = self.pacman["x"] marker.pose.position.y = self.pacman["y"] marker.pose.position.z = 0.0 marker.id = nr markerArray.markers.append(marker) for ghost in self.ghosts: curGhost = self.ghosts[ghost] if not curGhost["initialized"]: continue nr += 1 marker = Marker() marker.header.frame_id = "/map" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.mesh_use_embedded_materials = True if curGhost["eaten"]: marker.mesh_resource = "package://pacman_controller/meshes/dead.dae" elif self.state == State.FLEEING: marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae" else: marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation = curGhost["orientation"] marker.pose.position.x = curGhost["x"] marker.pose.position.y = curGhost["y"] marker.pose.position.z = 0.0 marker.id = nr markerArray.markers.append(marker) self.pubPositions.publish(markerArray) return
def publish_cluster(publisher, points, frame_id, namespace, cluster_id): """Publishes a marker representing a cluster. The x and y arguments specify the center of the target. Args: publisher: A visualization_msgs/Marker publisher points: A list of geometry_msgs/Point frame_id: The coordinate frame in which to interpret the points. namespace: string, a unique name for a group of clusters. cluster_id: int, a unique number for this cluster in the given namespace. """ marker = Marker() # TODO(jstn): Once the point clouds have the correct frame_id, # use them here. marker.header.frame_id = frame_id marker.header.stamp = rospy.Time().now() marker.ns = namespace marker.id = 2 * cluster_id marker.type = Marker.POINTS marker.action = Marker.ADD marker.color.r = random.random() marker.color.g = random.random() marker.color.b = random.random() marker.color.a = 0.5 marker.points = points marker.scale.x = 0.002 marker.scale.y = 0.002 marker.lifetime = rospy.Duration() center = [0, 0, 0] for point in points: center[0] += point.x center[1] += point.y center[2] += point.z center[0] /= len(points) center[1] /= len(points) center[2] /= len(points) text_marker = Marker() text_marker.header.frame_id = frame_id text_marker.header.stamp = rospy.Time().now() text_marker.ns = namespace text_marker.id = 2 * cluster_id + 1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = center[0] - 0.1 text_marker.pose.position.y = center[1] text_marker.pose.position.z = center[2] text_marker.color.r = 1 text_marker.color.g = 1 text_marker.color.b = 1 text_marker.color.a = 1 text_marker.scale.z = 0.05 text_marker.text = '{}'.format(cluster_id) text_marker.lifetime = rospy.Duration() _publish(publisher, marker, "cluster") _publish(publisher, text_marker, "text_marker") return marker
def publishMap(self): markerArray = MarkerArray() marker = Marker() marker.header.frame_id = "/map" marker.type = marker.SPHERE_LIST marker.action = marker.ADD marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 marker.id = 0 for p in self.mapPoints: if p["eaten"]: continue if p["powerup"]: continue point = Point() point.x = p["x"] point.y = p["y"] point.z = 0.15 marker.points.append(point) markerArray.markers.append(marker) marker = Marker() marker.header.frame_id = "/map" marker.type = marker.SPHERE_LIST marker.action = marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 marker.id = 1 for p in self.mapPoints: if p["eaten"]: continue if not p["powerup"]: continue point = Point() point.x = p["x"] point.y = p["y"] point.z = 0.2 marker.points.append(point) markerArray.markers.append(marker) self.pubMap.publish(markerArray)
def visualize_cluster(self, cluster, label=None): points = pc2.read_points(cluster.pointcloud, skip_nans=True) point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points] if len(point_list) == 0: rospy.logwarn('Point list was of size 0, skipping.') return marker_id = len(self._current_markers) marker = Marker() marker.header.frame_id = 'map' marker.header.stamp = rospy.Time().now() marker.ns = 'clusters' marker.id = marker_id marker.type = Marker.POINTS marker.action = Marker.ADD marker.color.r = random.random() marker.color.g = random.random() marker.color.b = random.random() marker.color.a = 0.5 + random.random() marker.points = point_list marker.scale.x = 0.002 marker.scale.y = 0.002 marker.lifetime = rospy.Duration() self.visualize_marker(marker) if label is not None: center = [0, 0, 0] for point in point_list: center[0] += point.x center[1] += point.y center[2] += point.z center[0] /= len(point_list) center[1] /= len(point_list) center[2] /= len(point_list) text_marker = Marker() text_marker.header.frame_id = 'map' text_marker.header.stamp = rospy.Time().now() text_marker.ns = 'labels' text_marker.id = marker_id + 1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = center[1] - 0.05 text_marker.pose.position.y = center[1] text_marker.pose.position.z = center[2] text_marker.color.r = 1 text_marker.color.g = 1 text_marker.color.b = 1 text_marker.color.a = 1 text_marker.scale.z = 0.05 text_marker.text = label text_marker.lifetime = rospy.Duration() self.visualize_marker(text_marker)
def publish_waypoint_markers(self): pub_waypoint_markers = rospy.Publisher('waypoint_markers', MarkerArray) marker_array = MarkerArray() for i in range(len(self.waypoints)): waypoint = self.waypoints[i] waypoint_marker = Marker() waypoint_marker.id = i waypoint_marker.header.frame_id = "/map" waypoint_marker.header.stamp = rospy.Time.now() if (waypoint.type == 'P'): waypoint_marker.type = 5 # Line List waypoint_marker.text = 'waypoint_%s_point' % i waypoint_marker.color.r = 176.0 waypoint_marker.color.g = 224.0 waypoint_marker.color.b = 230.0 waypoint_marker.color.a = 0.5 waypoint_marker.scale.x = 0.5 c = waypoint.coordinate waypoint_marker.points.append(Point(c.x, c.y, c.z)) waypoint_marker.points.append(Point(c.x, c.y, c.z + 3.0)) else: waypoint_marker.type = 3 # Cylinder waypoint_marker.text = 'waypoint_%s_cone' % i waypoint_marker.color.r = 255.0 waypoint_marker.color.g = 69.0 waypoint_marker.color.b = 0.0 waypoint_marker.color.a = 1.0 waypoint_marker.scale.x = 0.3 waypoint_marker.scale.y = 0.3 waypoint_marker.scale.z = 0.5 waypoint_marker.pose.position = waypoint.coordinate marker_array.markers.append(waypoint_marker) if self.current_waypoint_idx != len(self.waypoints): current_waypoint_marker = Marker() current_waypoint_marker.id = 999 current_waypoint_marker.header.frame_id = "/map" current_waypoint_marker.header.stamp = rospy.Time.now() current_waypoint_marker.type = 2 # Sphere current_waypoint_marker.text = 'current_waypoint' current_waypoint_marker.color.r = 255.0 current_waypoint_marker.color.g = 0.0 current_waypoint_marker.color.b = 0.0 current_waypoint_marker.color.a = 1.0 current_waypoint_marker.scale.x = 0.3 current_waypoint_marker.scale.y = 0.3 current_waypoint_marker.scale.z = 0.3 current_waypoint = self.waypoints[self.current_waypoint_idx] current_waypoint_marker.pose.position.x = current_waypoint.coordinate.x current_waypoint_marker.pose.position.y = current_waypoint.coordinate.y current_waypoint_marker.pose.position.z = 1.0 marker_array.markers.append(current_waypoint_marker) pub_waypoint_markers.publish(marker_array)
def node(): pub = rospy.Publisher('shapes', Marker, queue_size=10) rospy.init_node('plotter', anonymous=False) rate = rospy.Rate(1) points=Marker() line=Marker() #Set the frame ID and timestamp. See the TF tutorials for information on these. points.header.frame_id=line.header.frame_id="/map" points.header.stamp=line.header.stamp=rospy.Time.now() points.ns=line.ns = "markers" points.id = 0 line.id =2 points.type = Marker.POINTS line.type=Marker.LINE_STRIP #Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points.action = line.action = Marker.ADD; points.pose.orientation.w = line.pose.orientation.w = 1.0; line.scale.x = 0.02; points.scale.x=0.03; line.scale.y= 0.02; points.scale.y=0.03; line.color.r = 1.0; points.color.g = 1.0; points.color.a=line.color.a = 1.0; points.lifetime =line.lifetime = rospy.Duration(); p=Point() p.x = 1; p.y = 1; p.z = 1; pp=[] pp.append(copy(p)) while not rospy.is_shutdown(): p.x+=0.1 pp.append(copy(p)) points.points=pp #print points.points,'\n','----------------','\n' line.points=pp pub.publish(points) pub.publish(line) rate.sleep()
def publish(self, target_frame, timestamp): ma = MarkerArray() for id in self.marker_list: marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = target_frame marker.ns = "landmark_kf" marker.id = id marker.type = Marker.CYLINDER marker.action = Marker.ADD Lkf = self.marker_list[id] marker.pose.position.x = Lkf.L[0,0] marker.pose.position.y = Lkf.L[1,0] marker.pose.position.z = 0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.scale.x = max(3*sqrt(Lkf.P[0,0]),0.05) marker.scale.y = max(3*sqrt(Lkf.P[1,1]),0.05) marker.scale.z = 0.5; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.lifetime.secs=3.0; ma.markers.append(marker) marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = target_frame marker.ns = "landmark_kf" marker.id = 1000+id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD Lkf = self.marker_list[id] marker.pose.position.x = Lkf.L[0,0] marker.pose.position.y = Lkf.L[1,0] marker.pose.position.z = 1.0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.text = str(id) marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 0.2 marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 1.0; marker.lifetime.secs=3.0; ma.markers.append(marker) self.marker_pub.publish(ma)
def pubViz(self, ast, bst): rate = rospy.Rate(10) for i in range(self.winSize): msgs = MarkerArray() #use1について msg = Marker() #markerのプロパティ msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'j1' msg.action = 0 msg.id = 1 msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[2] #ジョイントポイントを入れる処理 for j1 in range(self.jointSize): point = Point() point.x = self.pdata[0][ast+i][j1][0] point.y = self.pdata[0][ast+i][j1][1] point.z = self.pdata[0][ast+i][j1][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'j2' msg.action = 0 msg.id = 2 msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[1] for j2 in range(self.jointSize): point = Point() point.x = self.pdata[1][bst+i][j2][0] point.y = self.pdata[1][bst+i][j2][1] point.z = self.pdata[1][bst+i][j2][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) self.mpub.publish(msgs) rate.sleep()
def add_rviz_rod(self, rod): marker = Marker() actual = self.irpos.get_cartesian_pose() marker.header.frame_id = "/pl_base" marker.ns = "basic_shapes" marker.id = 10 + rod marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.position.x = actual.position.x marker.pose.position.y = actual.position.y marker.pose.position.z = actual.position.z - 0.108 + 0.009 marker.pose.orientation.w = 1.0 marker.scale.x = 0.01 marker.scale.y = 0.01 marker.scale.z = 0.095 marker.color.r = 0.75 marker.color.g = 0.70 marker.color.b = 0.5 marker.color.a = 1.0 self.rviz_pub.publish(marker) # pink top of the rod marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z = marker.pose.position.z + 0.0475 marker.scale.z = 0.01 marker.color.r = 1. marker.color.g = 0.4 marker.color.b = 0.4 self.rviz_pub.publish(marker) if(rod == 1): marker.type = marker.CUBE marker.pose.position.z = actual.position.z - 0.143 marker.scale.x = 0.079 marker.scale.y = 0.23 marker.scale.z = 0.014 marker.color.r = 0.75 marker.color.g = 0.70 marker.color.b = 0.5 marker.id = 20 self.rviz_pub.publish(marker) marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z
def pubRviz(self, pos, joints): msgs = MarkerArray() for p in range(len(pos)): msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'marker' msg.action = 0 msg.id = p msg.type = 4 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[2] for i in range(len(pos[p])): point = Point() point.x = pos[p][i][0] point.y = pos[p][i][1] point.z = pos[p][i][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) for j in range(len(joints)): msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'joints' msg.action = 0 msg.id = j msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[j] #print "joints len:"+str(len(joints[j])) for i in range(len(joints[j])): point = Point() point.x = joints[j][i][0] point.y = joints[j][i][1] point.z = joints[j][i][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) self.mpub.publish(msgs)
def publishObstacles(self): """ Publishes the obstacles as markers """ mk = Marker() mk.header.stamp = rospy.get_rostime() mk.header.frame_id = '/base_link' mk.ns='basic_shapes' mk.id = 0 mk.type = Marker.POINTS mk.scale.x = 0.3 mk.scale.y = 0.3 mk.scale.z = 0.3 mk.color.r = 1.0 mk.color.a = 1.0 for value in self.obstacle_map.obstacles_in_memory: p = Point() p.x = value[0] p.y = value[1] mk.points.append(p) self.obs_pub.publish(mk)
def __init__(self): self.layout = rospy.get_param('/thruster_layout/thrusters') # Add thruster layout from ROS param set by mapper assert self.layout is not None, 'Could not load thruster layout from ROS param' ''' Create MarkerArray with an arrow marker for each thruster at index node_id. The position of the marker is as specified in the layout, but the length of the arrow will be set at each thrust callback. ''' self.markers = MarkerArray() for i in xrange(len(self.layout)): # Initialize each marker (color, scale, namespace, frame) m = Marker() m.header.frame_id = '/base_link' m.type = Marker.ARROW m.ns = 'thrusters' m.color.a = 0.8 m.scale.x = 0.01 # Shaft diameter m.scale.y = 0.05 # Head diameter m.action = Marker.DELETE m.lifetime = rospy.Duration(0.1) self.markers.markers.append(m) for key, layout in self.layout.iteritems(): # Set position and id of marker from thruster layout idx = layout['node_id'] pt = numpy_to_point(layout['position']) self.markers.markers[idx].points.append(pt) self.markers.markers[idx].points.append(pt) self.markers.markers[idx].id = idx # Create publisher for marker and subscribe to thrust self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5) self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
def create_marker(self, markerArray, view, pose, view_values): marker1 = Marker() marker1.id = self.marker_id self.marker_id += 1 marker1.header.frame_id = "/map" marker1.type = marker1.TRIANGLE_LIST marker1.action = marker1.ADD marker1.scale.x = 1 marker1.scale.y = 1 marker1.scale.z = 2 marker1.color.a = 0.25 vals = view_values.values() max_val = max(vals) non_zero_vals = filter(lambda a: a != 0, vals) min_val = min(non_zero_vals) print min_val, max_val, view_values[view.ID] marker1.color.r = r_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1))) marker1.color.g = g_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1))) marker1.color.b = b_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1))) marker1.pose.orientation = pose.orientation marker1.pose.position = pose.position marker1.points = [Point(0,0,0.01),Point(2.5,-1.39,0.01),Point(2.5,1.39,0.01)] markerArray.markers.append(marker1)
def input_handler(space): global marker_color global marker_pub global matches for str in matches: if space.aux_payload.find(str) == -1: # print "FAIL: str='%s' aux_payload='%s'" % (str, space.aux_payload) return # print space # print marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time() marker.ns = "" marker.id = 0 marker.type = Marker.POINTS marker.action = Marker.ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.orientation.w = 0 marker.scale.x = .1 marker.scale.y = .1 marker.scale.z = .1 marker.color = marker_color marker.points = [] # this line cuts off anything before the string "convex_set: " in the aux_payload convex_set = space.aux_payload[ space.aux_payload.find("convex_set: ") :] # this line cuts off that string at the next newline character convex_set = convex_set.split('\n')[0] # this line splits the string into pieces delineated by spaces convex_set = convex_set.split(' ') # print convex_set for candidate in convex_set: if len(candidate) == 0 or candidate[0] != '(': continue coords = candidate[1:-1].split(',') # print coords marker.points.append(Point(float(coords[0]), float(coords[1]), float(coords[2]))) # print marker.points[-1] # print # print marker.points # print marker_pub.publish(marker)
def publish(anns, data): wall_list = WallList() marker_list = MarkerArray() marker_id = 1 for a, d in zip(anns, data): # Walls object = deserialize_msg(d.data, Wall) wall_list.obstacles.append(object) # Markers marker = Marker() marker.id = marker_id marker.header = a.pose.header marker.type = a.shape marker.ns = "wall_obstacles" marker.action = Marker.ADD marker.lifetime = rospy.Duration.from_sec(0) marker.pose = copy.deepcopy(a.pose.pose.pose) marker.scale = a.size marker.color = a.color marker_list.markers.append(marker) marker_id = marker_id + 1 marker_pub = rospy.Publisher('wall_marker', MarkerArray, latch=True, queue_size=1) wall_pub = rospy.Publisher('wall_pose_list', WallList, latch=True, queue_size=1) wall_pub.publish(wall_list) marker_pub.publish(marker_list) return
def addPolygonFilled(self, points): oid = self.oid+1 name = "/polygonFilled"+str(self.oid) marker = Marker() marker.id = self.oid marker.ns = "/polygonFilled" marker.header.frame_id = name marker.type = marker.TRIANGLE_LIST marker.action = marker.ADD marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.points = [] for i in range(0,len(points)-2,1): pt = Point(points[0][0], points[0][1], points[0][2]) marker.points.append(pt) pt = Point(points[i+1][0], points[i+1][1], points[i+1][2]) marker.points.append(pt) pt = Point(points[i+2][0], points[i+2][1], points[i+2][2]) marker.points.append(pt) self.markerArray.markers.append(marker)
def plot_3d_vel(self, p_arr, v_arr, v_scale=1.0): marker_array = MarkerArray() for i in xrange(len(p_arr)): p = p_arr[i] v = vx,vy,vz = v_arr[i] marker = Marker() marker.header.frame_id = "/openni_rgb_optical_frame" marker.type = marker.ARROW marker.action = marker.ADD marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.points.append(Point(p[0],p[1],p[2])) marker.points.append(Point(p[0]+vx*v_scale,p[1]+vy*v_scale,p[2]+vz*v_scale)) marker.scale.x=0.05 marker.scale.y=0.1 marker.id = i marker_array.markers.append(marker) self.marker_pub.publish(marker_array)
def __publish_marker_for_object(self, object, txt): marker = Marker() marker.header.frame_id = object.object.header.frame_id marker.header.stamp = rospy.Time(0) marker.ns = "scene_classifier_marker" self.marker_id = self.marker_id + 1 marker.id = self.marker_id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.pose.position.x = object.c_centroid.x marker.pose.position.y = object.c_centroid.y marker.pose.position.z = object.c_centroid.z 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.scale.x = 0.5 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.text = txt marker.lifetime = rospy.Duration.from_sec(10) self.marker_publisher.publish(marker)
def new_marker(self, ns="/debug", frame="enu", time=None, type = Marker.CUBE , position=(0,0,0), orientation=(0,0,0,1), color=(1,0,0)): marker = Marker() marker.ns = ns if time != None: marker.header.stamp = time marker.header.frame_id = frame marker.type = type marker.action = marker.ADD marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = 1.0 marker.id = self.last_id 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.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] self.last_id += 1 self.markers.markers.append(marker)
def to_marker(self): """ :return: a marker representing the map. :type: Marker """ marker = Marker() marker.type = Marker.CUBE_LIST for x in range(0, len(self.field)): for y in range(0, len(self.field[0])): marker.header.frame_id = '/odom_combined' marker.ns = 'suturo_planning/map' marker.id = 23 marker.action = Marker.ADD (x_i, y_i) = self.index_to_coordinates(x, y) marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.w = 1 marker.scale.x = self.cell_size marker.scale.y = self.cell_size marker.scale.z = 0.01 p = Point() p.x = x_i p.y = y_i marker.points.append(p) c = self.get_cell_by_index(x, y) marker.colors.append(self.__cell_to_color(c)) marker.lifetime = rospy.Time(0) return marker
def publish_prob2(self, waypoints, objects, probs): prob_msg = MarkerArray() i = 0 idx = 0 n_waypoints = len(waypoints) n_objects = len(objects) scaling_factor = max(probs) current_probs = [0 for foo in objects] for node in self._topo_map: if node.name in waypoints: for j in range(0, n_objects): marker = Marker() marker.header.frame_id = 'map' marker.id = idx marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose = node.pose prob = probs[n_objects*i + j] prob = prob/(scaling_factor) print "AHAHHAHBHBHBHBHBHB", prob marker.pose.position.z = marker.pose.position.z + current_probs[j] marker.scale.x = 1*prob marker.scale.y = 1*prob marker.scale.z = 1*prob current_probs[j] = current_probs[j] + prob + 0.1 marker.color.a = 1.0 marker.color.r = 1.0*prob marker.color.g = 1.0*prob marker.color.b = 1.0*prob prob_msg.markers.append(marker) idx = idx + 1 i = i + 1 self._prob_marker_pub.publish(prob_msg)
def publish_prob(self, waypoints, objects, probs): prob_msg = MarkerArray() i = 0 n_waypoints = len(waypoints) n_objects = len(objects) scaling_factor = max(probs) for node in self._topo_map: if node.name in waypoints: marker = Marker() marker.header.frame_id = 'map' marker.id = i marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose = node.pose prob = 1 for j in range(0, n_objects): prob = prob*probs[n_objects*i + j] prob = prob/(scaling_factor**2) print prob marker.scale.x = 1*prob marker.scale.y = 1*prob marker.scale.z = 1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 prob_msg.markers.append(marker) i = i + 1 self._prob_marker_pub.publish(prob_msg)
def drawlandmark(pub): global pt_count points_tag = Marker() points_tag.header.frame_id = "/base_link" points_tag.action = Marker.ADD points_tag.header.stamp = rospy.Time.now() points_tag.lifetime = rospy.Time(0) points_tag.scale.x = 0.1; points_tag.scale.y = 0.1; points_tag.scale.z = 0.1; points_tag.color.a = 1.0; points_tag.color.r = 1.0; points_tag.color.g = 0.0; points_tag.color.b = 0.0; points_tag.ns = "pts_line" pt_count+=1 points_tag.id = pt_count points_tag.type = Marker.POINTS for x in range(6): p1 = Point() p1.x = tagnum_x[x]/100 p1.y = tagnum_y[x]/100 p1.z = 0 points_tag.points.append(p1) pub.publish(points_tag)
def pose_cb(self, pose): pose = self.listener.transformPose(self.reference_frame,pose) q = pose.pose.orientation qvec = [q.x,q.y,q.z,q.w] euler = euler_from_quaternion(qvec) self.goal_x = pose.pose.position.x self.goal_y = pose.pose.position.y self.goal_theta = euler[2] (ex,ey,etheta) = self.compute_error() if ex < -self.dist_threshold: self.goal_theta = norm_angle(etheta + pi) print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta) marker = Marker() marker.header = pose.header marker.ns = "goal_marker" marker.id = 10 marker.type = Marker.ARROW marker.action = Marker.ADD marker.pose = pose.pose marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 1.0; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.lifetime.secs=-1.0 self.marker_pub.publish(marker)
def publish_marker(self): # create marker marker = Marker() marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time.now() marker.ns = "color" marker.id = 0 marker.type = 2 # SPHERE marker.action = 0 # ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 1.5 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.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = self.color.a #Transparency marker.color.r = self.color.r marker.color.g = self.color.g marker.color.b = self.color.b # publish marker self.pub_marker.publish(marker)
def publish_markerArray(publisher, points, rgba=(1, 0, 0, 1), shape=Marker.CUBE, duration=rospy.Duration(360), ns='ns'): #points is expected to be a list of tuples (x,y) #It's recommended that the publisher is created with latch=True _id = 0 ma = MarkerArray() for p in points: m = Marker() m.header.frame_id = '/map' m.header.stamp = rospy.get_rostime() m.ns = ns m.id = _id m.type = shape m.action = m.ADD m.pose.position.x = p[0] m.pose.position.y = p[1] m.pose.orientation.w = 1 m.scale.x = 0.5 m.scale.y = 0.5 m.scale.z = 0.5 m.color.r = rgba[0] m.color.g = rgba[1] m.color.b = rgba[2] m.color.a = rgba[3] m.lifetime = duration ma.markers.append(m) _id += 1 publisher.publish(ma)
def handle_velodyne_msg(msg, arg=None): t_start = time.time() global tf_segmenter_graph global last_known_position, last_known_box_size, last_known_yaw if segmenter_model is None: init_segmenter(args.segmenter_model) if localizer_model is None: init_localizer(args.localizer_model) assert msg._type == 'sensor_msgs/PointCloud2' # PointCloud2 reference http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html if verbose: print 'stamp: %s' % msg.header.stamp #print 'number of points: %i' % msg.width * msg.height # PERFORMANCE WARNING START # this preparation code is super slow b/c it uses generator, ideally the code should receive 3 arrays: # lidar_d lidar_h lidar_i already preprocessed in C++ # points = 0 # for x, y, z, intensity, ring in pc2.read_points(msg): # cloud[points] = x, y, z, intensity, ring # points += 1 cloud = pcl.msg2cloud(msg) lidar, lidar_int, angle_at_edge = DidiTracklet.filter_lidar_rings( #cloud[:points], cloud, rings, points_per_ring, clip=CLIP_DIST, clip_h=CLIP_HEIGHT, return_lidar_interpolated=True, return_angle_at_edges=True) points_per_sector = points_per_ring // sectors _sectors = 2 * sectors if segmenter_phased else sectors lidar_d = np.empty((_sectors, points_per_sector, len(rings)), dtype=np.float32) lidar_h = np.empty((_sectors, points_per_sector, len(rings)), dtype=np.float32) lidar_i = np.empty((_sectors, points_per_sector, len(rings)), dtype=np.float32) s_start = 0 for sector in range(sectors): s_end = s_start + points_per_sector for ring in range(len(rings)): lidar_d[sector, :, ring] = lidar[ring, s_start:s_end, 0] lidar_h[sector, :, ring] = lidar[ring, s_start:s_end, 1] lidar_i[sector, :, ring] = lidar[ring, s_start:s_end, 2] s_start = s_end if segmenter_phased: s_start = points_per_sector // 2 for sector in range(sectors - 1): _sector = sectors + sector s_end = s_start + points_per_sector for ring in range(len(rings)): lidar_d[_sector, :, ring] = lidar[ring, s_start:s_end, 0] lidar_h[_sector, :, ring] = lidar[ring, s_start:s_end, 1] lidar_i[_sector, :, ring] = lidar[ring, s_start:s_end, 2] s_start = s_end for ring in range(len(rings)): lidar_d[_sectors - 1, :points_per_sector // 2, ring] = lidar[ring, :points_per_sector // 2, 0] lidar_d[_sectors - 1, points_per_sector // 2:, ring] = lidar[ring, points_per_ring - points_per_sector // 2:, 0] lidar_h[_sectors - 1, :points_per_sector // 2, ring] = lidar[ring, :points_per_sector // 2, 1] lidar_h[_sectors - 1, points_per_sector // 2:, ring] = lidar[ring, points_per_ring - points_per_sector // 2:, 1] lidar_i[_sectors - 1, :points_per_sector // 2, ring] = lidar[ring, :points_per_sector // 2, 2] lidar_i[_sectors - 1, points_per_sector // 2:, ring] = lidar[ring, points_per_ring - points_per_sector // 2:, 2] # PERFORMANCE WARNING END if K._backend == 'tensorflow': with tf_segmenter_graph.as_default(): time_seg_infe_start = time.time() class_predictions_by_angle = segmenter_model.predict( [lidar_d, lidar_h, lidar_i], batch_size=_sectors) time_seg_infe_end = time.time() else: time_seg_infe_start = time.time() class_predictions_by_angle = segmenter_model.predict( [lidar_d, lidar_h, lidar_i], batch_size=_sectors) time_seg_infe_end = time.time() if verbose: print ' Seg inference: %0.3f ms' % ( (time_seg_infe_end - time_seg_infe_start) * 1000.0) if segmenter_phased: _class_predictions_by_angle = class_predictions_by_angle.reshape( (-1, points_per_ring, len(rings))) class_predictions_by_angle = np.copy(_class_predictions_by_angle[0, :]) class_predictions_by_angle[points_per_sector // 2 : points_per_ring - (points_per_sector//2)] += \ _class_predictions_by_angle[1 , : points_per_ring - points_per_sector] class_predictions_by_angle[0 : points_per_sector // 2 ] += \ _class_predictions_by_angle[1 , points_per_ring - points_per_sector : points_per_ring - (points_per_sector // 2)] class_predictions_by_angle[points_per_ring - (points_per_sector // 2) : ] += \ _class_predictions_by_angle[1 , points_per_ring - (points_per_sector // 2): ] class_predictions_by_angle_idx = np.argwhere( class_predictions_by_angle >= (2 * segmenter_threshold)) else: class_predictions_by_angle = np.squeeze( class_predictions_by_angle.reshape( (-1, points_per_ring, len(rings))), axis=0) if segmenter_threshold == 0.: # dynamic thresholding number_of_segmented_points = 0 dyn_threshold = 0.7 while (number_of_segmented_points < 100) and dyn_threshold >= 0.2: class_predictions_by_angle_thresholded = ( class_predictions_by_angle >= dyn_threshold) number_of_segmented_points = np.sum( class_predictions_by_angle_thresholded) dyn_threshold -= 0.1 class_predictions_by_angle_idx = np.argwhere( class_predictions_by_angle_thresholded) # print(dyn_threshold + 0.1, number_of_segmented_points) else: #for prob in [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8]: # print(prob, np.sum(class_predictions_by_angle >= prob)) class_predictions_by_angle_idx = np.argwhere( class_predictions_by_angle >= segmenter_threshold) filtered_points_xyz = np.empty((0, 3)) if (class_predictions_by_angle_idx.shape[0] > 0): # the idea of de-interpolation is to remove artifacts created by same-neighbor interpolation # by checking repeated values (which are going to be nearest-neighbor interpolated values with high prob) # for code convenience, we'e just taking the unique indexes as returned by np.unique but we # could further improve this by calculating the center of mass on the X axis of the prediction # vector (with the unique elements only), and take the index closest to the center for each duplicated stride. if deinterpolate: #((a.shape[0] - 1 - i_l ) + (i_f)) // 2 deinterpolated_class_predictions_by_angle_idx = np.empty((0, 2)) lidar_d_interpolated = lidar_d.reshape( (-1, points_per_ring, len(rings)))[0] for ring in range(len(rings)): predictions_idx_in_ring = class_predictions_by_angle_idx[ class_predictions_by_angle_idx[:, 1] == ring] if predictions_idx_in_ring.shape[0] > 1: lidar_d_predictions_in_ring = lidar_d_interpolated[ predictions_idx_in_ring[:, 0], ring] _, lidar_d_predictions_in_ring_unique_idx_first = np.unique( lidar_d_predictions_in_ring, return_index=True) _, lidar_d_predictions_in_ring_unique_idx_last = np.unique( lidar_d_predictions_in_ring[::-1], return_index=True) lidar_d_predictions_in_ring_unique_idx = \ (lidar_d_predictions_in_ring.shape[0] - 1 - lidar_d_predictions_in_ring_unique_idx_last + lidar_d_predictions_in_ring_unique_idx_first ) // 2 deinterpolated_class_predictions_by_angle_idx_this_ring = \ predictions_idx_in_ring[lidar_d_predictions_in_ring_unique_idx] deinterpolated_class_predictions_by_angle_idx = np.concatenate( (deinterpolated_class_predictions_by_angle_idx, deinterpolated_class_predictions_by_angle_idx_this_ring )) class_predictions_by_angle_idx = deinterpolated_class_predictions_by_angle_idx.astype( int) segmented_points = lidar_int[class_predictions_by_angle_idx[:, 0] + points_per_ring * class_predictions_by_angle_idx[:, 1]] # remove capture vehicle, helps in ford01.bag (and doesn't hurt) segmented_points = DidiTracklet._remove_capture_vehicle( segmented_points) # TODO: use PREDICTED position instead of last known for false positive rejection if reject_false_positives and last_known_position is not None and segmented_points.shape[ 0] > 2: original_number_of_points = segmented_points.shape[0] time_start = time.time() rfp_implementation = 2 if rfp_implementation == 1: import hdbscan clusterer = hdbscan.HDBSCAN(allow_single_cluster=True, metric='l2', min_cluster_size=50) cluster_labels = clusterer.fit_predict(segmented_points[:, :2]) number_of_clusters = len( set(cluster_labels)) - (1 if -1 in cluster_labels else 0) if verbose: print("Clusters " + str(number_of_clusters) + ' from ' + str(segmented_points.shape[0]) + ' points') if number_of_clusters > 1: unique_clusters = set(cluster_labels) closest_cluster_center_of_mass = np.array([1e20, 1e20]) best_cluster = None for cluster in unique_clusters: points_in_cluster = segmented_points[cluster_labels == cluster] points_in_cluster_center_of_mass = np.mean( points_in_cluster[:, :2], axis=0) if verbose: print(cluster, points_in_cluster.shape, points_in_cluster_center_of_mass) if cluster != -1: if np.linalg.norm(closest_cluster_center_of_mass - last_known_position[:2]) >\ np.linalg.norm(points_in_cluster_center_of_mass - last_known_position[:2]): closest_cluster_center_of_mass = points_in_cluster_center_of_mass best_cluster = cluster #if verbose: print("best cluster", best_cluster, 'last_known_position at ', last_known_position) selected_clusters = [best_cluster] for cluster in unique_clusters: if (cluster != -1) and (cluster != best_cluster): points_in_cluster = segmented_points[cluster_labels == cluster] distances_from_last_known_position = np.linalg.norm( points_in_cluster[:, :2] - closest_cluster_center_of_mass, axis=1) if np.all(distances_from_last_known_position < 2. ): # TODO ADJUST selected_clusters.append(cluster) filtered_points_xyz = segmented_points[np.in1d( cluster_labels, selected_clusters, invert=True), :3] segmented_points = segmented_points[np.in1d( cluster_labels, selected_clusters)] if verbose: print('selected_clusters: ' + str(selected_clusters) + ' with points ' + str(segmented_points.shape[0]) + '/' + str(original_number_of_points)) else: within_tolerance_idx = ( ((segmented_points[:, 0] - last_known_position[0])**2 + (segmented_points[:, 0] - last_known_position[0])**2) <= (5**2)) filtered_points_xyz = segmented_points[ ~within_tolerance_idx, :3] if filtered_points_xyz.shape[0] < original_number_of_points: segmented_points = segmented_points[within_tolerance_idx] if verbose: print 'clustering filter: {:.2f}ms'.format( 1e3 * (time.time() - time_start)) else: segmented_points = np.empty((0, 3)) detection = 0 pose = np.zeros((3)) box_size = np.zeros((3)) yaw = np.zeros((1)) if segmented_points.shape[0] >= localizer_points_threshold: detection = 1 # filter outlier points if True: time_start = time.time() cloud_orig = pcl.PointCloud(segmented_points[:, :3].astype( np.float32)) fil = cloud_orig.make_statistical_outlier_filter() fil.set_mean_k(50) fil.set_std_dev_mul_thresh(1.0) inlier_inds = fil.filter_ind() segmented_points = segmented_points[inlier_inds, :] if verbose: print 'point cloud filter: {:.2f}ms'.format( 1e3 * (time.time() - time_start)) #filtered_points_xyz = segmented_points[:,:3] segmented_points_mean = np.mean(segmented_points[:, :3], axis=0) angle = np.arctan2(segmented_points_mean[1], segmented_points_mean[0]) aligned_points = point_utils.rotZ(segmented_points, angle) segmented_and_aligned_points_mean = np.mean(aligned_points[:, :3], axis=0) aligned_points[:, :3] -= segmented_and_aligned_points_mean distance_to_segmented_and_aligned_points = np.linalg.norm( segmented_and_aligned_points_mean[:2]) aligned_points_resampled = DidiTracklet.resample_lidar( aligned_points[:, :4], pointnet_points) aligned_points_resampled = np.expand_dims(aligned_points_resampled, axis=0) distance_to_segmented_and_aligned_points = np.expand_dims( distance_to_segmented_and_aligned_points, axis=0) if K._backend == 'tensorflow': with tf_localizer_graph.as_default(): time_loc_infe_start = time.time() pose, box_size, yaw = localizer_model.predict_on_batch([ aligned_points_resampled, distance_to_segmented_and_aligned_points ]) time_loc_infe_end = time.time() else: time_loc_infe_start = time.time() pose, box_size, yaw = localizer_model.predict_on_batch([ aligned_points_resampled, distance_to_segmented_and_aligned_points ]) time_loc_infe_end = time.time() pose = np.squeeze(pose, axis=0) box_size = np.squeeze(box_size, axis=0) yaw = np.squeeze(yaw, axis=0) if verbose: print ' Loc inference: %0.3f ms' % ( (time_loc_infe_end - time_loc_infe_start) * 1000.0) pose += segmented_and_aligned_points_mean pose = point_utils.rotZ(pose, -angle) yaw = point_utils.remove_orientation(yaw + angle) pose_angle = np.arctan2(pose[1], pose[0]) angle_diff = angle_at_edge - pose_angle if angle_diff < 0.: angle_diff += 2 * np.pi # ALI => delta_time is the time difference in milliseconds (0-100) from the start of the lidar # scan to the time the object was detected, i don' know if the lidar msg is referenced to be # beginning of the scan or the end... so basically adjust the lidar observation for cases which # we need to test: # observation_time = msg.header.stamp.to_sec() + delta_time # observation_time = msg.header.stamp.to_sec() - delta_time # observation_time = msg.header.stamp.to_sec() + 100 msecs - delta_time delta_time = 0.1 * angle_diff / (2 * np.pi) if verbose: print(angle_at_edge, pose_angle, angle_diff) if verbose: print(pose, box_size, yaw, delta_time) if delta_time < 0: print(angle_at_edge, pose_angle, angle_diff, delta_time) # fix lidar static tilt Rx = rotXMat(np.deg2rad(g_roll_correction)) Ry = rotYMat(np.deg2rad(g_pitch_correction)) Rz = rotZMat(np.deg2rad(g_yaw_correction)) pose = Rz.dot(Ry.dot(Rx.dot([pose[0], pose[1], pose[2]]))) pose[2] += g_z_correction # scale bbox size box_size[2] = g_bbox_scale_l * box_size[2] box_size[1] = g_bbox_scale_w * box_size[1] box_size[0] = g_bbox_scale_h * box_size[0] last_known_position = pose last_known_box_size = box_size # FUSION with g_fusion_lock: observation = LidarObservation(msg.header.stamp.to_sec(), pose[0], pose[1], pose[2], yaw) g_fusion.filter(observation) segmented_points_cloud_msg = pc2.create_cloud_xyz32( msg.header, segmented_points[:, :3]) # publish car prediction data as separate regular ROS messages just for vizualization (dunno how to visualize custom messages in rviz) publish_rviz_topics = False if publish_rviz_topics and detection > 0: # point cloud seg_pnt_pub = rospy.Publisher(name='segmented_obj', data_class=PointCloud2, queue_size=1) seg_msg = PointCloud2() seg_pnt_pub.publish(segmented_points_cloud_msg) # car pose frame yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, yaw) br = ros_tf.TransformBroadcaster() br.sendTransform(tuple(pose), tuple(yaw_q), rospy.Time.now(), 'obj_lidar_centroid', 'velodyne') # give bbox different color, depending on the predicted object class if detection == 1: # car bbox_color = ColorRGBA(r=1., g=1., b=0., a=0.5) else: # ped bbox_color = ColorRGBA(r=0., g=0., b=1., a=0.5) # bounding box marker = Marker() marker.header.frame_id = "obj_lidar_centroid" marker.header.stamp = rospy.Time.now() marker.type = Marker.CUBE marker.action = Marker.ADD marker.scale.x = box_size[2] marker.scale.y = box_size[1] marker.scale.z = box_size[0] marker.color = bbox_color marker.lifetime = rospy.Duration() pub = rospy.Publisher("obj_lidar_bbox", Marker, queue_size=10) pub.publish(marker) # filtered point cloud fil_points_msg = pc2.create_cloud_xyz32(msg.header, filtered_points_xyz) rospy.Publisher(name='segmented_filt_obj', data_class=PointCloud2, queue_size=1).publish(fil_points_msg) print "TIME: ", time.time() - t_start return { 'detection': detection, 'x': pose[0], 'y': pose[1], 'z': pose[2], 'l': box_size[2], 'w': box_size[1], 'h': box_size[0], 'yaw': np.squeeze(yaw) }
def node(): global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count rospy.init_node('filter', anonymous=False) #rospy.loginfo('filter and send: I am here') # fetching all parameters map_topic = rospy.get_param('~map_topic', '/map') threshold = rospy.get_param('~costmap_clearing_threshold', 70) # this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate info_radius = rospy.get_param('~info_radius', 1.0) goals_topic = rospy.get_param('~goals_topic', '/detected_points') n_robots = rospy.get_param('~n_robots', 1) namespace = rospy.get_param('~namespace', '') namespace_init_count = rospy.get_param('namespace_init_count', 1) rateHz = rospy.get_param('~rate', 100) global_costmap_topic = rospy.get_param( '~global_costmap_topic', '/move_base/global_costmap/costmap' ) #Initially /move_base_node/global_costmap/costmap robot_frame = rospy.get_param('~robot_frame', 'base_link') litraIndx = len(namespace) rate = rospy.Rate(rateHz) # ------------------------------------------- rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) # --------------------------------------------------------------------------------------------------------------- for i in range(0, n_robots): globalmaps.append(OccupancyGrid()) if len(namespace) > 0: for i in range(0, n_robots): rospy.Subscriber( namespace + str(i + namespace_init_count) + global_costmap_topic, OccupancyGrid, globalMap) elif len(namespace) == 0: rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap) # wait if map is not received yet while (len(mapData.data) < 1): rospy.loginfo('Waiting for the map') rospy.sleep(0.1) pass # wait if any of robots' global costmap map is not received yet for i in range(0, n_robots): while (len(globalmaps[i].data) < 1): rospy.loginfo('Waiting for the global costmap') rospy.sleep(0.1) pass global_frame = "/" + mapData.header.frame_id tfLisn = tf.TransformListener() if len(namespace) > 0: for i in range(0, n_robots): tfLisn.waitForTransform( global_frame[1:], namespace + str(i + namespace_init_count) + '/' + robot_frame, rospy.Time(0), rospy.Duration(10.0)) elif len(namespace) == 0: tfLisn.waitForTransform(global_frame[1:], '/' + robot_frame, rospy.Time(0), rospy.Duration(10.0)) rospy.Subscriber(goals_topic, PointStamped, callback=callBack, callback_args=[tfLisn, global_frame[1:]]) pub = rospy.Publisher('frontiers', Marker, queue_size=10) pub2 = rospy.Publisher('centroids', Marker, queue_size=10) filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10) rospy.loginfo("the map and global costmaps are received") # wait if no frontier is received yet while len(frontiers) < 1: pass #rospy.loginfo('filter and send: I am here') points = Marker() points_clust = Marker() # Set the frame ID and timestamp. See the TF tutorials for information on these. points.header.frame_id = mapData.header.frame_id points.header.stamp = rospy.Time.now() points.ns = "markers2" points.id = 0 points.type = Marker.POINTS # Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points.action = Marker.ADD points.pose.orientation.w = 1.0 points.scale.x = 0.2 points.scale.y = 0.2 points.color.r = 255.0 / 255.0 points.color.g = 255.0 / 255.0 points.color.b = 0.0 / 255.0 points.color.a = 1 points.lifetime = rospy.Duration() p = Point() p.z = 0 pp = [] pl = [] points_clust.header.frame_id = mapData.header.frame_id points_clust.header.stamp = rospy.Time.now() points_clust.ns = "markers3" points_clust.id = 4 points_clust.type = Marker.POINTS # Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points_clust.action = Marker.ADD points_clust.pose.orientation.w = 1.0 points_clust.scale.x = 0.2 points_clust.scale.y = 0.2 points_clust.color.r = 0.0 / 255.0 points_clust.color.g = 255.0 / 255.0 points_clust.color.b = 0.0 / 255.0 points_clust.color.a = 1 points_clust.lifetime = rospy.Duration() temppoint = PointStamped() temppoint.header.frame_id = mapData.header.frame_id temppoint.header.stamp = rospy.Time(0) temppoint.point.z = 0.0 arraypoints = PointArray() tempPoint = Point() tempPoint.z = 0.0 # ------------------------------------------------------------------------- # --------------------- Main Loop ------------------------------- # ------------------------------------------------------------------------- while not rospy.is_shutdown(): # ------------------------------------------------------------------------- # Clustering frontier points centroids = [] front = copy(frontiers) if len(front) > 1: ms = MeanShift(bandwidth=0.3) ms.fit(front) centroids = ms.cluster_centers_ # centroids array is the centers of each cluster # if there is only one frontier no need for clustering, i.e. centroids=frontiers if len(front) == 1: centroids = front # ------------------------------------------------------------------------- # clearing old frontiers z = 0 while z < len(centroids): cond = False temppoint.point.x = centroids[z][0] temppoint.point.y = centroids[z][1] for i in range(0, n_robots): transformedPoint = tfLisn.transformPoint( globalmaps[i].header.frame_id, temppoint) x = array([transformedPoint.point.x, transformedPoint.point.y]) cond = (gridValue(globalmaps[i], x) > threshold) or cond if (cond or (informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius * 0.5)) < 0.2): centroids = delete(centroids, (z), axis=0) z = z - 1 z += 1 frontiers = copy(centroids) #Change this to after the cleannup # ------------------------------------------------------------------------- # publishing arraypoints.points = [] for i in centroids: tempPoint.x = i[0] tempPoint.y = i[1] arraypoints.points.append(copy(tempPoint)) filterpub.publish(arraypoints) pp = [] for q in range(0, len(frontiers)): p.x = frontiers[q][0] p.y = frontiers[q][1] pp.append(copy(p)) points.points = pp pp = [] for q in range(0, len(centroids)): p.x = centroids[q][0] p.y = centroids[q][1] pp.append(copy(p)) points_clust.points = pp pub.publish(points) pub2.publish(points_clust) rate.sleep()
def plotPolygons(self): # https://answers.ros.org/question/203782/rviz-marker-line_strip-is-not-displayed/ marker = Marker() marker.header.frame_id = "yumi_base_link" marker.type = marker.LINE_LIST marker.action = marker.ADD # marker scale marker.scale.x = 0.002 marker.scale.y = 0.002 marker.scale.z = 0.002 # marker color marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 # marker orientaiton 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 position marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 objectList = self.objects # marker line points marker.points = [] for j in range(len(objectList)): for ii in range(objectList[j].lines.shape[0]): lineIndex = objectList[j].lines[ii] lineVerticeis = objectList[j].verticesTransformed[lineIndex] height = objectList[j].heightTransformed x_data = lineVerticeis[:, 0].tolist() y_data = lineVerticeis[:, 1].tolist() point1 = Point() point1.x = x_data[0] point1.y = y_data[0] point1.z = height[0] marker.points.append(point1) point2 = Point() point2.x = x_data[1] point2.y = y_data[1] point2.z = height[0] marker.points.append(point2) point3 = Point() point3.x = x_data[0] point3.y = y_data[0] point3.z = height[1] marker.points.append(point1) marker.points.append(point3) point4 = Point() point4.x = x_data[1] point4.y = y_data[1] point4.z = height[1] marker.points.append(point2) marker.points.append(point4) point5 = Point() point5.x = x_data[0] point5.y = y_data[0] point5.z = height[1] marker.points.append(point5) point6 = Point() point6.x = x_data[1] point6.y = y_data[1] point6.z = height[1] marker.points.append(point6) self.linePub.publish(marker)
def forward_kinematics(data): if not correct(data): rospy.logerr('Incorrect position! ' + str(data)) return a, d, al, th = params['i1'] al, a, d, th = float(al), float(a), float(d), float(th) tz = translation_matrix((0, 0, d)) rz = rotation_matrix(data.position[0], zaxis) tx = translation_matrix((a, 0, 0)) rx = rotation_matrix(al, xaxis) T1 = concatenate_matrices(rx, tx, rz, tz) a, d, al, th = params['i2'] al, a, d, th = float(al), float(a), float(d), float(th) tz = translation_matrix((0, 0, d)) rz = rotation_matrix(data.position[1], zaxis) tx = translation_matrix((a, 0, 0)) rx = rotation_matrix(al, xaxis) T2 = concatenate_matrices(rx, tx, rz, tz) a, d, al, th = params['i3'] al, a, d, th = float(al), float(a), float(d), float(th) tz = translation_matrix((0, 0, data.position[2])) rz = rotation_matrix(th, zaxis) tx = translation_matrix((a, 0, 0)) rx = rotation_matrix(al, xaxis) T3 = concatenate_matrices(rx, tx, rz, tz) Tk = concatenate_matrices(T1, T2, T3) x, y, z = translation_from_matrix(Tk) qx, qy, qz, qw = quaternion_from_matrix(Tk) pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z pose.pose.orientation.x = qx pose.pose.orientation.y = qy pose.pose.orientation.z = qz pose.pose.orientation.w = qw pub.publish(pose) marker = Marker() marker.header.frame_id = 'base_link' marker.type = marker.CUBE marker.action = marker.ADD marker.pose.orientation.w = 1 time = rospy.Duration() marker.lifetime = time marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = z marker.pose.orientation.x = qx marker.pose.orientation.y = qy marker.pose.orientation.z = qz marker.pose.orientation.w = qw marker.color.a = 0.7 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker_pub.publish(marker)
def publish(self, target_frame, timestamp): pose = PoseStamped() pose.header.frame_id = target_frame pose.header.stamp = timestamp pose.pose.position.x = self.X[0, 0] pose.pose.position.y = self.X[1, 0] pose.pose.position.z = 0.0 Q = tf.transformations.quaternion_from_euler(0, 0, self.X[2, 0]) pose.pose.orientation.x = Q[0] pose.pose.orientation.y = Q[1] pose.pose.orientation.z = Q[2] pose.pose.orientation.w = Q[3] self.pose_pub.publish(pose) ma = MarkerArray() marker = Marker() marker.header = pose.header marker.ns = "kf_uncertainty" marker.id = 5000 marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose = pose.pose marker.pose.position.z = -0.1 marker.scale.x = 3 * sqrt(self.P[0, 0]) marker.scale.y = 3 * sqrt(self.P[1, 1]) marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 1.0 ma.markers.append(marker) for id in self.idx.iterkeys(): marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = target_frame marker.ns = "landmark_kf" marker.id = id marker.type = Marker.CYLINDER marker.action = Marker.ADD l = self.idx[id] marker.pose.position.x = self.X[l, 0] marker.pose.position.y = self.X[l + 1, 0] marker.pose.position.z = -0.1 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.scale.x = 3 * sqrt(self.P[l, l]) marker.scale.y = 3 * sqrt(self.P[l + 1, l + 1]) marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.lifetime.secs = 3.0 ma.markers.append(marker) marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = target_frame marker.ns = "landmark_kf" marker.id = 1000 + id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.pose.position.x = self.X[l + 0, 0] marker.pose.position.y = self.X[l + 1, 0] marker.pose.position.z = 1.0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.text = str(id) marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 0.2 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.lifetime.secs = 3.0 ma.markers.append(marker) self.marker_pub.publish(ma)
def gnd_marker_pub(gnd_label, marker_pub, cfg, color = "red"): length = int(cfg.grid_range[2] - cfg.grid_range[0]) # x direction width = int(cfg.grid_range[3] - cfg.grid_range[1]) # y direction gnd_marker = Marker() gnd_marker.header.frame_id = "/kitti/base_link" gnd_marker.header.stamp = rospy.Time.now() gnd_marker.type = gnd_marker.LINE_LIST gnd_marker.action = gnd_marker.ADD gnd_marker.scale.x = 0.05 gnd_marker.scale.y = 0.05 gnd_marker.scale.z = 0.05 if(color == "red"): gnd_marker.color.a = 1.0 gnd_marker.color.r = 1.0 gnd_marker.color.g = 0.0 gnd_marker.color.b = 0.0 if(color == "green"): gnd_marker.color.a = 1.0 gnd_marker.color.r = 0.0 gnd_marker.color.g = 1.0 gnd_marker.color.b = 0.0 gnd_marker.points = [] # gnd_labels are arranged in reverse order for j in range(gnd_label.shape[0]): for i in range(gnd_label.shape[1]): pt1 = Point() pt1.x = i + cfg.grid_range[0] pt1.y = j + cfg.grid_range[1] pt1.z = gnd_label[j,i] if j>0 : pt2 = Point() pt2.x = i + cfg.grid_range[0] pt2.y = j-1 +cfg.grid_range[1] pt2.z = gnd_label[j-1, i] gnd_marker.points.append(pt1) gnd_marker.points.append(pt2) if i>0 : pt2 = Point() pt2.x = i -1 + cfg.grid_range[0] pt2.y = j + cfg.grid_range[1] pt2.z = gnd_label[j, i-1] gnd_marker.points.append(pt1) gnd_marker.points.append(pt2) if j < width-1 : pt2 = Point() pt2.x = i + cfg.grid_range[0] pt2.y = j + 1 + cfg.grid_range[1] pt2.z = gnd_label[j+1, i] gnd_marker.points.append(pt1) gnd_marker.points.append(pt2) if i < length-1 : pt2 = Point() pt2.x = i + 1 + cfg.grid_range[0] pt2.y = j + cfg.grid_range[1] pt2.z = gnd_label[j, i+1] gnd_marker.points.append(pt1) gnd_marker.points.append(pt2) marker_pub.publish(gnd_marker)
def collision_state(): global ms, co_svc, root_link_name, root_link_offset, last_collision_status diagnostic = DiagnosticArray() now = rospy.Time.now() diagnostic.header.stamp = now if not co.isActive(): rospy.loginfo("co is not activated") return collision_status = co_svc.getCollisionStatus() if (now - last_collision_status > rospy.Duration(5.0)): num_collision_pairs = len(collision_status[1].lines) rospy.loginfo( "check %d collision status, %f Hz", num_collision_pairs, num_collision_pairs / (now - last_collision_status).to_sec()) last_collision_status = now # check if ther are collision status = DiagnosticStatus(name='CollisionDetector', level=DiagnosticStatus.OK, message="Ok") #if any(a): # this calls omniORB.any for collide in collision_status[1].collide: if collide: status.level = DiagnosticStatus.ERROR status.message = "Robots is in collision mode" status.values.append( KeyValue(key="Time", value=str(collision_status[1].time))) status.values.append( KeyValue(key="Computation Time", value=str(collision_status[1].computation_time))) status.values.append( KeyValue(key="Safe Posture", value=str(collision_status[1].safe_posture))) status.values.append( KeyValue(key="Recover Time", value=str(collision_status[1].recover_time))) status.values.append( KeyValue(key="Loop for check", value=str(collision_status[1].loop_for_check))) frame_id = root_link_name # root id markerArray = MarkerArray() for line in collision_status[1].lines: p1 = Point(*(numpy.dot(root_link_offset[3, 3], line[0]) + root_link_offset[0:3, 3])) p2 = Point(*(numpy.dot(root_link_offset[3, 3], line[1]) + root_link_offset[0:3, 3])) sphere_color = ColorRGBA(0, 1, 0, 1) line_width = 0.01 line_length = numpy.linalg.norm( numpy.array((p1.x, p1.y, p1.z)) - numpy.array((p2.x, p2.y, p2.z))) sphere_scale = 0.02 # color changes between 0.145(green) -> 0.02(red), under 0.02, it always red if (line_length <= 0.145): sphere_scale = 0.02 if (line_length <= 0.0002): sphere_scale = 0.045 sphere_color = ColorRGBA(1, 0, 1, 1) ## color is purple, if collide elif (line_length < 0.02): sphere_scale = 0.04 sphere_color = ColorRGBA(1, 0, 0, 1) ## color is red else: ratio = 1.0 - (line_length - 0.02) * 8 # 0.0 (0.145) -> 1.0 ( 0.02) sphere_scale = 0.02 + ratio * 0.02 # 0.02 -> 0.04 sphere_color = ColorRGBA(ratio, 1 - ratio, 0, 1) # green -> red marker = Marker() marker.header.frame_id = frame_id marker.type = marker.LINE_LIST marker.action = marker.ADD marker.color = sphere_color marker.points = [p1, p2] marker.scale.x = line_width markerArray.markers.append(marker) sphere_scale = Vector3(sphere_scale, sphere_scale, sphere_scale) marker = Marker() marker.header.frame_id = frame_id marker.type = marker.SPHERE marker.action = marker.ADD marker.scale = sphere_scale marker.color = sphere_color marker.pose.orientation.w = 1.0 marker.pose.position = p1 markerArray.markers.append(marker) marker = Marker() marker.header.frame_id = frame_id marker.type = marker.SPHERE marker.action = marker.ADD marker.scale = sphere_scale marker.color = sphere_color marker.pose.orientation.w = 1.0 marker.pose.position = p2 markerArray.markers.append(marker) id = 0 for m in markerArray.markers: m.lifetime = rospy.Duration(1.0) m.id = id id += 1 pub_collision.publish(markerArray) diagnostic.status.append(status) pub_diagnostics.publish(diagnostic)
punkte = [] for h in np.arange(0.1, 12.7, 0.2): hilfspunkt = Point() hilfspunkt.x = f(h) hilfspunkt.y = f2(h) hilfspunkt.z = 0.0 punkte.append(hilfspunkt) punkte.append(punkte[0]) marker = Marker() marker.header.frame_id = "map" marker.ns = "road" marker.id = 0 marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.y = 0.0 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 = punkte marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0
def yolo_darknet(self, img, depth_image, imgstamp): cv2.imwrite('/home/ubuntu/SSD-Tensorflow/test.jpg', img) yolo_results = {} yolo_results[0] = "name" yolo_results[1] = "" #"x" yolo_results[2] = "" #"y" yolo_results[3] = "" #"w" yolo_results[4] = "" #"h" name = "" #global frame global flag_find #frame += 1 #if frame%10 == 0: yolo_process = Popen(['python', 'darknet_test.py'], stdout=PIPE, stderr=STDOUT) results = "" for line in yolo_process.stdout: results += line.decode('utf-8', "replace") i = 0 '''with open('/home/ubuntu/SSD-Tensorflow/result_yolo.txt', mode = 'rt') as f: txtdata = list(f) print txtdata[0].strip('\n') print int(txtdata[4]) name = txtdata[0].strip('\n') x = int(txtdata[1]) y = int(txtdata[2]) w = int(txtdata[3]) h = int(txtdata[4])''' name, x, y, w, h = read_file() if name != " ": flag_find = 1 else: flag_find = 0 rx = 0.0 ry = 0.0 rw = 0.0 rh = 0.0 if flag_find == 1: name, x, y, w, h = read_file() rx = (x - w / 2) / 320 ry = (y - h / 2) / 240 rw = (x + w / 2) / 320 rh = (y + h / 2) / 240 x = int(x) y = int(y) w = int(w) h = int(h) cv2.rectangle(img, (int(x - w / 2), int(y - h / 2)), (int(x + w / 2), int(y + h / 2)), (255, 0, 0), 2) roi_color = img[y:y + h, x:x + w] cv2.putText(img, name, (int(x - w / 2), int(y - h / 2)), cv2.FONT_HERSHEY_DUPLEX, 0.4, (255, 0, 0), 1) print(rx, ry, rw, rh) self.distance = [] distance_count = [] distance_r = 0.10 self.center = 0.0 x1 = x - w / 2 y1 = y - h / 2 x2 = x + w / 2 y2 = y + h / 2 if (x1 < 0): x1 = 0 if (y1 < 0): y1 = 0 if (x2 > 320): x2 = 319 if (y2 > 240): y2 = 239 img.flags.writeable = True for i in range(y1, y2 + 1): for j in range(x1, x2 + 1): if depth_image.item(i, j) == depth_image.item(i, j): self.distance.append((depth_image.item(i, j), 0)) img.itemset((i, j, 0), 0) img.itemset((i, j, 1), 0) print(len(self.distance)) if (len(self.distance) == 0): return name, rx, ry, rw, rh if (len(self.distance) != 0): #self.center = median(self.distance) distance_count = [0] * len(self.distance) distance_kd_tree = ss.KDTree(self.distance, leafsize=10) distance_res = list(distance_kd_tree.query_pairs(distance_r)) for j in range(len(distance_res)): for k in range(2): distance_count[distance_res[j][k]] += 1 max_index = distance_count.index(max(distance_count)) self.center = self.distance[max_index][0] - 0.15 print("%f [m]" % self.center) tmp = math.radians((((x2 + x1) / 2.0) - (320.0 / 2.0)) * (self.depth_horizon / 320.0)) self.robot_x = abs(self.center * math.cos(tmp)) if (x2 + x1 / 2.0) < 160: self.robot_y = abs(self.center * math.sin(tmp)) else: self.robot_y = -abs(self.center * math.sin(-tmp)) print(self.robot_x, self.robot_y) #TF transformation point_result = PointStamped() point_result.header.frame_id = "base_footprint" #point_result.header.stamp = rospy.Time(0) point_result.header.stamp = imgstamp self._latest_point = point_result point_result.point.x = self.robot_x point_result.point.y = self.robot_y point_result.point.z = 0.0 print("###transformation") try: self._latest_point = self._tf_listener.transformPoint( "/map", point_result) #self._latest_point = self._tf_listener.transformPoint("/map",rospy.Time.now(),imgstamp,point_result,"/map") #self._latest_point = self._tf_listener.transformPoint("/map",rospy.Time(0),imgstamp,point_result,"/map") except (rostf.LookupException, rostf.ConnectivityException, rostf.ExtrapolationException) as e: rospy.loginfo("Exception: {}".format(e)) print(self._latest_point.point.x, self._latest_point.point.y) marker_data = Marker() marker_data.header.frame_id = "map" marker_data.header.stamp = rospy.Time.now() marker_data.ns = "text" marker_data.id = 0 marker_data.action = Marker.ADD marker_data.type = 9 marker_data.text = "object" marker_data.pose.position.x = self._latest_point.point.x marker_data.pose.position.y = self._latest_point.point.y marker_data.pose.position.z = 0.0 marker_data.pose.orientation.x = 0.0 marker_data.pose.orientation.y = 0.0 marker_data.pose.orientation.z = 0.0 marker_data.pose.orientation.w = 0.0 marker_data.color.r = 1.0 marker_data.color.g = 0.0 marker_data.color.b = 0.0 marker_data.color.a = 1.0 marker_data.scale.x = 1.0 marker_data.scale.y = 0.1 marker_data.scale.z = 0.1 self.marker_pub.publish(marker_data) #print("#Delay:", (rospy.Time.now()-rgb_data.header.stamp).to_sec()) if (name == "banana"): self.banana_data.append( (self._latest_point.point.x, self._latest_point.point.y)) self.data_count = [0] * len(self.banana_data) kd_tree = ss.KDTree(self.banana_data, leafsize=10) res = list(kd_tree.query_pairs(self.r)) for j in range(len(res)): for k in range(2): self.data_count[res[j][k]] += 1 print("------- self.data_coount -------") print(self.data_count) if (max(self.data_count) != 0): max_index = self.data_count.index(max(self.data_count)) print(self.banana_data[max_index]) print("max_data_count:", max(self.data_count)) else: max_index = 0 marker_voting = Marker() marker_voting.header.frame_id = "map" marker_voting.header.stamp = rospy.Time.now() marker_voting.ns = "voting_point" marker_voting.id = 0 marker_voting.action = Marker.ADD marker_voting.type = 9 marker_voting.text = "BANANA" marker_voting.pose.position.x = self.banana_data[max_index][0] marker_voting.pose.position.y = self.banana_data[max_index][1] marker_voting.pose.position.z = 0.0 marker_voting.pose.orientation.x = 0.0 marker_voting.pose.orientation.y = 0.0 marker_voting.pose.orientation.z = 0.0 marker_voting.pose.orientation.w = 0.0 marker_voting.color.r = 0.0 marker_voting.color.g = 0.0 marker_voting.color.b = 1.0 marker_voting.color.a = 1.0 marker_voting.scale.x = 1.0 marker_voting.scale.y = 0.1 marker_voting.scale.z = 0.1 self.marker_voting_banana_pub.publish(marker_voting) #cv2.namedWindow("color_image") #cv2.imshow("color_image", img) #cv2.waitKey(10) self._pub_banana_point.publish( Float32MultiArray(data=[ self.banana_data[max_index][0], self.banana_data[max_index][1] ])) #self._pub_banana_point.publish(Float32MultiArray(data=[self.banana_data[max_index][0]-0.2, self.banana_data[max_index][1]-0.2])) elif (name == "apple"): self.apple_data.append( (self._latest_point.point.x, self._latest_point.point.y)) self.data_count = [0] * len(self.apple_data) kd_tree = ss.KDTree(self.apple_data, leafsize=10) res = list(kd_tree.query_pairs(self.r)) for j in range(len(res)): for k in range(2): self.data_count[res[j][k]] += 1 print("------- self.data_coount -------") print(self.data_count) if (max(self.data_count) != 0): max_index = self.data_count.index(max(self.data_count)) print(self.apple_data[max_index]) print("max_data_count:", max(self.data_count)) else: max_index = 0 marker_voting = Marker() marker_voting.header.frame_id = "map" marker_voting.header.stamp = rospy.Time.now() marker_voting.ns = "voting_point" marker_voting.id = 0 marker_voting.action = Marker.ADD marker_voting.type = 9 marker_voting.text = "APPLE" marker_voting.pose.position.x = self.apple_data[max_index][0] marker_voting.pose.position.y = self.apple_data[max_index][1] marker_voting.pose.position.z = 0.0 marker_voting.pose.orientation.x = 0.0 marker_voting.pose.orientation.y = 0.0 marker_voting.pose.orientation.z = 0.0 marker_voting.pose.orientation.w = 0.0 marker_voting.color.r = 0.0 marker_voting.color.g = 0.0 marker_voting.color.b = 1.0 marker_voting.color.a = 1.0 marker_voting.scale.x = 1.0 marker_voting.scale.y = 0.1 marker_voting.scale.z = 0.1 self.marker_voting_apple_pub.publish(marker_voting) #cv2.namedWindow("color_image") #cv2.imshow("color_image", img) #cv2.waitKey(10) self._pub_apple_point.publish( Float32MultiArray(data=[ self.apple_data[max_index][0], self.apple_data[max_index][1] ])) #self._pub_apple_point.publish(Float32MultiArray(data=[self.apple_data[max_index][0]-0.2, self.apple_data[max_index][1]-0.2])) elif (name == "cable"): self.cable_data.append( (self._latest_point.point.x, self._latest_point.point.y)) self.data_count = [0] * len(self.cable_data) kd_tree = ss.KDTree(self.cable_data, leafsize=10) res = list(kd_tree.query_pairs(self.r)) for j in range(len(res)): for k in range(2): self.data_count[res[j][k]] += 1 print("------- self.data_coount -------") print(self.data_count) if (max(self.data_count) != 0): max_index = self.data_count.index(max(self.data_count)) print(self.cable_data[max_index]) print("max_data_count:", max(self.data_count)) else: max_index = 0 marker_voting = Marker() marker_voting.header.frame_id = "map" marker_voting.header.stamp = rospy.Time.now() marker_voting.ns = "voting_point" marker_voting.id = 0 marker_voting.action = Marker.ADD marker_voting.type = 9 marker_voting.text = "CABLE" marker_voting.pose.position.x = self.cable_data[max_index][0] marker_voting.pose.position.y = self.cable_data[max_index][1] marker_voting.pose.position.z = 0.0 marker_voting.pose.orientation.x = 0.0 marker_voting.pose.orientation.y = 0.0 marker_voting.pose.orientation.z = 0.0 marker_voting.pose.orientation.w = 0.0 marker_voting.color.r = 0.0 marker_voting.color.g = 0.0 marker_voting.color.b = 1.0 marker_voting.color.a = 1.0 marker_voting.scale.x = 1.0 marker_voting.scale.y = 0.1 marker_voting.scale.z = 0.1 self.marker_voting_cable_pub.publish(marker_voting) #cv2.namedWindow("color_image") #cv2.imshow("color_image", img) #cv2.waitKey(10) #self._pub_cable_point.publish(Float32MultiArray(data=[self.cable_data[max_index][0]-0.2, self.cable_data[max_index][1]-0.2])) #self._pub_cable_point.publish(Float32MultiArray(data=[self.cable_data[max_index][0], self.cable_data[max_index][1]])) self.latest_cable_x = self.cable_data[max_index][0] self.latest_cable_y = self.cable_data[max_index][1] return name, rx, ry, rw, rh
import numpy as np rospy.init_node("marker_node", anonymous=True) pub = rospy.Publisher("marker_test", Marker, queue_size=10) rate = rospy.Rate(25) while not rospy.is_shutdown(): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.id = 0 marker.action = Marker.ADD marker.lifetime = rospy.Duration() marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://kitti/bmw_x5/BMW X5 4.dae" # marker.mesh_use_embedded_materials = True marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = -1.73 q = tf.transformations.quaternion_from_euler(np.pi / 2, 0, np.pi) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] marker.color.r = 1.0 marker.color.g = 1.0
def callback(msg, tmp_list): """""" global old_yaw [ particle_filter, publisher_position, publisher_mavros, publisher_particles, broadcaster, publisher_marker ] = tmp_list # particle filter algorithm particle_filter.predict() # move particles # get length of message num_meas = len(msg.detections) orientation_yaw_pitch_roll = np.zeros((num_meas, 3)) # if new measurement: update particles if num_meas >= 1: measurements = np.zeros((num_meas, 1 + PART_DIM)) # get data from topic /tag_detection if rviz: markerArray = MarkerArray() for i, tag in enumerate(msg.detections): tag_id = int(tag.id[0]) distance = np.array(([ tag.pose.pose.pose.position.x, tag.pose.pose.pose.position.y, tag.pose.pose.pose.position.z ])) measurements[i, 0] = np.linalg.norm(distance) tmpquat = Quaternion(w=tag.pose.pose.pose.orientation.w, x=tag.pose.pose.pose.orientation.x, y=tag.pose.pose.pose.orientation.y, z=tag.pose.pose.pose.orientation.z) orientation_yaw_pitch_roll[i, :] = tmpquat.inverse.yaw_pitch_roll index = np.where(tags[:, 0] == tag_id) measurements[i, 1:4] = tags[index, 1:4] # print(measurements[i, 1:4]) if rviz: marker = Marker() marker.header.frame_id = "global_tank" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = measurements[ i, 0] * 2 # r*2 of distance to camera from tag_14 marker.scale.y = measurements[i, 0] * 2 marker.scale.z = measurements[i, 0] * 2 marker.color.g = 1 marker.color.a = 0.1 # transparency marker.pose.orientation.w = 1.0 marker.pose.position.x = tags[index, 1][0] # x marker.pose.position.y = tags[index, 2][0] # y marker.pose.position.z = tags[index, 3][0] # z markerArray.markers.append(marker) if rviz: # print(len(markerArray.markers)) publisher_marker.publish(markerArray) # print(index) particle_filter.update(measurements) yaw = np.mean(orientation_yaw_pitch_roll[:, 0]) pitch = np.mean(orientation_yaw_pitch_roll[:, 1]) roll = np.mean(orientation_yaw_pitch_roll[:, 2]) else: yaw = old_yaw old_yaw = yaw # print "reale messungen: " + str(measurements) print("Angle yaw:", yaw * 180 / np.pi) estimated_orientation = yaw_pitch_roll_to_quat(-(yaw - np.pi / 2), 0, 0) # estimated_orientation = yaw_pitch_roll_to_quat(yaw, 0, 0)#evtl wrong # calculate position as mean of particle positions estimated_position = particle_filter.get_position_estimate() # [mm] x_mean_ned = estimated_position[ 0] * 1000 # global Tank Coordinate System(NED) y_mean_ned = estimated_position[1] * 1000 z_mean_ned = estimated_position[2] * 1000 # publish estimated_pose [m] in mavros to /mavros/vision_pose/pose # this pose needs to be in ENU mavros_position = PoseStamped() mavros_position.header.stamp = rospy.Time.now() mavros_position.header.frame_id = "map" mavros_position.pose.position.x = y_mean_ned / 1000 # NED Coordinate to ENU(ROS) mavros_position.pose.position.y = x_mean_ned / 1000 mavros_position.pose.position.z = -z_mean_ned / 1000 mavros_position.pose.orientation.w = estimated_orientation.w mavros_position.pose.orientation.x = estimated_orientation.x mavros_position.pose.orientation.y = estimated_orientation.y mavros_position.pose.orientation.z = estimated_orientation.z # publish estimated_pose [m] position = PoseStamped() position.header.stamp = rospy.Time.now() position.header.frame_id = "global_tank" # ned position.pose.position.x = x_mean_ned / 1000 position.pose.position.y = y_mean_ned / 1000 position.pose.position.z = z_mean_ned / 1000 estimated_orientation = yaw_pitch_roll_to_quat(yaw, 0, 0) position.pose.orientation.w = estimated_orientation.w position.pose.orientation.x = estimated_orientation.x position.pose.orientation.y = estimated_orientation.y position.pose.orientation.z = estimated_orientation.z publisher_position.publish(position) # yaw = 0 / 180.0 * np.pi # tmp = yaw_pitch_roll_to_quat(-(yaw-np.pi/2), 0, 0) # print(tmp) # mavros_position.pose.orientation.w = tmp.w # mavros_position.pose.orientation.x = tmp.x # mavros_position.pose.orientation.y = tmp.y # mavros_position.pose.orientation.z = tmp.z publisher_mavros.publish(mavros_position) # For Debugging # mavros_position = PoseStamped() # mavros_position.header.stamp = rospy.Time.now() # mavros_position.header.frame_id = "map" # mavros_position.pose.position.x = 1.0 + np.random.normal(0, 0.01) # mavros_position.pose.position.y = 2.0 + np.random.normal(0, 0.01) # mavros_position.pose.position.z = 3.0 + np.random.normal(0, 0.01) # # mavros_position.pose.orientation.w = 1.0 # mavros_position.pose.orientation.x = 2.0 # mavros_position.pose.orientation.y = 3.0 # mavros_position.pose.orientation.z = 4.0 # publisher_mavros.publish(mavros_position) """ # publish transform broadcaster.sendTransform((estimated_position[0], estimated_position[1], estimated_position[2]), (1.0, 0, 0, 0), rospy.Time.now(), "TestPose", "world") """ if rviz: # publish particles as PoseArray pose_array = PoseArray() pose_array.header.stamp = rospy.Time.now() pose_array.header.frame_id = "global_tank" # for i in range(num_meas): # print(orientation_yaw_pitch_roll[i, 0] * 180 / np.pi, orientation_yaw_pitch_roll[i, 1] * 180 / np.pi, # orientation_yaw_pitch_roll[i, 2] * 180 / np.pi) print("done") for i in range(particle_filter.NUM_P): pose = Pose() pose.position.x = particle_filter.particles[i, 0] pose.position.y = particle_filter.particles[i, 1] pose.position.z = particle_filter.particles[i, 2] # pose.orientation.x = # pose.orientation.y = # pose.orientation.z = # pose.orientation.w = pose_array.poses.append(pose) publisher_particles.publish(pose_array)
def callback(data): xaxis = (1,0,0) zaxis = (0,0,1) #ARM 1 a, d, al, th = dh['i1'] a, d, al, th = float(a), float(d), float(al), float(th) tz = translation_matrix((0, 0, d)) rz = rotation_matrix(data.position[0], zaxis) tx = translation_matrix((a, 0, 0)) rx = rotation_matrix(al, xaxis) T1 = concatenate_matrices(rx, tx, rz, tz) #ARM 2 a, d, al, th = dh['i2'] a, d, al, th = float(a), float(d), float(al), float(th) tz = translation_matrix((0, 0, d)) rz = rotation_matrix(data.position[1], zaxis) tx = translation_matrix((a, 0, 0)) rx = rotation_matrix(al, xaxis) T2 = concatenate_matrices(rx, tx, rz, tz) #ARM 3 a, d, al, th = dh['i3'] a, d, al, th = float(a), float(d), float(al), float(th) tz = translation_matrix((0, 0, data.position[2]-d)) rz = rotation_matrix(th, zaxis) tx = translation_matrix((a, 0, 0)) rx = rotation_matrix(al, xaxis) T3 = concatenate_matrices(rx, tx, rz, tz) #FINAL final_matrix = concatenate_matrices(T1, T2, T3) x, y, z = translation_from_matrix(final_matrix) qx, qy, qz, qw = quaternion_from_matrix(final_matrix) pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z+baseHeight pose.pose.orientation.x = qx pose.pose.orientation.y = qy pose.pose.orientation.z = qz pose.pose.orientation.w = qw pubP.publish(pose) marker = Marker() marker.header.frame_id = 'base_link' path.header.frame_id = 'base_link' marker.header.stamp = rospy.Time.now() path.header.stamp = rospy.Time.now() marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = z+baseHeight marker.pose.orientation.x = qx marker.pose.orientation.y = qy marker.pose.orientation.z = qz marker.pose.orientation.w = qw marker.color.a = 0.5 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0 path.poses.append(pose) pubM.publish(marker) pubH.publish(path) #SHOW MATRIX print("T1:") print(T1) print("T2:") print(T2) print("T3:") print(T3) print("FINAL:") print(final_matrix)
def interactive_callback(self, data): if (data.markers!=[]): eps=1 n = 0 self.markerArray.markers = [] self.obstacle_matrix = np.zeros( (self.map_size[0]/self.resolution+10,self.map_size[1]/self.resolution+10,2),dtype='f' ) self.obstacle_matrix.fill(-100) for index in range(0,2): self.obstacle=data.markers[index].pose x1,y1=[data.markers[index].pose.position.x*100,data.markers[index].pose.position.y*100] x_index=np.int(round((x1+self.map_origin[0])/self.resolution)) y_index=np.int(round((y1+self.map_origin[1])/self.resolution)) if ((x_index>-1)and(x_index<(self.map_size[0]/self.resolution-1))and(y_index>-1)and(y_index<(self.map_size[1]/self.resolution-1))): for i in range(-10,10): for j in range(-10,10): x_index_=x_index+i if (x_index_<0): x_index_=0 if (x_index_>((self.map_size[0]/self.resolution)-1)): x_index_=(self.map_size[0]/self.resolution-1) y_index_=y_index+j if (y_index_<0): y_index_=0 if (y_index_>((self.map_size[1]/self.resolution)-1)): y_index_=self.map_size[1]/self.resolution-1 theta=np.arctan2(self.matrix[x_index,y_index,1],self.matrix[x_index,y_index,0]) if ((((np.cos(theta)*i+np.sin(theta)*j)**2)/100+((np.sin(theta)*i-np.cos(theta)*j)**2)/10)<1.0): Lg= np.sqrt(self.matrix[x_index_,y_index_,0]**2+self.matrix[x_index_,y_index_,1]**2) Lo= np.sqrt(i**2+j**2) if (Lo!=0): if (self.obstacle_matrix[x_index_,y_index_,0]!=-100): self.obstacle_matrix[x_index_,y_index_,0]=self.obstacle_matrix[x_index_,y_index_,0]+self.matrix[x_index_,y_index_,0]+i*(Lg/Lo)*max(1.0,(4.0/(abs(Lo)+0.1))) self.obstacle_matrix[x_index_,y_index_,1]=self.obstacle_matrix[x_index_,y_index_,1]+self.matrix[x_index_,y_index_,1]+j*(Lg/Lo)*max(1.0,(4.0/(abs(Lo)+0.1))) else: self.obstacle_matrix[x_index_,y_index_,0]=self.matrix[x_index_,y_index_,0]+i*(Lg/Lo)*max(1.0,(3.0/(abs(Lo)+0.1))) self.obstacle_matrix[x_index_,y_index_,1]=self.matrix[x_index_,y_index_,1]+j*(Lg/Lo)*max(1.0,(3.0/(abs(Lo)+0.1))) quaternion = quaternion_from_euler(0, 0, np.arctan2(self.obstacle_matrix[x_index+i,y_index+j,1],2.5*self.obstacle_matrix[x_index+i,y_index+j,0])) marker = Marker() marker.ns = 'obstacles3' marker.id = n marker.type = Marker.ARROW marker.action = Marker.ADD marker.lifetime = rospy.Duration(0) marker.scale.x = 0.12 marker.scale.y = 0.03 marker.scale.z = 0.04 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 1.0 pose = Pose() pose.position.x = ((x_index+i)*self.resolution-self.map_origin[0])/100.0; pose.position.y = ((y_index+j)*self.resolution-self.map_origin[1])/100.0; pose.position.z = 0; pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] marker.header.frame_id = 'map' marker.header.stamp = rospy.Time.now() marker.pose=pose self.markerArray.markers.append(marker) n = n+1 self.marker_pub3.publish(self.markerArray)
def run(self): thrust = 0 print("jello") while not rospy.is_shutdown(): if self.state == Controller.TakeOff: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") if self.listener.canTransform("/mocap", "/Nano_Mark_Uni2", t): position, quaternion = self.listener.lookupTransform( "/mocap", "/Nano_Mark_Gon4", t) print(position[0], position[1], position[2]) #if position[2] > 2.0 or thrust > 54000: if thrust > 55000: self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki #self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 500 #self.power = thrust msg = self.cmd_vel_telop msg.linear.z = thrust self.pubNav.publish(msg) if self.state == Controller.Land: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") if self.listener.canTransform("/mocap", "/Nano_Mark_Uni2", t): position, quaternion = self.listener.lookupTransform( "/mocap", "/Nano_Mark_Gon4", t) if position[2] > 0.05: msg_land = self.cmd_vel_telop self.power -= 100 msg_land.linear.z = self.power self.pubNav.publish(msg_land) else: msg_land = self.cmd_vel_telop msg_land.linear.z = 0 self.pubNav.publish(msg_land) if self.state == Controller.Automatic: # transform target world coordinates into local coordinates t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") print(t) if self.listener.canTransform("/mocap", "/Nano_Mark_Uni2", t): position, quaternion = self.listener.lookupTransform( "/mocap", "/Nano_Mark_Gon4", t) #print(position[0],position[1],position[2]) euler = tf.transformations.euler_from_quaternion( quaternion) print(euler[2] * (180 / math.pi)) msg = self.cmd_vel_telop #print(self.power) #Descompostion of the x and y contributions following the Z-Rotation x_prim = self.pidX.update(0.0, self.targetX - position[0]) y_prim = self.pidY.update(0.0, self.targetY - position[1]) msg.linear.x = x_prim * math.cos( euler[2]) - y_prim * math.sin(euler[2]) msg.linear.y = x_prim * math.sin( euler[2]) + y_prim * math.cos(euler[2]) #---old stuff--- #msg.linear.x = self.pidX.update(0.0, 0.0-position[0]) #msg.linear.y = self.pidY.update(0.0,-1.0-position[1]) #msg.linear.z = self.pidZ.update(position[2],1.0) #z_prim = self.pidZ.update(position[2],self.targetZ) #print(z_prim) #if z_prim < self.power: # msg.linear.z = self.power #else: # msg.linear.z = z_prim #msg.linear.z = self.power #print(self.power) msg.linear.z = self.pidZ.update( 0.0, self.targetZ - position[2] ) #self.pidZ.update(position[2], self.targetZ) msg.angular.z = self.pidYaw.update( 0.0, self.des_angle * (math.pi / 180) + euler[2]) #*(180/math.pi)) #msg.angular.z = self.pidYaw.update(0.0,self.des_angle - euler[2])#*(180/math.pi)) print(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) #print(euler[2]) #print(msg.angular.z) self.pubNav.publish(msg) #Publish Real and Target position self.pubtarX.publish(self.targetX) self.pubtarY.publish(self.targetY) self.pubtarZ.publish(self.targetZ) self.pubrealX.publish(position[0]) self.pubrealY.publish(position[1]) self.pubrealZ.publish(position[2]) #change square point if abs(self.targetX-position[0])<0.08 and \ abs(self.targetY-position[1])<0.08 and \ abs(self.targetZ-position[2])<0.08 and \ self.square_start == True: self.square_go() #Landing if abs(self.targetX-position[0])<0.1 and \ abs(self.targetY-position[1])<0.1 and \ abs(self.targetZ-position[2])<0.1 and \ self.land_flag == True: self.state = Controller.Land self.power = msg.linear.z #Publish Path #point = Marker() #line = Marker() #point.header.frame_id = line.header.frame_id = 'mocap' #POINTS #point.action = point.ADD #point.pose.orientation.w = 1.0 #point.id = 0 #point.type = point.POINTS #point.scale.x = 0.01 #point.scale.y = 0.01 #point.color.g = 1.0 #point.color.a = 1.0 #LINE #line.action = line.ADD #line.pose.orientation.w = 1.0 #line.id = 1 #line.type = line.LINE_STRIP #line.scale.x = 0.01 #line.color.g = 1.0 #line.color.a = 1.0 #p = Point() #p.x = position[0] #p.y = position[1] #p.z = position[2] #point.points.append(p) # line.points.append(p) #self.path.markers.append(p) #id = 0 #for m in self.path.markers: # m.id = id # id += 1 #self.pubPath.publish(self.path) #self.pubPath.publish(point) #self.pubPath.publish(line) point = Marker() point.header.frame_id = 'mocap' point.type = point.SPHERE #points.header.stamp = rospy.Time.now() point.ns = 'cf_Uni_path' point.action = point.ADD #points.id = 0; point.scale.x = 0.005 point.scale.y = 0.005 point.scale.z = 0.005 point.color.a = 1.0 point.color.r = 1.0 point.color.g = 1.0 point.color.b = 0.0 point.pose.orientation.w = 1.0 point.pose.position.x = position[0] point.pose.position.y = position[1] point.pose.position.z = position[2] self.path.markers.append(point) id = 0 for m in self.path.markers: m.id = id id += 1 self.pubPath.publish(self.path) #point = Point() #point.x = position[0] #point.y = position[1] #point.z = position[2] #points.points.append(point) #self.p.append(pos2path) #self.path.header.stamp = rospy.Time.now() #self.path.header.frame_id = 'mocap' #self.path.poses = self.p #self.pubPath.publish(points) rospy.sleep(0.01)
def scan_callback(self, msg): atic = timer() # edge case: first callback if self.msg_prev is None: self.msg_prev = msg return if self.odom is None: print("odom not received yet") return if self.tf_rob_in_fix is None: print("tf_rob_in_fix not found yet") return if self.tf_goal_in_fix is None: self.tf_goal_in_fix = self.tf_rob_in_fix print("goal set") # TODO check that odom and tf are not old # measure rotation TODO s = np.array(msg.ranges) # prediction dt = (msg.header.stamp - self.msg_prev.header.stamp).to_sec() s_prev = np.array(self.msg_prev.ranges) ds = (s - s_prev) max_ds = self.kMaxObstacleVel_ms * dt ds_capped = ds ds_capped[np.abs(ds) > max_ds] = 0 s_next = np.maximum(0, s + ds_capped).astype(np.float32) # cluster EUCLIDEAN_CLUSTERING_THRESH_M = 0.05 angles = np.linspace(0, 2 * np.pi, s_next.shape[0] + 1, dtype=np.float32)[:-1] clusters, x, y = clustering.euclidean_clustering( s_next, angles, EUCLIDEAN_CLUSTERING_THRESH_M) cluster_sizes = clustering.cluster_sizes(len(s_next), clusters) s_next[cluster_sizes <= 3] = 25 # dwa # Get state # goal in robot frame goal_in_robot_frame = apply_tf_to_pose( Pose2D(self.tf_goal_in_fix), inverse_pose2d(Pose2D(self.tf_rob_in_fix))) gx = goal_in_robot_frame[0] gy = goal_in_robot_frame[1] # robot speed in robot frame u = self.odom.twist.twist.linear.x v = self.odom.twist.twist.linear.y w = self.odom.twist.twist.angular.z DWA_DT = 0.5 COMFORT_RADIUS_M = self.kRobotComfortRadius_m MAX_XY_VEL = 0.5 tic = timer() best_u, best_v, best_score = dynamic_window.linear_dwa( s_next, angles, u, v, gx, gy, DWA_DT, DV=0.05, UMIN=0 if self.args.forward_only else -MAX_XY_VEL, UMAX=MAX_XY_VEL, VMIN=-MAX_XY_VEL, VMAX=MAX_XY_VEL, AMAX=10., COMFORT_RADIUS_M=COMFORT_RADIUS_M, ) toc = timer() # print(best_u * DWA_DT, best_v * DWA_DT, best_score) # print("DWA: {:.2f} Hz".format(1/(toc-tic))) # Slow turn towards goal # TODO best_w = 0 WMAX = 0.5 angle_to_goal = np.arctan2(gy, gx) # [-pi, pi] if np.sqrt(gx * gx + gy * gy) > 0.5: # turn only if goal is far away if np.abs(angle_to_goal) > (np.pi / 4 / 10): # deadzone best_w = np.clip(angle_to_goal, -WMAX, WMAX) # linear ramp if not self.STOP: # publish cmd_vel cmd_vel_msg = Twist() SIDEWAYS_FACTOR = 0.3 if self.args.forward_only else 1. cmd_vel_msg.linear.x = np.clip(best_u * 0.5, -0.3, 0.3) cmd_vel_msg.linear.y = np.clip(best_v * 0.5 * SIDEWAYS_FACTOR, -0.3, 0.3) cmd_vel_msg.angular.z = best_w self.cmd_vel_pub.publish(cmd_vel_msg) # publish cmd_vel vis pub = rospy.Publisher("/dwa_cmd_vel", Marker, queue_size=1) mk = Marker() mk.header.frame_id = self.kRobotFrame mk.ns = "arrows" mk.id = 0 mk.type = 0 mk.action = 0 mk.scale.x = 0.02 mk.scale.y = 0.02 mk.color.b = 1 mk.color.a = 1 mk.frame_locked = True pt = Point() pt.x = 0 pt.y = 0 pt.z = 0.03 mk.points.append(pt) pt = Point() pt.x = 0 + best_u * DWA_DT pt.y = 0 + best_v * DWA_DT pt.z = 0.03 mk.points.append(pt) pub.publish(mk) pub = rospy.Publisher("/dwa_goal", Marker, queue_size=1) mk = Marker() mk.header.frame_id = self.kRobotFrame mk.ns = "arrows" mk.id = 0 mk.type = 0 mk.action = 0 mk.scale.x = 0.02 mk.scale.y = 0.02 mk.color.g = 1 mk.color.a = 1 mk.frame_locked = True pt = Point() pt.x = 0 pt.y = 0 pt.z = 0.03 mk.points.append(pt) pt = Point() pt.x = 0 + gx pt.y = 0 + gy pt.z = 0.03 mk.points.append(pt) pub.publish(mk) pub = rospy.Publisher("/dwa_radius", Marker, queue_size=1) mk = Marker() mk.header.frame_id = self.kRobotFrame mk.ns = "radius" mk.id = 0 mk.type = 3 mk.action = 0 mk.pose.position.z = -0.1 mk.scale.x = COMFORT_RADIUS_M * 2 mk.scale.y = COMFORT_RADIUS_M * 2 mk.scale.z = 0.01 mk.color.b = 1 mk.color.g = 1 mk.color.r = 1 mk.color.a = 1 mk.frame_locked = True pub.publish(mk) # publish scan prediction msg_next = deepcopy(msg) msg_next.ranges = s_next # for pretty colors cluster_ids = clustering.cluster_ids(len(x), clusters) random_map = np.arange(len(cluster_ids)) np.random.shuffle(random_map) cluster_ids = random_map[cluster_ids] msg_next.intensities = cluster_ids self.pubs[0].publish(msg_next) # publish past msg_prev = deepcopy(msg) msg_prev.ranges = self.msg_prev.ranges self.pubs[1].publish(msg_prev) # ... # finally, set up for next callback self.msg_prev = msg atoc = timer() if self.args.hz: print("DWA callback: {:.2f} Hz".format(1 / (atoc - atic)))
def publish_FOV(self, event): try: self.dist_FOV = 4.0 self.angle = 45 self.delta_point_FOV = 22.5 A_local = np.array([0,0]) B_local = np.array([self.dist_FOV * np.cos(self.angle * np.pi / 180) , self.dist_FOV * np.sin(self.angle * np.pi / 180) ]) C_local = np.array([self.dist_FOV * np.cos((self.angle-self.delta_point_FOV) * np.pi / 180) , self.dist_FOV * np.sin((self.angle-self.delta_point_FOV) * np.pi / 180) ]) D_local = np.array([self.dist_FOV * np.cos((self.angle-2*self.delta_point_FOV) * np.pi / 180) , self.dist_FOV * np.sin((self.angle-2*self.delta_point_FOV) * np.pi / 180) ]) E_local = np.array([self.dist_FOV * np.cos((self.angle-3*self.delta_point_FOV) * np.pi / 180) , self.dist_FOV * np.sin((self.angle-3*self.delta_point_FOV) * np.pi / 180) ]) F_local = np.array([self.dist_FOV * np.cos((self.angle-4*self.delta_point_FOV) * np.pi / 180) , self.dist_FOV * np.sin((self.angle-4*self.delta_point_FOV) * np.pi / 180) ]) A_map = A_local B_map = B_local C_map = C_local D_map = D_local E_map = E_local F_map = F_local # A_map = A_local + self.T A_map = np.matmul(A_map , self.R_minus) A_map = A_map + self.T # B_map = B_local + self.T B_map = np.matmul(B_map , self.R_minus) B_map = B_map + self.T # C_map = C_local + self.T C_map = np.matmul(C_map , self.R_minus) C_map = C_map + self.T D_map = np.matmul(D_map , self.R_minus) D_map = D_map + self.T E_map = np.matmul(E_map , self.R_minus) E_map = E_map + self.T F_map = np.matmul(F_map , self.R_minus) F_map = F_map + self.T A_map = A_map.flatten() B_map = B_map.flatten() C_map = C_map.flatten() D_map = D_map.flatten() E_map = E_map.flatten() F_map = F_map.flatten() marker_publisher = rospy.Publisher('/FOV_marker', Marker, queue_size=10) marker = Marker() marker.id = 1500 marker.ns = "FOV" marker.header.frame_id = "/map" marker.type = marker.LINE_LIST marker.action = marker.ADD marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 0.5 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 pA = Point() ; pA.x = A_map[0] ; pA.y = A_map[1] ; pA.z = 0 pB = Point() ; pB.x = B_map[0] ; pB.y = B_map[1] ; pB.z = 0 pC = Point() ; pC.x = C_map[0] ; pC.y = C_map[1] ; pC.z = 0 pD = Point() ; pD.x = D_map[0] ; pD.y = D_map[1] ; pD.z = 0 pE = Point() ; pE.x = E_map[0] ; pE.y = E_map[1] ; pE.z = 0 pF = Point() ; pF.x = F_map[0] ; pF.y = F_map[1] ; pF.z = 0 p_list = [pA, pB, pB, pC, pC, pD, pD, pE, pE, pF, pF, pA] marker.points = p_list marker_publisher.publish(marker) rospy.Rate(100).sleep() except: return
def tf_callback(self, msg): for transform in msg.transforms: #Check if in all transform head is published if transform.child_frame_id == 'head': #If head tf was publish update the marker marker = Marker() marker.id = 0 marker.ns = 'head' marker.header.frame_id = 'head' marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://blindbuy_oakd/meshes/head.dae" marker.action = Marker.ADD marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 #https://wiki.ogre3d.org/tiki-index.php?page=Quaternion%20and%20Rotation%20Primer#Some_useful_normalized_quaternions 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.scale.x = 0.07 marker.scale.y = 0.07 marker.scale.z = 0.07 #Skin color # marker.color.r=0.98824 # marker.color.g=0.81569 # marker.color.b=0.70588 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 self.publisher_.publish(marker) self.update_head = False if transform.child_frame_id == 'palm': #If head tf was publish update the marker marker = Marker() marker.id = 0 marker.ns = 'palm' marker.header.frame_id = 'palm' marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://blindbuy_oakd/meshes/hand.dae" marker.action = Marker.ADD marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 #https://wiki.ogre3d.org/tiki-index.php?page=Quaternion%20and%20Rotation%20Primer#Some_useful_normalized_quaternions 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.scale.x = 0.01 marker.scale.y = 0.01 marker.scale.z = 0.01 #Skin color # marker.color.r=0.98824 # marker.color.g=0.81569 # marker.color.b=0.70588 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 self.publisher_.publish(marker) self.update_head = False
def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids): marker_array = MarkerArray() for track_id, corners_3d_velo in enumerate(corners_3d_velos): 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 b, g, r = DETECTION_COLOR_MAP[types[track_id]] 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.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) text_marker = Marker() text_marker.header.frame_id = FRAME_ID text_marker.header.stamp = rospy.Time.now() text_marker.id = track_id + 1000 #avoid rpeat same id 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_ids[track_id]) text_marker.scale.x = 1 text_marker.scale.y = 1 text_marker.scale.z = 1 b, g, r = DETECTION_COLOR_MAP[types[track_id]] 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) marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = -1 marker.action = Marker.ADD marker.lifetime = rospy.Duration(LIFETIME) marker.type = Marker.LINE_LIST marker.color.r = 0 marker.color.g = 1 marker.color.b = 1 marker.color.a = 1.0 marker.scale.x = 0.1 marker.points = [] for l in LINES: print(l) p1 = EGOCAR[l[0]] print(p1) marker.points.append(Point(p1[0], p1[1], p1[2])) p2 = EGOCAR[l[1]] print(p2) marker.points.append(Point(p2[0], p2[1], p2[2])) marker_array.markers.append(marker) text_marker = Marker() text_marker.header.frame_id = FRAME_ID text_marker.header.stamp = rospy.Time.now() text_marker.id = -2 text_marker.action = Marker.ADD text_marker.lifetime = rospy.Duration(LIFETIME) text_marker.type = Marker.TEXT_VIEW_FACING p4 = EGOCAR[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 = 'EGOCAR' text_marker.scale.x = 1 text_marker.scale.y = 1 text_marker.scale.z = 1 b, g, r = DETECTION_COLOR_MAP['Car'] 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) box3d_pub.publish(marker_array)
def _create_visualization_marker(self, obj, obj_type): # Text marker to display object name text_marker = Marker() text_marker.header.frame_id = "map" text_marker.header.stamp = rospy.Time() # Time zero, always show text_marker.ns = obj text_marker.id = 0 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.text = obj text_marker.pose.orientation.x = 0.0 text_marker.pose.orientation.y = 0.0 text_marker.pose.orientation.z = 0.0 text_marker.pose.orientation.w = 1.0 text_marker.color.a = 1.0 text_marker.color.r = 1.0 text_marker.color.g = 1.0 text_marker.color.b = 1.0 text_marker.scale.x = 0.2 text_marker.scale.y = 0.2 text_marker.scale.z = 0.1 text_marker.lifetime = rospy.Time(3) #Set position, depends on type if obj_type in ["drone", "turtlebot"]: text_marker.header.frame_id = "/%s/base_link" % obj if obj_type == "drone" else "/base_link" text_marker.frame_locked = True text_marker.pose.position.x = 0.0 text_marker.pose.position.y = 0.0 text_marker.pose.position.z = 0.5*text_marker.scale.z elif obj_type == "person": wp_pos = self.waypoint_positions[self.obj2loc[obj]] text_marker.pose.position.x = wp_pos['x'] text_marker.pose.position.y = wp_pos['y'] text_marker.pose.position.z = 0.5*text_marker.scale.z elif obj_type == "waypoint": wp_pos = self.waypoint_positions[obj] text_marker.pose.position.x = wp_pos['x'] text_marker.pose.position.y = wp_pos['y'] text_marker.pose.position.z = 0.5*text_marker.scale.z text_marker.color.a = 0.2 #Early exit for types that only needs text if obj_type == "turtlebot": self.vis_markers[obj] = [text_marker] return # Colored marker marker = deepcopy(text_marker) marker.id = 1 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.1 marker.color.a = 0.8 if obj_type == "drone": marker.type = Marker.SPHERE marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 elif obj_type == "box": marker.type = Marker.CUBE marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 elif obj_type == "waypoint": marker.type = Marker.CYLINDER marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.01 marker.color.a = 0.2 elif obj_type == "person": marker.type = Marker.CYLINDER #Set color in update step self.vis_markers[obj] = [text_marker,marker]
wpoints = [] while waypoints is None or start_flag == 0: if waypoints is None: print "no waypoints" lock_aux_pointx = 0 waypoints = new_waypoints for i in range(waypoints.shape[0]): p = Point() p.x = waypoints[i][0] p.y = waypoints[i][1] p.z = 0 wpoints.append(p) marker = Marker() marker.header.frame_id = "/odom" marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.points = wpoints t = rospy.Duration() marker.lifetime = t marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.4 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 pub_marker.publish(marker)
def callback(bbox_data, laserScan_data): nameArray = [] xMinArray = [] yMinArray = [] xMaxArray = [] yMaxArray = [] detected_Objects = [[]] global id_stock global markerArray global subArray i = 0 #Bounding box verisini kontrol el if (bbox_data is not None): for i in range(len(bbox_data.bounding_boxes)): try: tmp = bbox_data.bounding_boxes[i].Class nameArray.append(bbox_data.bounding_boxes[i].Class) xMinArray.append(bbox_data.bounding_boxes[i].xmin) yMinArray.append(bbox_data.bounding_boxes[i].ymin) xMaxArray.append(bbox_data.bounding_boxes[i].xmax) yMaxArray.append(bbox_data.bounding_boxes[i].ymax) i += 1 except: print("hata") objCount = i counter = 0 #Laser scan verisinden obje uzakligini bul for counter in range(objCount): rangeMin = 380 + ((640 - xMaxArray[counter]) / 2) rangeMax = 700 - (xMinArray[counter] / 2) sum = 0 k = 0 ''' for j in range (int(round(rangeMax-rangeMin))): sum += laserScan_data.ranges[rangeMin+j] mean = sum/(rangeMax-rangeMin) ''' midPoint = int(round((rangeMin + rangeMax) / 2)) mean = laserScan_data.ranges[midPoint] detected_Objects.append([nameArray[counter], mean, midPoint]) i = 1 print("Bulunan obje sayisi: " + str(len(detected_Objects))) print(detected_Objects) print(" | Class " + " | Dist | " + "pos |") while (i < len(detected_Objects)): #Bulunan objeleri yazdir text = " " text += str(i) + ".| " text += str(detected_Objects[i][0]) text += " | " text += "{:.2f}".format(detected_Objects[i][1]) text += "m | " text += str(detected_Objects[i][2]) text += " | " print(text) #Marker olustur marker = Marker() marker.header.frame_id = "/robot/odom" marker.header.stamp = rospy.get_rostime() marker.id = id_stock[0] id_stock.pop(0) marker.ns = str(detected_Objects[i][0]) marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 0.1 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 #Robotun global koordinatlarini bul (trans, rot) = listener.lookupTransform('/robot/odom', '/robot/base_link', rospy.Time(0)) tX = trans[0] tY = trans[1] tZ = trans[2] rX = rot[0] rY = rot[1] rZ = rot[2] rW = rot[3] quaternion = (rX, rY, rZ, rW) euler = tf.transformations.euler_from_quaternion(quaternion) print("Trans: " + "{:.2f}".format(tX) + " {:.2f}".format(tY) + " {:.2f}".format(tZ)) print("Roll: " + "{:.2f}".format(degrees(euler[0])) + " Pitch: " + "{:.2f}".format(degrees(euler[1])) + " Yaw: " + "{:.2f}".format(degrees(euler[2]))) #Kameranin merkezine gore obje acisini bul if (detected_Objects[i][2] > 539): degree = (float(detected_Objects[i][2]) - 539) * laserScan_data.angle_increment else: degree = (539 - float( detected_Objects[i][2])) * laserScan_data.angle_increment degree = degrees(degree) #Global aciyi bul if (detected_Objects[i][2] < 540): degree = degree * (-1) yaw = round(degrees(euler[2]), 2) globalYaw = yaw + degree print("ObjeDegree: " + str(degree) + " Yaw: " + str(yaw) + " Global Yaw: " + str(globalYaw)) xAdd = detected_Objects[i][1] * cos(radians(globalYaw)) yAdd = detected_Objects[i][1] * sin(radians(globalYaw)) marker.pose.position.x = trans[0] marker.pose.position.y = trans[1] marker.pose.position.z = 0.2 marker.pose.position.x += xAdd marker.pose.position.y += yAdd j = 0 different = True sameID = -1 pointIncRate = 0.1 pointDecRate = 0.03 #Bulunan obje daha once isaretlenmis mi diye kontrol et while (j < (len(subArray.markers))): if (subArray.markers[j].ns == str(detected_Objects[i][0])): x1 = subArray.markers[j].pose.position.x x2 = marker.pose.position.x y1 = subArray.markers[j].pose.position.y y2 = marker.pose.position.y distanceBetween = sqrt(pow((x1 - x2), 2) + pow((y1 - y2), 2)) print("Distance between:" + str(distanceBetween)) if (distanceBetween != float("inf")): if (subArray.markers[j].color.a < 1.0): if (distanceBetween < 0.1): print("Color.a " + str(subArray.markers[j].color.a)) different = False sameID = j else: if (distanceBetween < 1): print("Color.a2 " + str(subArray.markers[j].color.a)) different = False sameID = j j += 1 if (different): #marker.id = id_stock[len(id_stock)-1] #id_stock.pop(len(id_stock)-1) subArray.markers.append(marker) else: id_stock.append(marker.id) if (subArray.markers[sameID].color.a < 1.0): subArray.markers[sameID].color.a += pointIncRate if (subArray.markers[sameID].color.a >= 1.0): #subArray.markers[sameID].text= str(detected_Objects[i][0]) tmpMarker = Marker() tmpMarker.header.frame_id = "/robot/odom" tmpMarker.header.stamp = rospy.get_rostime() tmpMarker.id = subArray.markers[sameID].id tmpMarker.ns = "textmarker" tmpMarker.type = marker.TEXT_VIEW_FACING tmpMarker.action = marker.ADD tmpMarker.scale.x = 0.2 tmpMarker.scale.y = 0.2 tmpMarker.scale.z = 0.2 tmpMarker.color.a = 1.0 tmpMarker.color.r = 0.0 tmpMarker.color.g = 0.0 tmpMarker.color.b = 1.0 #print("MarkerPoseZ: " + str(subArray.markers[sameID].pose.position.z) + " TextPoseZ: " + str(tmpMarker.pose.position.z)) tmpMarker.pose.orientation.w = 1.0 tmpMarker.pose.position.x = subArray.markers[ sameID].pose.position.x tmpMarker.pose.position.y = subArray.markers[ sameID].pose.position.y tmpMarker.pose.position.z = subArray.markers[ sameID].pose.position.z tmpMarker.pose.position.z += 0.2 if (str(subArray.markers[sameID].ns) == "QR"): tmpMarker.text = "QR_" print("i: " + str(i)) tmp_xMin = xMinArray[i - 1] tmp_yMin = yMinArray[i - 1] tmp_xMax = xMaxArray[i - 1] tmp_yMax = yMaxArray[i - 1] img = rospy.wait_for_message( "/robot/camera/rgb/image_raw", Image) cv2_img = bridge.imgmsg_to_cv2(img, "bgr8") #cv2.imshow("asf",cv2_img) #time.sleep(1) #time.sleep(10) #print("Xmin: " +str(tmp_xMin)+ " yMin: " +str(tmp_yMin)+ " tmp_xMax: " +str(tmp_xMax)+ " tmp_yMax: "+str(tmp_yMax)) if (tmp_xMin > 20): tmp_xMin -= 20 if (tmp_yMin > 20): tmp_yMin -= 20 if (tmp_xMax < 620): tmp_xMax += 20 if (tmp_yMax < 460): tmp_yMax += 20 #print("Xmin: " +str(tmp_xMin)+ " yMin: " +str(tmp_yMin)+ " tmp_xMax: " +str(tmp_xMax)+ " tmp_yMax: "+str(tmp_yMax)) #print("tmp_yMin: "+ str(tmp_yMin) + " tmp_yMax-tmp_yMin: "+ str(tmp_yMax-tmp_yMin) + "tmp_xMin: "+ str(tmp_xMin) + " tmp_xMax-tmp_xMin" + str(tmp_xMax-tmp_xMin)) cv2_img = cv2_img[tmp_yMin:tmp_yMax, tmp_xMin:tmp_xMax] #qr = qrtools.QR() #qr.decode(cv2_img) qrData = pyzbar.decode(cv2_img) #print("QR data:" + str(qrData[0][0])) #time.sleep(10) #print(cv2_img) #cv2.imshow("QRimg",cv2_img) #cv2.waitKey(1) #img = rospy.Subscriber("/robot/camera/rgb/image_raw", Image, img_callback) #print("QR hata") #cv2.imshow(img) #cv2.waitKey(1) #time.sleep(10) if (qrData): subArray.markers[sameID].color.a = 1.0 subArray.markers[sameID].color.r = 0.0 subArray.markers[sameID].color.g = 1.0 tmpMarker.text += str(qrData[0][0]) subArray.markers.append(tmpMarker) #markerArray.markers.append(tmpMarker) print("qr okundu") else: subArray.markers[sameID].color.a = 0.2 print("qr okunamadi") #time.sleep(10) #time.sleep(10) else: subArray.markers[sameID].color.a = 1.0 subArray.markers[sameID].color.r = 0.0 subArray.markers[sameID].color.g = 1.0 tmpMarker.text = subArray.markers[sameID].ns subArray.markers.append(tmpMarker) #markerArray.markers.append(tmpMarker) #print("MarkerPoseZ: " + str(subArray.markers[sameID].pose.position.z) + " TextPoseZ: " + str(tmpMarker.pose.position.z)) #time.sleep(10) #markerArray.markers.append(subArray.markers[sameID]) print(subArray.markers[sameID]) j = 0 while (j < (len(subArray.markers))): if (subArray.markers[j].ns == str( detected_Objects[i][0])): x1 = subArray.markers[j].pose.position.x x2 = marker.pose.position.x y1 = subArray.markers[j].pose.position.y y2 = marker.pose.position.y distanceBetween = sqrt( pow((x1 - x2), 2) + pow((y1 - y2), 2)) if (distanceBetween != float("inf")): if ((subArray.markers[j].color.a < 1.0) and (distanceBetween < 1)): subArray.markers[j].color.a = 0.0 j += 1 i += 1 print("\n") j = 0 deleteArray = [] marker_pub.publish(subArray) while j < (len(subArray.markers)): if ((subArray.markers[j].ns == "QR") and (subArray.markers[j].color.a >= 1.0)): print("lensub" + str(len(subArray.markers)) + " id: " + str(subArray.markers[j].id) + " NS: " + str(subArray.markers[j].ns) + " text:" + str(subArray.markers[j].text) + " color.a: " + str(subArray.markers[j].color.a)) if (subArray.markers[j].color.a < 1.0): print("dec") print("lensub" + str(len(subArray.markers)) + " id: " + str(subArray.markers[j].id) + " NS: " + str(subArray.markers[j].ns) + " text:" + str(subArray.markers[j].text) + " color.a: " + str(subArray.markers[j].color.a)) if (subArray.markers[j] > 0.0): subArray.markers[j].color.a -= pointDecRate if (subArray.markers[j].color.a <= 0.0): deleteArray.append(subArray.markers[j]) j += 1 if (deleteArray is not None): deleteArray.sort(reverse=True) for j in range(len(deleteArray)): id_stock.append(deleteArray[j].id) subArray.markers.remove(deleteArray[j]) #marker_pub.publish(subArray) #marker_pub.publish(markerArray) print("Marker sayisi: " + str(len(markerArray.markers))) print("id_stock: " + str(len(id_stock)))
def update(self, x_pos, y_pos, yaw): global waypoints, waypoint_index, station_keep_flag, stable, lock, lock_aux_pointx, lock_aux_pointy self.pos_SetPointx = waypoints[waypoint_index][0] self.pos_SetPointy = waypoints[waypoint_index][1] # get waypoint index last_waypoint_index = waypoint_index if np.sqrt((waypoints[waypoint_index][0] - x_pos) * (waypoints[waypoint_index][0] - x_pos) + (waypoints[waypoint_index][1] - y_pos) * (waypoints[waypoint_index][1] - y_pos)) < 5: if waypoint_index == waypoints.shape[0] - 1: waypoint_index = waypoint_index else: waypoint_index = waypoint_index + 1 if waypoint_index == waypoints.shape[0] - 1 and np.sqrt( (waypoints[waypoint_index][0] - x_pos) * (waypoints[waypoint_index][0] - x_pos) + (waypoints[waypoint_index][1] - y_pos) * (waypoints[waypoint_index][1] - y_pos)) < 5: station_keep_flag = 1 else: station_keep_flag = 0 if time.time() - self.wait_start > 10: self.wait_flag = 0 #print "delta t", time.time() - self.wait_start pos_error = np.sqrt((self.pos_SetPointx - x_pos) * (self.pos_SetPointx - x_pos) + (self.pos_SetPointy - y_pos) * (self.pos_SetPointy - y_pos)) waypoint_error = np.sqrt((waypoints[waypoint_index][0] - x_pos) * (waypoints[waypoint_index][0] - x_pos) + (waypoints[waypoint_index][1] - y_pos) * (waypoints[waypoint_index][1] - y_pos)) if np.abs( waypoint_error) < 5 and station_keep_flag == 1 and stable == 1: print "turn to desired yaw" # lock yaw if waypoints[waypoint_index][2] != -10: error = waypoints[waypoint_index][2] - yaw else: error = np.arctan2((waypoints[waypoint_index][1] - waypoints[waypoint_index - 1][1]), (waypoints[waypoint_index][0] - waypoints[waypoint_index - 1][0])) - yaw else: error = np.arctan2((self.SetPointy - y_pos), (self.SetPointx - x_pos)) - yaw if waypoint_index >= 1: last_waypointx = waypoints[waypoint_index - 1][0] last_waypointy = waypoints[waypoint_index - 1][1] else: if lock_aux_pointx == 0: lock_aux_pointx = x_pos lock_aux_pointy = y_pos last_waypointx = lock_aux_pointx last_waypointy = lock_aux_pointy if np.abs(waypoints[waypoint_index][0] - last_waypointx) < 1: dist_to_line = np.abs(x_pos - last_waypointx) tmp_x = last_waypointx tmp_y = y_pos dir_vectorx = 0 dir_vectory = 1 else: line_slope = (waypoints[waypoint_index][1] - last_waypointy) / ( waypoints[waypoint_index][0] - last_waypointx) line_intercept = waypoints[waypoint_index][ 1] - line_slope * waypoints[waypoint_index][0] dist_to_line = np.abs(line_slope * x_pos + line_intercept - y_pos) / np.sqrt(line_slope * line_slope + 1) tmp_x = x_pos + (line_slope / np.sqrt(line_slope * line_slope + 1) ) * dist_to_line tmp_y = y_pos - dist_to_line #print line_slope, line_intercept, dist_to_line if np.abs(line_slope * tmp_x + line_intercept - tmp_y) / np.sqrt( line_slope * line_slope + 1) > dist_to_line: tmp_x = x_pos - (line_slope / np.sqrt(line_slope * line_slope + 1)) * dist_to_line tmp_y = y_pos + dist_to_line dir_vectorx = 1 / np.sqrt(line_slope * line_slope + 1) dir_vectory = line_slope / np.sqrt(line_slope * line_slope + 1) #print dir_vectorx, dir_vectory #print dist_to_line proc_dist = np.sqrt(5 * 5 - dist_to_line * dist_to_line) aux_pointx = tmp_x + dir_vectorx * proc_dist aux_point3x = tmp_x + 3 * dir_vectorx * proc_dist aux_pointy = tmp_y + dir_vectory * proc_dist aux_point3y = tmp_y + 3 * dir_vectory * proc_dist if np.abs((waypoints[waypoint_index][0] - tmp_x) * (waypoints[waypoint_index][0] - tmp_x) + (waypoints[waypoint_index][1] - tmp_y) * (waypoints[waypoint_index][1] - tmp_y)) < np.abs( (waypoints[waypoint_index][0] - aux_pointx) * (waypoints[waypoint_index][0] - aux_pointx) + (waypoints[waypoint_index][1] - aux_pointy) * (waypoints[waypoint_index][1] - aux_pointy)): aux_pointx = tmp_x - dir_vectorx * proc_dist aux_point3x = tmp_x - 3 * dir_vectorx * proc_dist aux_pointy = tmp_y - dir_vectory * proc_dist aux_point3y = tmp_y - 3 * dir_vectory * proc_dist #print np.sqrt((tmp_x - x_pos)*(tmp_x - x_pos)+(tmp_y - y_pos)*(tmp_y - y_pos)) if np.sqrt((tmp_x - x_pos) * (tmp_x - x_pos) + (tmp_y - y_pos) * (tmp_y - y_pos)) < 5: if station_keep_flag == 1 and pos_error < 5: if lock == 0: self.SetPointx = aux_point3x self.SetPointy = aux_point3y aux_pointx = aux_point3x aux_pointy = aux_point3y lock = 1 aux_pointx = self.SetPointx aux_pointy = self.SetPointy else: self.SetPointx = aux_pointx self.SetPointy = aux_pointy lock = 0 #print "aux", aux_pointx, aux_pointy wpoints = [] for i in range(1): p = Point() p.x = aux_pointx p.y = aux_pointy p.z = 0 wpoints.append(p) marker = Marker() marker.header.frame_id = "/odom" marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.points = wpoints t = rospy.Duration() marker.lifetime = t marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.4 marker.color.a = 1.0 marker.color.r = 0 marker.color.g = 0 marker.color.b = 1.0 self.pub_aux.publish(marker) else: lock = 0 if station_keep_flag == 1: self.SetPointx = waypoints[waypoint_index][0] self.SetPointy = waypoints[waypoint_index][1] tmp_x = waypoints[waypoint_index][0] tmp_y = waypoints[waypoint_index][1] else: if (last_waypointx - x_pos) * ( waypoints[waypoint_index][0] - last_waypointx) + (last_waypointy - y_pos) * ( waypoints[waypoint_index][1] - last_waypointy) > 0: self.SetPointx = tmp_x self.SetPointy = tmp_y else: self.SetPointx = waypoints[waypoint_index][0] self.SetPointy = waypoints[waypoint_index][1] print "tmp", tmp_x, tmp_y wpoints = [] for i in range(1): p = Point() p.x = tmp_x p.y = tmp_y p.z = 0 wpoints.append(p) marker = Marker() marker.header.frame_id = "/odom" marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.points = wpoints t = rospy.Duration() marker.lifetime = t marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.4 marker.color.a = 1.0 marker.color.r = 0 marker.color.g = 1.0 marker.color.b = 0 self.pub_aux.publish(marker) wpoints = [] for i in range(1): p = Point() p.x = waypoints[waypoint_index][0] p.y = waypoints[waypoint_index][1] p.z = 0 wpoints.append(p) marker2 = Marker() marker2.header.frame_id = "/odom" marker2.type = marker.POINTS marker2.action = marker.ADD marker2.pose.orientation.w = 1 marker2.points = wpoints t = rospy.Duration() marker2.lifetime = t marker2.scale.x = 0.4 marker2.scale.y = 0.4 marker2.scale.z = 0.4 marker2.color.a = 1.0 marker2.color.r = 0.5 marker2.color.g = 0 marker2.color.b = 0.5 self.pub_tgwp.publish(marker2) wpoints = [] for i in range(1): p = Point() p.x = last_waypointx p.y = last_waypointy p.z = 0 wpoints.append(p) marker3 = Marker() marker3.header.frame_id = "/odom" marker3.type = marker.POINTS marker3.action = marker.ADD marker3.pose.orientation.w = 1 marker3.points = wpoints t = rospy.Duration() marker3.lifetime = t marker3.scale.x = 0.4 marker3.scale.y = 0.4 marker3.scale.z = 0.4 marker3.color.a = 1.0 marker3.color.r = 0 marker3.color.g = 0.5 marker3.color.b = 0.5 self.pub_lwp.publish(marker3) ''' #print np.abs(waypoint_error) if np.abs(waypoint_error) < 5 and waypoints is not None: if waypoint_index < waypoints.shape[0]-1: if self.wait_flag == 0 and station_keep_flag == 0: waypoint_index = waypoint_index + 1 self.wait_flag = 1 self.wait_start = time.time() print "target waypoint", waypoint_index #print waypoint_index, waypoints.shape[0] else: station_keep_flag = 1 print "station keep in last waypoint" ''' #print "error", np.arctan((self.pos_SetPointy - y_pos)/(self.pos_SetPointx - x_pos)) - yaw if np.abs(error) > np.pi: if error > 0: error = error - 2 * np.pi else: error = error + 2 * np.pi if station_keep_flag == 1 and np.abs(waypoint_error) > 5: if np.abs(error) > np.pi / 2: if error < 0: error = error + np.pi else: error = error - np.pi #print "error", error #print "yaw", yaw #print "target", np.arctan2((self.pos_SetPointy - y_pos), (self.pos_SetPointx - x_pos)) self.current_time = time.time() delta_time = self.current_time - self.last_time delta_error = error - self.last_error if (delta_time >= self.sample_time): self.PTerm = self.Kp * error self.ITerm += error * delta_time if (self.ITerm < -self.windup_guard): self.ITerm = -self.windup_guard elif (self.ITerm > self.windup_guard): self.ITerm = self.windup_guard self.DTerm = 0.0 if delta_time > 0: self.DTerm = delta_error / delta_time self.last_time = self.current_time self.last_error = error self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm)
pic[i] = cv2.resize(pic[i], (pic_resize, pic_resize), interpolation=cv2.INTER_AREA) kp[i], des[i] = sift.detectAndCompute(pic[i], None) #kp_f[i], des_f[i] = sift.detectAndCompute(pic_f[i],None) voting.append(0) voting2.append(0) published.append(False) # BFMatcher with default params bf = cv2.BFMatcher() pub = rospy.Publisher('visualization_marker', Marker, queue_size=100) marker_arr = [] for i in range(0, pic_len): marker = Marker() marker.header.frame_id = "camera_link" marker.type = marker.TEXT_VIEW_FACING marker.action = marker.ADD marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.text = name[i] marker.id = i marker_arr.append(marker) def pub_marking(img, name_id): #Camera resectioning & make marker
from sailboat_message.msg import WTST_msg from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Point from visualization_msgs.msg import Marker ## fleet race # target_x = [0, 58.7091157621, 64.8362530808, 250.098605093, 213.669988671] # target_y = [0, 115.264402922, 91.6514720561, 112.199262859, 207.218604858], e5: 366.770497586 ## station_keeping target_x = [0, -151.061785525] target_y = [0, 366.770497586] path = Path() marker = Marker() marker.header.frame_id = 'WTST' marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.scale.x = 5.0 marker.scale.y = 5.0 marker.scale.z = 5.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0 marker.color.a = 0.5 for xx, yy in zip(target_x, target_y): p = Point() p.x = xx p.y = yy marker.points.append(p)
def plan_path(self, start_point, end_point, map_obj): """ RRT*. Tries to find a collision free path from start to goal. """ # STUFF FOR TESTING self.trajectory.clear() if self.enable_vis: marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.scale.x = 0.1 marker.scale.y = 0.1 self.vis_pub.publish(marker) exploration_bias = 1.0 - self.goal_bias final_node = None num_existing_path_points_added = 0 self.rrt_star = RRTStar(Node(start_point)) self.max_iterations = self.rrt_star.max_size while self.rrt_star.size <= self.max_iterations: p = np.random.uniform() if p < exploration_bias: x_rand = self.map.sample_free_space() else: if final_node is None: x_rand = end_point else: x_rand = self.branched_from_existing_path( final_node, depth_underestimate=num_existing_path_points_added) num_existing_path_points_added += 1 x_nearest = self.rrt_star.nearest( x_rand) # Find the nearest node to x_rand path = self.map.generate_line_path(x_nearest.value, x_rand, eta=self.eta) if path is not None: # no obstacles between x_nearest and x_rand x_new = path[-1] X_nearby_connectable = self.find_nearby_connectable( x_nearest, x_new) cost_min, node_min = self.find_best_parent( X_nearby_connectable, x_new) X_nearby_connectable.remove( node_min ) # Remove x_new's parent node from the list of nearby nodes so it is not considered for rewiring # Create the new node at x_new! node_new = self.rrt_star.add_config(node_min, x_new) if self.enable_vis: # FOR TESTING ONLY # # Code to publish marker for new node ########################################################################################### TEMP = Point() TEMP.x = x_new[0] TEMP.y = x_new[1] TEMP.z = .05 marker.points.append(TEMP) TEMP = ColorRGBA() TEMP.r = 1 TEMP.g = 0 TEMP.b = 0 TEMP.a = 1 marker.colors.append(TEMP) self.vis_pub.publish(marker) ########################################################################################### self.rewire(cost_min, node_new, X_nearby_connectable) if np.allclose(node_new.value, end_point, .05, 0) and ( final_node is None): #np.array_equal(node_new.value, end_point): final_node = node_new # reduce exploration bias so that we reinforce the existing path exploration_bias = .5 if VERBOSE: print("Path found!!!!") print(final_node.cost) if rospy.get_time() - self.start_time > self.time_thresh: if VERBOSE: print(self.rrt_star.size) break if final_node is not None: if self.enable_vis: marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.scale.x = 0.1 marker.scale.y = 0.1 marker.points = [] marker.colors = [] def recur(node): if self.enable_vis: TEMP = Point() TEMP.x = node.value[0] TEMP.y = node.value[1] TEMP.z = .05 marker.points.append(TEMP) TEMP = ColorRGBA() TEMP.r = 1 TEMP.g = 0 TEMP.b = 0 TEMP.a = 1 marker.colors.append(TEMP) self.trajectory.points.append([node.value[0], node.value[1]]) parent = node.parent if parent is not None: recur(parent) recur(final_node) self.trajectory.points.reverse() if self.enable_vis: self.vis_pub.publish(marker) if VERBOSE: print(final_node.depth) else: # TODO:give best guess- find closest node to goal if VERBOSE: print("No path found! Please try again.")
counter = 0 for irow in blue: icone = Cone() icone.posx = irow[0] icone.posy = irow[1] icone.color = 0 conesmsg.cones.append(icone) imarker = Marker() imarker.pose.position.x = irow[0] imarker.pose.position.y = irow[1] imarker.header.frame_id = 'map' imarker.id = counter imarker.scale.x = .5 imarker.scale.y = .5 imarker.scale.z = .5 imarker.type = 3 imarker.color.a = 1.0 imarker.color.b = 1.0 imarker.lifetime.secs = 10 markersmsg.markers.append(imarker) counter = counter + 1 # at this point, have all blue markers /cones ready to publish for irow in yellow: icone = Cone() icone.posx = irow[0] icone.posy = irow[1] icone.color = 1 conesmsg.cones.append(icone) imarker = Marker() imarker.pose.position.x = irow[0]
def callback(msg): x = msg.pose.pose.position.x y = msg.pose.pose.position.y qx = msg.pose.pose.orientation.x qy = msg.pose.pose.orientation.y qz = msg.pose.pose.orientation.z qw = msg.pose.pose.orientation.w pose_quaternion = np.array([qx, qy, qz, qw]) yaw = tf.transformations.euler_from_quaternion(pose_quaternion)[2] # 2. Find the path point closest to the vehicle that is >= 1 lookahead distance from vehicle's current location. global LOOKAHEAD_DISTANCE goal = find_goal_point(x, y, yaw, LOOKAHEAD_DISTANCE) goal_point = path_points[goal] previous_goal = goal # # 3. Transform the goal point to vehicle coordinates. goal_pose_msg, goal_pose = transform_point(goal_point) # 4. Calculate the curvature = 1/r = 2x/l^2 # The curvature is transformed into steering wheel angle and published to the 'drive_param' topic. abs_y = abs(goal_pose.pose.position.y) curvature = ((2.0 * abs_y) / (LOOKAHEAD_DISTANCE**2)) angle = np.arctan(LOOKAHEAD_DISTANCE * curvature) if (goal_pose.pose.position.y < 0): angle = -angle #Right Steering else: angle = angle #Left Steering angle = np.clip( angle, -0.4189, 0.4189 ) # 0.4189 radians = 24 degrees because car can only turn 24 degrees max # clipping speeds and Lookahead degree_angle = math.degrees(angle) if abs(degree_angle) < ANGLE_LEVEL_1: vel = SPEED_LEVEL_1 LOOKAHEAD_DISTANCE = 2 elif ANGLE_LEVEL_1 <= abs(degree_angle) and abs( degree_angle) < ANGLE_LEVEL_2: vel = SPEED_LEVEL_2 LOOKAHEAD_DISTANCE = 1.5 else: vel = SPEED_LEVEL_3 LOOKAHEAD_DISTANCE = 1.0 msg = drive_param() msg.velocity = vel msg.angle = angle pub.publish(msg) goal_pub.publish(goal_pose_msg) lookahead_marker = Marker() lookahead_marker.type = Marker.TEXT_VIEW_FACING lookahead_marker.header.frame_id = '/map' lookahead_marker.scale.z = 0.5 lookahead_marker.color.a = 1 lookahead_marker.color.r = 1.0 lookahead_marker.color.g = 0.0 lookahead_marker.color.b = 0.0 lookahead_marker.pose.position.x = 0.0 lookahead_marker.pose.position.y = 4.0 rospy.set_param('LOOKAHEAD_DISTANCE', LOOKAHEAD_DISTANCE) multiline_str = 'LOOKAHEAD_DISTANCE: %s' % str( LOOKAHEAD_DISTANCE) + ' \n' + 'VELOCITY: %s' % str(vel) lookahead_marker.text = multiline_str marker_pub.publish(lookahead_marker)
def initPathMarkers(): # cannot write in one equation!!! sourcePoint = Marker() goalPoint = Marker() neighbourPoint = Marker() finalPath = Marker() sourcePoint.header.frame_id = 'path_planner' goalPoint.header.frame_id = 'path_planner' neighbourPoint.header.frame_id = 'path_planner' finalPath.header.frame_id = 'path_planner' sourcePoint.header.stamp = rospy.get_rostime() goalPoint.header.stamp = rospy.get_rostime() neighbourPoint.header.stamp = rospy.get_rostime() finalPath.header.stamp = rospy.get_rostime() sourcePoint.ns = "path_planner" goalPoint.ns = "path_planner" neighbourPoint.ns = "path_planner" finalPath.ns = "path_planner" sourcePoint.action = 0 # add/modify an object goalPoint.action = 0 neighbourPoint.action = 0 finalPath.action = 0 sourcePoint.id = 0 goalPoint.id = 1 neighbourPoint.id = 2 finalPath.id = 3 # sourcePoint.text = 'sourcePoint' # goalPoint.text = 'goalPoint' # neighbourPoint.text = 'neighbourPoint' # finalPath.text = 'finalPath' sourcePoint.type = 2 # Sphere goalPoint.type = 2 neighbourPoint.type = 8 # Points finalPath.type = 4 # Line Strip sourcePoint.pose.orientation.w = 1.0 goalPoint.pose.orientation.w = 1.0 neighbourPoint.pose.orientation.w = 1.0 finalPath.pose.orientation.w = 1.0 sourcePoint.pose.position.x = 0.0 sourcePoint.pose.position.y = 0.0 sourcePoint.pose.position.z = 0.0 goalPoint.pose.position.x = 10.0 goalPoint.pose.position.y = 10.0 goalPoint.pose.position.z = 0.0 neighbourPoint.pose.position.x = 0.0 neighbourPoint.pose.position.y = 0.0 neighbourPoint.pose.position.z = 0.0 sourcePoint.scale.x = sourcePoint.scale.y = sourcePoint.scale.z = 1.0 goalPoint.scale.x = goalPoint.scale.y = goalPoint.scale.z = 1.0 neighbourPoint.scale.x = neighbourPoint.scale.y = neighbourPoint.scale.z = 0.1 finalPath.scale.x = 0.5 # scale.x controls the width of the line segments sourcePoint.color.g = 1.0 goalPoint.color.r = 1.0 neighbourPoint.color.r = 0.8 neighbourPoint.color.g = 0.4 finalPath.color.r = 0.2 finalPath.color.g = 0.2 finalPath.color.b = 1.0 sourcePoint.color.a = 1.0 goalPoint.color.a = 1.0 neighbourPoint.color.a = 0.5 finalPath.color.a = 1.0 return (sourcePoint, goalPoint, neighbourPoint, finalPath)
def compute_optimal_diffdrive_action(self,curr_state, currently_executing_action, step): x_index = self.x_index y_index = self.y_index yaw_index = self.yaw_index move_to_next= 0 curr_seg = int (np.copy(self.curr_line_segment)) moved_to_next = 0 #array of "the point"... for each sim pt = np.copy(curr_state) # N x state #arrays of line segment points... for each sim curr_start = self.desired_states[curr_seg] curr_end = self.desired_states[curr_seg+1] next_start = self.desired_states[curr_seg+1] next_end = self.desired_states[curr_seg+2] ############ closest distance from point to curr line segment #vars a = pt[x_index]- curr_start[0] b = pt[y_index]- curr_start[1] c = curr_end[0]- curr_start[0] d = curr_end[1]- curr_start[1] #project point onto line segment which_line_section = np.divide((np.multiply(a,c) + np.multiply(b,d)), (np.multiply(c,c) + np.multiply(d,d))) #point on line segment that's closest to the pt closest_pt_x = np.copy(which_line_section) closest_pt_y = np.copy(which_line_section) if(which_line_section<0): closest_pt_x=curr_start[0] closest_pt_y=curr_start[1] elif(which_line_section>1): closest_pt_x=curr_end[0] closest_pt_y=curr_end[1] else: closest_pt_x= curr_start[0] + np.multiply(which_line_section,c) closest_pt_y= curr_start[1] + np.multiply(which_line_section,d) #min dist from pt to that closest point (ie closest dist from pt to line segment) min_perp_dist = np.sqrt((pt[x_index]-closest_pt_x)*(pt[x_index]-closest_pt_x) + (pt[y_index]-closest_pt_y)*(pt[y_index]-closest_pt_y)) #"forward-ness" of the pt... for each sim curr_forward = which_line_section ############ closest distance from point to next line segment #vars a = pt[x_index]- next_start[0] b = pt[y_index]- next_start[1] c = next_end[0]- next_start[0] d = next_end[1]- next_start[1] #project point onto line segment which_line_section = np.divide((np.multiply(a,c) + np.multiply(b,d)), (np.multiply(c,c) + np.multiply(d,d))) #point on line segment that's closest to the pt closest_pt_x = np.copy(which_line_section) closest_pt_y = np.copy(which_line_section) if(which_line_section<0): closest_pt_x=next_start[0] closest_pt_y=next_start[1] elif(which_line_section>1): closest_pt_x=next_end[0] closest_pt_y=next_end[1] else: closest_pt_x= next_start[0] + np.multiply(which_line_section,c) closest_pt_y= next_start[1] + np.multiply(which_line_section,d) #min dist from pt to that closest point (ie closes dist from pt to line segment) dist = np.sqrt((pt[x_index]-closest_pt_x)*(pt[x_index]-closest_pt_x) + (pt[y_index]-closest_pt_y)*(pt[y_index]-closest_pt_y)) #pick which line segment it's closest to, and update vars accordingly if(dist<=min_perp_dist): curr_seg += 1 moved_to_next = 1 curr_forward = which_line_section min_perp_dist=dist ################## publish the desired traj markerArray2 = MarkerArray() marker_id=0 for des_pt_num in range(8): #5 for all, 8 for circle, 4 for zigzag marker = Marker() marker.id=marker_id marker.header.frame_id = "/world" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.15 marker.scale.y = 0.15 marker.scale.z = 0.15 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.pose.position.x = self.desired_states[des_pt_num,0] marker.pose.position.y = self.desired_states[des_pt_num,1] marker.pose.position.z = 0 markerArray2.markers.append(marker) marker_id+=1 self.publish_markers_desired.publish(markerArray2) ################## advance which line segment we are on if(moved_to_next==1): self.curr_line_segment+=1 print("**************************** MOVED ONTO NEXT LINE SEG") ################## given the current state, the perpendicular distance, and desired heading, calculate the velocities to output for each motor #what side of the line segment is the point on angle_to_point = np.arctan2(pt[y_index]-curr_start[1], pt[x_index]-curr_start[0]) angle_of_line = np.arctan2(curr_end[1]-curr_start[1], curr_end[0]-curr_start[0]) if(moving_distance(angle_of_line, angle_to_point)>0): on_right = True else: on_right = False #angle for moving robot toward desired heading heading_turn_amount = moving_distance(angle_of_line, pt[yaw_index]) if(on_right): heading_turn_amount += self.horiz*min_perp_dist else: heading_turn_amount -= self.horiz*min_perp_dist #angle --> velocities # + heading_turn_amount means ccw (toward left, so L slower and R faster) #left_vel = self.nominal - heading_turn_amount*self.heading #right_vel = self.nominal + heading_turn_amount*self.heading angular_vel = heading_turn_amount*self.heading #clip '''if(left_vel>self.max_motor_gain): left_vel=self.max_motor_gain if(left_vel<self.min_motor_gain): left_vel=self.min_motor_gain if(right_vel>self.max_motor_gain): right_vel=self.max_motor_gain if(right_vel<self.min_motor_gain): right_vel=self.min_motor_gain''' if(angular_vel<self.min_motor_gain): angular_vel=self.min_motor_gain if(angular_vel>self.max_motor_gain): angular_vel=self.max_motor_gain action_to_take = angular_vel #########[left_vel*math.pow(2,16)*0.001, right_vel*math.pow(2,16)*0.001] save_perp_dist= min_perp_dist save_forward_dist= curr_forward save_moved_to_next= moved_to_next save_desired_heading= angle_of_line save_curr_heading= pt[yaw_index] return action_to_take, save_perp_dist, save_forward_dist, save_moved_to_next, save_desired_heading, save_curr_heading
def publish_rviz_marker(self, stamp, ros_publisher, classes, positions, pos_covariances=None, track_ids=None): #remove all rviz markers before we publish the new ones self.delete_all_markers(ros_publisher) markers = MarkerArray() for i in range(len(classes)): cla = classes[i] #setup marker marker = Marker() marker.header.stamp = stamp marker.header.frame_id = positions[i]["frame_id"] if track_ids is not None: marker.id = track_ids[i] else: marker.id = i marker.ns = "mobility_aids" marker.type = Marker.SPHERE marker.action = Marker.MODIFY marker.lifetime = rospy.Duration() #maker position marker.pose.position.x = positions[i]["x"] marker.pose.position.y = positions[i]["y"] marker.pose.position.z = positions[i]["z"] #marker color color_box = self.viz_helper.colors_box[cla] marker.color.b = float(color_box[2]) marker.color.g = float(color_box[1]) marker.color.r = float(color_box[0]) marker.color.a = 1.0 #get error ellipse width, height, scale, angle = 0.5, 0.5, 0.5, 0.0 #if a pose covariance is given, like for tracking, plot ellipse marker if pos_covariances is not None: width, height, angle = Visualizer.get_error_ellipse( pos_covariances[i]) angle = angle + np.pi / 2 scale = 0.1 quat = tf.transformations.quaternion_from_euler(0, 0, angle) marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.scale.x = height marker.scale.y = width marker.scale.z = scale markers.markers.append(marker) ros_publisher.publish(markers)