def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None): markers = [] U = get_pr2_urdf() link = U.links[linkname] if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh): rospy.logdebug("%s is a mesh. drawing it.", linkname) marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD) marker.ns = ns marker.header = ps.header linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation)) origFromLink = conversions.pose_to_hmat(ps.pose) origFromGeom = np.dot(origFromLink, linkFromGeom) marker.pose = conversions.hmat_to_pose(origFromGeom) marker.scale = gm.Vector3(1,1,1) marker.color = stdm.ColorRGBA(1,1,0,.5) marker.id = 0 marker.lifetime = rospy.Duration() marker.mesh_resource = str(link.visual.geometry.filename) marker.mesh_use_embedded_materials = False markers.append(marker) else: rospy.logdebug("%s is not a mesh", linkname) if U.child_map.has_key(linkname): for (cjoint,clink) in U.child_map[linkname]: markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict)) return markers
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 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 __init__(self, publish_name, xWid,yHei): self.tf_listener = tf.TransformListener() self.tf_name = '' self.bb_publisher = rospy.Publisher(publish_name, Convex_Space) self.marker_pub = rospy.Publisher('/look_for_this/found_it/bounding_pyramids/markers', Marker) self.xWid = xWid self.yHei = yHei self.aspectXtoZ = 1.0 self.aspectYtoZ = 1.0 self.K_dict = {} global marker marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time() marker.ns = "" marker.id = 0 marker.type = 5 # 5 = LINELIST marker.action = 0 # 0 = ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 0.0 marker.scale.x = 0.01 marker.color.a = 1 marker.color.r = 0 marker.color.g = 0 marker.color.b = 0
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 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 __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 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 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(cls, x_cord, y_cord, mid): new_marker = Marker(type = Marker.CUBE, action=Marker.ADD) new_marker.header.frame_id = "/base_laser_front_link" new_marker.header.stamp = rospy.Time.now() new_marker.ns = "basic_shapes" new_marker.id = mid new_marker.pose.position.x = x_cord new_marker.pose.position.y = y_cord new_marker.pose.position.z = 0.0 #pointxyz new_marker.pose.orientation.x = 0 new_marker.pose.orientation.y = 0 new_marker.pose.orientation.z = 0.0 new_marker.pose.orientation.w = 1 new_marker.scale.x = 0.005 new_marker.scale.y = 0.005 new_marker.scale.z = 0.005 if mid == 0: new_marker.color.r = 1 elif mid == 5: new_marker.color.g = 1 elif mid == 10: new_marker.color.b = 1 new_marker.color.a = 1 new_marker.text = "marker" new_marker.lifetime = rospy.Duration(1) SmachGlobalData.pub_marker.publish(new_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 set_position_callback(self,marker,joy): print self.position_marker.ns print joy.buttons[3] == 1 print marker.ns == "PEOPLE" if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"): self.position_marker = marker print "set position" ref_marker = Marker() ref_marker.header.frame_id = "base_footprint" ref_marker.header.stamp = rospy.get_rostime() ref_marker.ns = "robot" ref_marker.type = 9 ref_marker.action = 0 ref_marker.pose.position.x = self.position_marker.pose.position.x ref_marker.pose.position.y = self.position_marker.pose.position.y ref_marker.pose.position.z = self.position_marker.pose.position.z ref_marker.scale.x = .25 ref_marker.scale.y = .25 ref_marker.scale.z = .25 ref_marker.color.r = 1.0 ref_marker.color.g = 0.0 ref_marker.color.a = 1.0 ref_marker.lifetime = rospy.Duration(0) self.ref_viz_pub.publish(ref_marker) else: pass
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(anns, data): ar_mk_list = AlvarMarkers() marker_list = MarkerArray() marker_id = 1 for a, d in zip(anns, data): # AR markers object = deserialize_msg(d.data, AlvarMarker) ar_mk_list.markers.append(object) # Visual markers marker = Marker() marker.id = marker_id marker.header = a.pose.header marker.type = a.shape marker.ns = "ar_mk_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('ar_mk_marker', MarkerArray, latch=True, queue_size=1) ar_mk_pub = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1) ar_mk_pub.publish(ar_mk_list) marker_pub.publish(marker_list) return
def __setMarker(self, id, waypoint, colors = [1,0,0,0]): try: marker = Marker() marker.header.frame_id = '/map' marker.header.stamp = rospy.Time.now() marker.ns = 'patrol' marker.id = id 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 = colors[0] marker.color.r = colors[1] marker.color.b = colors[2] marker.color.g = colors[3] marker.pose.orientation.w = 1.0 marker.pose.position.x = waypoint.target_pose.pose.position.x marker.pose.position.y = waypoint.target_pose.pose.position.y marker.pose.position.z = waypoint.target_pose.pose.position.z return marker except Exception as ex: rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
def createCubeMarker(offset=(0,0,0), marker_id = 0, rgba=(1,0,0,1), orientation=(0,0,0,1), scale=(0.1,0.1,0.1), frame_id="/map"): marker = Marker() marker.header.frame_id = frame_id marker.type = marker.CUBE marker.id = marker_id marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.a = rgba[3] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] obj_control = InteractiveMarkerControl() obj_control.always_visible = True obj_control.markers.append( marker ) return obj_control
def init_int_marker(self): self.ims = InteractiveMarkerServer("simple_marker") self.im = InteractiveMarker() self.im.header.frame_id = "/ned" self.im.name = "my_marker" self.im.description = "Simple 1-DOF control" bm = Marker() bm.type = Marker.CUBE bm.scale.x = bm.scale.y = bm.scale.z = 0.45 bm.color.r = 0.0 bm.color.g = 0.5 bm.color.b = 0.5 bm.color.a = 1.0 bc = InteractiveMarkerControl() bc.always_visible = True bc.markers.append(bm) self.im.controls.append(bc) rc = InteractiveMarkerControl() rc.name = "move_x" rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.im.controls.append(rc) self.ims.insert(self.im, self.process_marker_feedback) self.ims.applyChanges()
def pub_at_position(self, odom_msg): """ Handles necessary information for displaying things at ACCEPTED (NOVATEL) POSITION SOLUTION: -vehicle mesh !!!this should only be invoked when the accepted (novatel) position is updated """ ### G35 Mesh ############################################################# marker = Marker() pub = self.curpos_publisher marker.header = odom_msg.header marker.id = 0 # enumerate subsequent markers here marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY marker.pose = odom_msg.pose.pose marker.pose.position.z = 1.55 marker.lifetime = rospy.Duration() # will last forever unless modified marker.ns = "vehicle_model" marker.type = Marker.MESH_RESOURCE marker.scale.x = 0.0254 # artifact of sketchup export marker.scale.y = 0.0254 # artifact of sketchup export marker.scale.z = 0.0254 # artifact of sketchup export marker.color.r = .05 marker.color.g = .05 marker.color.b = .05 marker.color.a = .2 marker.mesh_resource = self.veh_mesh_resource marker.mesh_use_embedded_materials = False pub.publish(marker)
def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"): marker = Marker() marker.mesh_resource = resource; marker.header.frame_id = frame_id marker.type = marker.MESH_RESOURCE marker.scale.x = scale*scales[0] marker.scale.y = scale*scales[1] marker.scale.z = scale*scales[2] marker.color.a = rgba[3] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] obj_control = InteractiveMarkerControl() obj_control.always_visible = True obj_control.markers.append( marker ) return obj_control
def createPointMarker(points, colors, offset=(0,0,0), orientation=(0,0,0,1)): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.scale.x = 0.003 marker.scale.y = 0.003 marker.scale.z = 0.003 marker.id = 1 n = len(points)//3 for i in range(n): p = Point() p.x = points[i*3+0] p.y = points[i*3+1] p.z = points[i*3+2] marker.points.append(p) p = ColorRGBA() p.r = colors[i*3+0] p.g = colors[i*3+1] p.b = colors[i*3+2] p.a = 0.5 marker.colors.append(p) 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 = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] return marker
def location_marker(self, ID, location): marker = Marker() marker.header.frame_id = "torso" marker.ns = "headl" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 if ID == self.trackerTarget: marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 else: marker.color.r = 0.1 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = location[0] marker.pose.position.y = location[1] marker.pose.position.z = location[2] marker.lifetime.secs = 1 marker.id = int(ID) return marker
def createArrowMarker(points, color, offset=(0,0,0), orientation=(0,0,0,1)): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.ARROW marker.scale.x = 0.003 marker.scale.y = 0.005 marker.scale.z = 0 n = len(points)//3 marker.id = 2 for i in range(n): p = Point() p.x = points[i*3+0] p.y = points[i*3+1] p.z = points[i*3+2] marker.points.append(p) marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] 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 = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] return marker
def draw_base(self): marker = Marker() ratio = (self.rod_position[CENTER][1] - self.rod_position[LEFT][1]) / (self.rod_position[CENTER][0] - self.rod_position[LEFT][0]) alpha = numpy.arctan(ratio) - numpy.pi / 2.0 marker.header.frame_id = "/pl_base" marker.ns = "basic_shapes" marker.id = 5 marker.type = marker.CUBE marker.action = marker.ADD marker.pose.position.x = self.rod_position[CENTER][0] marker.pose.position.y = self.rod_position[CENTER][1] marker.pose.position.z = self.position_z + DIFF_DISTANCE_BASE marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = numpy.sin(alpha / 2.0) marker.pose.orientation.w = numpy.cos(alpha / 2.0) marker.scale.x = BASE_SCALE_XYZ[0] marker.scale.y = BASE_SCALE_XYZ[1] marker.scale.z = BASE_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)
def create_object_marker(self, soma_obj, soma_type, pose): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = soma_obj int_marker.description = "id" + soma_obj int_marker.pose = pose mesh_marker = Marker() mesh_marker.type = Marker.MESH_RESOURCE mesh_marker.scale.x = 1 mesh_marker.scale.y = 1 mesh_marker.scale.z = 1 random.seed(soma_type) val = random.random() mesh_marker.color.r = r_func(val) mesh_marker.color.g = g_func(val) mesh_marker.color.b = b_func(val) mesh_marker.color.a = 1.0 #mesh_marker.pose = pose mesh_marker.mesh_resource = self.mesh[soma_type] control = InteractiveMarkerControl() control.markers.append(mesh_marker) int_marker.controls.append(control) return int_marker
def line(self, frame_id, p, r, t=[0.0, 0.0], key=None): """ line: t r + p This will be drawn for t[0] .. t[1] """ r = np.array(r) p = np.array(p) m = Marker() m.header.frame_id = frame_id m.ns = key or 'lines' m.id = 0 if key else len(self.lines) m.type = Marker.LINE_STRIP m.action = Marker.MODIFY m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1)) m.scale = Vector3(0.005, 0.005, 0.005) m.color = ColorRGBA(0,0.8,0.8,1) m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)] m.colors = [m.color] * 10 key = key or element_key(m) with self.mod_lock: self.lines[key] = m return key
def _delete_markers(self): marker = Marker() marker.action = 3 marker.header.frame_id = "map" markerarray = MarkerArray() markerarray.markers.append(marker) self.markerpub.publish(markerarray)
def get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0,1,0,1), idx=0): (mesh, pose) = self.get_link_data(link) marker = Marker() if offset==None : marker.pose = pose else : marker.pose = toMsg(fromMsg(offset)*fromMsg(pose)) marker.header.frame_id = root marker.header.stamp = rospy.get_rostime() marker.ns = self.robot_name marker.mesh_resource = mesh marker.type = Marker.MESH_RESOURCE marker.action = Marker.MODIFY marker.scale.x = scale marker.scale.y = scale marker.scale.z = scale marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.text = link marker.id = idx marker.mesh_use_embedded_materials = True return marker
def createMarkerLine(pos_list, color = (.1, .1, 1), ID = 0, alpha = 1., size = 0.5): marker = Marker() marker.header.frame_id = "/openni_depth_frame" marker.id = ID marker.type = marker.LINE_STRIP marker.action = marker.ADD marker.scale.x = size marker.scale.y = size marker.scale.z = size marker.color.a = alpha marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.pose.orientation.w = 1.0 marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 for p in pos_list: marker.points.append( Point(p[0],p[1],0) ) return marker
def create_object_marker(self, soma_obj, roi, soma_type, pose,markerno): # create an interactive marker for our server marker = Marker() marker.header.frame_id = "map" #int_marker.name = soma_obj+'_'+str(markerno) #int_marker.description = soma_type + ' (' + roi +'_'+str(markerno)+ ')' marker.pose = pose marker.id = markerno; # print marker.pose marker.pose.position.z = 0.01 #marker = Marker() marker.type = Marker.SPHERE marker.action = 0 marker.scale.x = 0.25 marker.scale.y = 0.25 marker.scale.z = 0.25 marker.pose.position.z = (marker.scale.z / 2) random.seed(soma_type) val = random.random() marker.color.r = r_func(val) marker.color.g = g_func(val) marker.color.b = b_func(val) marker.color.a = 1.0 #marker.pose = pose return marker
def createSphereMarker(color, scale, offset=(0,0,0), orientation=(0,0,0,1)): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.SPHERE marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.id = 1 p = ColorRGBA() p.r = color[0] p.g = color[1] p.b = color[2] p.a = color[3] marker.color = p 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 = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] return marker
j.header.stamp.secs = now.secs j.header.stamp.nsecs = now.nsecs j.header.frame_id = '/camera_link' pose1 = Pose() pose1.position.x = now.secs % 10 pose1.position.y = 0.1 pose1.position.z = 0.9 pose1.orientation.w = 1.0 pose2 = Pose() pose2.position.x = 1 pose2.position.y = -0.1 pose2.position.z = 0.9 pose2.orientation.w = 1.0 markerPeople = Marker() markerPeople.header = Header() markerPeople.header.stamp.secs = now.secs markerPeople.header.stamp.nsecs = now.nsecs markerPeople.header.frame_id = '/camera_link' markerPeople.ns='people_tracker' markerPeople.id=1 markerPeople.type=markerPeople.CUBE scalePeople = Vector3() scalePeople.x=0.2 scalePeople.y=0.2 scalePeople.z=0.2 markerPeople.scale=scalePeople colorPeople = ColorRGBA() colorPeople.r=0.0 colorPeople.g=1.0
def marker_clear_all(frame_id): # Create a marker which clears all. marker = Marker() marker.header.frame_id = frame_id; marker.action = 3 # DELETEALL action. return marker
def find_best_angle(self): abs_real_angles = np.absolute(self.real_laser_angles) relevant_ranges = self.laser_ranges.copy() relevant_ranges[abs_real_angles > math.pi / 2] = 0.5 abs_real_angles *= -1 #self.laser_ranges[abs_real_angles > math.pi/2] = 0.5 # don't consider points behind car best_dist_index = np.lexsort((abs_real_angles, relevant_ranges))[-1] dist = self.laser_ranges[best_dist_index] angle = normalize_angle(self.real_laser_angles[best_dist_index] + self.FORWARD_ANGLE) # check the scan to the left and scan to the right # bias [angle proportional to distance] toward the scan with larger dist if self.best_pt_valid: # update the best_pt info (but not the actual best_pt) since we've moved closer to it self.best_pt_dist = np.hypot( self.cur_pose.position.x - self.best_pt[0], self.cur_pose.position.y - self.best_pt[1]) self.best_pt_angle = normalize_angle(math.atan2(self.best_pt[1] - self.cur_pose.position.y , \ self.best_pt[0] - self.cur_pose.position.x)) if self.best_pt_dist > 11: # out of range print "condition 1" self.best_pt_valid = False else: lidar_min = self.cur_angle + self.laser_scan_min # verified #index = int(np.hypot((self.best_pt_angle+4*math.pi)%(2*math.pi) ,(lidar_min+4*math.pi)%(2*math.pi)) / (self.STEP_SIZE * self.laser_scan_inc)) positive_best_pt_angle = self.best_pt_angle if self.best_pt_angle > 0 else self.best_pt_angle + 2 * math.pi positive_lidar_min = lidar_min if lidar_min > 0 else lidar_min + 2 * math.pi index = int((positive_best_pt_angle - positive_lidar_min) / (self.STEP_SIZE * self.laser_scan_inc)) if index > (self.real_laser_angles_size - 1) or index < 0: self.best_pt_valid = False print "condition 2" else: thresh = self.BLOCKED_TRAJ_THRESH self.best_pt_valid = (abs(self.laser_ranges[index-1] - self.best_pt_dist) < thresh or \ abs(self.laser_ranges[index] - self.best_pt_dist) < thresh or \ abs(self.laser_ranges[index+1] - self.best_pt_dist) < thresh) if not self.best_pt_valid: print "condition 3" #ps = PoseStamped() #ps.header = Utils.make_header("map") #ps.pose.position.x = self.cur_pose.position.x #ps.pose.position.y = self.cur_pose.position.y #ps.pose.orientation = Utils.angle_to_quaternion(self.cur_angle + self.forward_dir_delta) ##ps.pose.orientation = Utils.angle_to_quaternion(self.best_pt_angle if self.best_pt_angle > 0 else self.best_pt_angle) #self.forward_dir_pub.publish(ps) #or abs(abs(self.best_pt_angle - self.FORWARD_ANGLE) - \ # abs(angle - self.FORWARD_ANGLE)) > self.NEW_PT_ANGLE_THRESH \ if (not self.best_pt_valid or abs(self.best_pt_dist - dist) > self.NEW_PT_DIST_THRESH): print "condition 3" self.best_pt_dist = dist self.best_pt_angle = angle self.best_pt = (self.cur_pose.position.x + dist*math.cos(angle), \ self.cur_pose.position.y + dist*math.sin(angle)) self.best_pt_valid = True #left_scan = self.real_laser_angles[best_dist_index-1] if best_dist_index > 0 else 0 #right_scan = self.real_laser_angles[best_dist_index+1] if best_dist_index < self.real_laser_angles_size else 0 #best_dist_angle = best_dist_angle + (-1 if left_scan < right_scan else 1) * 0.1*((math.pi/2)/((max(min(left_scan, right_scan), 0))+1)) if self.best_pt is not None: traj = Marker() traj.header = Utils.make_header("map") traj.ns = "my_namespace" traj.id = 0 traj.type = 5 #LINELIST traj.action = 0 # ADD traj.pose.position.x = 0 traj.pose.position.y = 0 traj.pose.position.z = 0 traj.pose.orientation.x = 0.0 traj.pose.orientation.y = 0.0 traj.pose.orientation.z = 0.0 traj.pose.orientation.w = 0.0 traj.scale.x = 0.1 traj.scale.y = 0.1 traj.scale.z = 0.1 traj.color.a = 1.0 traj.color.r = 1.0 traj.color.g = 0.0 traj.color.b = 0.0 traj.points = [ Point(self.cur_pose.position.x, self.cur_pose.position.y, 0), Point(self.best_pt[0], self.best_pt[1], 0) ] collide = Marker() collide.header = Utils.make_header("map") collide.ns = "my_namespace2" collide.id = 0 collide.type = 8 #POINT CLOUD collide.action = 0 # ADD collide.pose.position.x = 0 collide.pose.position.y = 0 collide.pose.position.z = 0 collide.pose.orientation.x = 0.0 collide.pose.orientation.y = 0.0 collide.pose.orientation.z = 0.0 collide.pose.orientation.w = 0.0 collide.scale.x = 0.2 collide.scale.y = 0.2 collide.scale.z = 0.2 collide.color.a = 1.0 collide.color.r = 1.0 collide.color.g = 1.0 collide.color.b = 0.0 #points = [Point(x[0], x[1], 0) for x in self.pol2cart(self.cur_pose.position.x, self.cur_pose.position.y, self.FORWARD_ANGLE, self.laser_ranges[self.points_in_box], self.real_laser_angles[self.points_in_box])] #print self.points_in_box #print self.closest_pt_index if self.closest_pt_index is not None: points = [ Point(x[0], x[1], 0) for x in self.pol2cart( self.cur_pose.position.x, self.cur_pose.position.y, self.FORWARD_ANGLE, self.laser_ranges[ self.closest_pt_index], self.real_laser_angles[ self.closest_pt_index]) ] else: points = [] collide.points = points deflected = Marker() deflected.header = Utils.make_header("map") deflected.ns = "my_namespace3" deflected.id = 0 deflected.type = 5 #LINELIST deflected.action = 0 # ADD deflected.pose.position.x = 0 deflected.pose.position.y = 0 deflected.pose.position.z = 0 deflected.pose.orientation.x = 0.0 deflected.pose.orientation.y = 0.0 deflected.pose.orientation.z = 0.0 deflected.pose.orientation.w = 0.0 deflected.scale.x = 0.1 deflected.scale.y = 0.1 deflected.scale.z = 0.1 deflected.color.a = 1.0 deflected.color.r = 0.5 deflected.color.g = 1.0 deflected.color.b = 0.75 if self.deflected_pt is not None: deflected.points = [ Point(self.cur_pose.position.x, self.cur_pose.position.y, 0), Point(self.deflected_pt[0], self.deflected_pt[1], 0) ] markers = MarkerArray([collide, traj, deflected]) self.free_space_path_pub.publish(markers)
def get_ekf_msgs(ekf): """ Create messages to visualize EKFs. The messages are odometry and uncertainity. :param EKF ekf: the EKF filter. :returns: a list of messages containing [the odometry of the filter, the uncertainty, the translation from origin, the rotation from origin, the map lines. :rtype: :py:obj:`list` """ # Time time = rospy.Time.now() # Odometry msg_odom = Odometry() msg_odom.header.stamp = time msg_odom.header.frame_id = 'world' msg_odom.pose.pose.position.x = ekf.xk[0] msg_odom.pose.pose.position.y = ekf.xk[1] msg_odom.pose.pose.position.z = 0 quat = quaternion_from_yaw(ekf.xk[2]) msg_odom.pose.pose.orientation.x = quat[0] msg_odom.pose.pose.orientation.y = quat[1] msg_odom.pose.pose.orientation.z = quat[2] msg_odom.pose.pose.orientation.w = quat[3] # Uncertainity uncert = ekf.Pk[:2, :2].copy() val, vec = np.linalg.eigh(uncert) yaw = np.arctan2(vec[1, 0], vec[0, 0]) quat = quaternion_from_yaw(yaw) msg_ellipse = Marker() msg_ellipse.header.frame_id = "world" msg_ellipse.header.stamp = time msg_ellipse.type = Marker.CYLINDER msg_ellipse.pose.position.x = ekf.xk[0] msg_ellipse.pose.position.y = ekf.xk[1] msg_ellipse.pose.position.z = -0.1 # below others msg_ellipse.pose.orientation.x = quat[0] msg_ellipse.pose.orientation.y = quat[1] msg_ellipse.pose.orientation.z = quat[2] msg_ellipse.pose.orientation.w = quat[3] msg_ellipse.scale.x = 2 * math.sqrt(val[0]) msg_ellipse.scale.y = 2 * math.sqrt(val[1]) msg_ellipse.scale.z = 0.05 msg_ellipse.color.a = 0.6 msg_ellipse.color.r = 0.0 msg_ellipse.color.g = 0.7 msg_ellipse.color.b = 0.7 # TF trans = (msg_odom.pose.pose.position.x, msg_odom.pose.pose.position.y, msg_odom.pose.pose.position.z) rotat = (msg_odom.pose.pose.orientation.x, msg_odom.pose.pose.orientation.y, msg_odom.pose.pose.orientation.z, msg_odom.pose.pose.orientation.w) # Reconstruct the map to visualize (if is SLAM) room_map_polar = np.zeros((0, 2)) room_map_points = np.zeros((0, 4)) if hasattr(ekf, 'get_number_of_features_in_map'): for i in range(0, ekf.get_number_of_features_in_map()): if ekf.featureObservedN.shape[0] == 0 or \ ekf.featureObservedN[i] >= ekf.min_observations: rho = ekf.xk[2 * i + 3] phi = ekf.xk[2 * i + 4] plline = np.array([rho, phi]) room_map_polar = np.vstack([room_map_polar, plline]) aux = np.zeros((1, 4)) if np.abs(np.abs(phi) - np.pi / 2) < np.deg2rad(45): # Horizontal line aux[0, 0] = -5 aux[0, 2] = 5 aux[0, 1] = polar2y(plline, aux[0, 0]) aux[0, 3] = polar2y(plline, aux[0, 2]) else: # Vertical line aux[0, 1] = -5 aux[0, 3] = 5 aux[0, 0] = polar2x(plline, aux[0, 1]) aux[0, 2] = polar2x(plline, aux[0, 3]) aux[0, 1] = polar2y(plline, aux[0, 0]) aux[0, 3] = polar2y(plline, aux[0, 2]) room_map_points = np.vstack([room_map_points, aux]) return msg_odom, msg_ellipse, trans, rotat, room_map_points
def get_particle_msgs(pfilter, time): """ Create messages to visualize particle filters. First message contains all particles. Second message contains the particle representing the whole filter. :param ParticleFilter pfilter: the particle filter. :param rospy.Time time: the timestamp for the message. :returns: a list of messages containing [all the particles positions, the mean particle, the mean odometry, the translation to the mean particle, the rotation to the mean particle, the weights] :rtype: :py:obj:`list` """ # Pose array msg = PoseArray() msg.header.stamp = time msg.header.frame_id = 'world' # Weights as spheres msg_weight = MarkerArray() idx = 0 wmax = pfilter.p_wei.max() wmin = pfilter.p_wei.min() if wmax == wmin: wmin = 0.0 for i in range(pfilter.num): # Pose m = Pose() m.position.x = pfilter.p_xy[0, i] m.position.y = pfilter.p_xy[1, i] m.position.z = 0 quat = quaternion_from_euler(0, 0, pfilter.p_ang[i]) m.orientation.x = quat[0] m.orientation.y = quat[1] m.orientation.z = quat[2] m.orientation.w = quat[3] # Append msg.poses.append(m) # Marker constant marker = Marker() marker.header.frame_id = "world" marker.header.stamp = time marker.ns = "weights" marker.type = marker.SPHERE marker.action = marker.ADD marker.color.a = 0.5 marker.color.r = 1.0 marker.color.g = 0.55 marker.color.b = 0.0 # MArker variable marker.id = idx marker.pose.position.x = pfilter.p_xy[0, i] marker.pose.position.y = pfilter.p_xy[1, i] marker.pose.position.z = 0 marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] scale = 0.005 + 0.08 * (pfilter.p_wei[i] - wmin) / (wmax - wmin) marker.scale.x = scale marker.scale.y = scale marker.scale.z = 0.02 idx += 1 msg_weight.markers.append(marker) # Pose Stamped msg_mean = PoseStamped() msg_mean.header.stamp = time msg_mean.header.frame_id = 'world' particle = pfilter.get_mean_particle() msg_mean.pose.position.x = particle[0] msg_mean.pose.position.y = particle[1] msg_mean.pose.position.z = 0 quat = quaternion_from_euler(0, 0, particle[2]) msg_mean.pose.orientation.x = quat[0] msg_mean.pose.orientation.y = quat[1] msg_mean.pose.orientation.z = quat[2] msg_mean.pose.orientation.w = quat[3] # Odometry msg_odom = Odometry() msg_odom.header.stamp = time msg_odom.header.frame_id = 'world' msg_odom.pose.pose.position = msg_mean.pose.position msg_odom.pose.pose.orientation = msg_mean.pose.orientation # TF trans = (msg_odom.pose.pose.position.x, msg_odom.pose.pose.position.y, msg_odom.pose.pose.position.z) rotat = (msg_odom.pose.pose.orientation.x, msg_odom.pose.pose.orientation.y, msg_odom.pose.pose.orientation.z, msg_odom.pose.pose.orientation.w) return msg, msg_mean, msg_odom, trans, rotat, msg_weight
def createMarker(m_id, m_type, action, pose, scale, color, target_frame, text=None): global _marker_seq marker = Marker() marker.header.frame_id = target_frame marker.header.stamp = rospy.Time.now() marker.header.seq = _marker_seq _marker_seq += 1 marker.ns = "people_tracker" marker.id = m_id marker.type = m_type if text is not None: marker.text = text marker.action = action marker.pose = pose marker.scale = scale marker.color = color marker.lifetime = rospy.Duration(1) return marker
def triangulate(self, data): # rospy.logwarn("triangulating: sidelen = "+str(len(self.fishuvside))+" toplen: "+str(len(self.fishuvtop))) if ((len(self.fishuvside) > 0)): if ( 1 ): #( (self.currentsidestamp != self.lastsidestamp) and (self.currenttopstamp!=self.lasttopstamp) ): self.lastsidestamp = self.currentsidestamp #print "triangulating!" D2 = array([0.0, 0.0, 0.0, 0.0]) # This works! K2 = array([self.CIside.K]).reshape(3, 3) x2 = self.fishuvside #vstack(self.fishuvside) # x2 = vstack((x2,1)) #use the camera information and an estimate of fish plane depth to unproject rospy.logwarn("broadcasting measured fish!") # try: if 1: fishpos = self.Unproject(x2, array([1.0]), K2, D2) fishpos = fishpos[0] rospy.logwarn(fishpos) # fishpos/=fishpos[3] #print ballpos #send transforms to show ball's coordinate system br = tf.TransformBroadcaster() fishquat = tf.transformations.quaternion_from_euler( 0, 0, self.angle) #I think the ball position needs to be inverted... why? Not sure but I think #it may be because there is a confusion in the T matrix between camera->object vs. object->camera if not isnan(fishpos).any(): if not isinf(fishpos).any(): br.sendTransform( [fishpos[0], fishpos[1], fishpos[2]], fishquat, rospy.Time.now(), '/fishmeasured', 'camera2') else: rospy.logerr("FISH PISITION INFINITE") else: rospy.logerr("FISH POSITION IS NAN") #create a marker fishmarker = Marker() fishmarker.header.frame_id = '/fishmeasured' fishmarker.header.stamp = rospy.Time.now() fishmarker.type = fishmarker.SPHERE fishmarker.action = fishmarker.MODIFY fishmarker.scale.x = .12 fishmarker.scale.y = .12 fishmarker.scale.z = .12 fishmarker.pose.orientation.w = 1 fishmarker.pose.orientation.x = 0 fishmarker.pose.orientation.y = 0 fishmarker.pose.orientation.z = 0 fishmarker.pose.position.x = 0 fishmarker.pose.position.y = 0 fishmarker.pose.position.z = 0 fishmarker.color.r = 0.0 fishmarker.color.g = 0 fishmarker.color.b = 1.0 fishmarker.color.a = 0.5 self.markerpub.publish(fishmarker) posemsg = PoseStamped() posemsg.header.stamp = rospy.Time.now() posemsg.pose.position.x = 0. - fishpos[0] posemsg.pose.position.y = 0. - fishpos[1] posemsg.pose.position.z = 0. - fishpos[2] self.posepub.publish(posemsg)
def new_goal(self, psm): rospy.loginfo("[%s] I just got a goal location. I will now start reaching!" %rospy.get_name()) psm.header.stamp = rospy.Time.now() #self.goal_pose_pub.publish(psm) self.pub_feedback("Reaching toward goal location") #return # This is to use tf to get head location. # Otherwise, there is a subscriber to get a head location. #Comment out if there is no tf to use. #now = rospy.Time.now() + rospy.Duration(0.5) #self.listener.waitForTransform('/base_link', '/head_frame', now, rospy.Duration(10)) #pos_temp, ori_temp = self.listener.lookupTransform('/base_link', '/head_frame', now) #self.head = createBMatrix(pos_temp,ori_temp) #self.pr2_B_wc = np.matrix([[ self.head[0,0], self.head[0,1], 0., self.head[0,3]], # [ self.head[1,0], self.head[1,1], 0., self.head[1,3]], # [ 0., 0., 1., 0.], # [ 0., 0., 0., 1]]) #self.pr2_B_wc = wheelchair_location = self.pr2_B_wc * self.corner_B_head.I self.wheelchair.SetTransform(np.array(wheelchair_location)) pos_goal = wheelchair_location[:3,3] ori_goal = tr.matrix_to_quaternion(wheelchair_location[0:3,0:3]) marker = Marker() marker.header.frame_id = "base_link" marker.header.stamp = rospy.Time() marker.ns = "arm_reacher_wc_model" marker.id = 0 marker.type = Marker.MESH_RESOURCE; marker.action = Marker.ADD marker.pose.position.x = pos_goal[0] marker.pose.position.y = pos_goal[1] marker.pose.position.z = pos_goal[2] marker.pose.orientation.x = ori_goal[0] marker.pose.orientation.y = ori_goal[1] marker.pose.orientation.z = ori_goal[2] marker.pose.orientation.w = ori_goal[3] marker.scale.x = 0.0254 marker.scale.y = 0.0254 marker.scale.z = 0.0254 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 #only if using a MESH_RESOURCE marker type: marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae" self.vis_pub.publish( marker ) v = self.robot.GetActiveDOFValues() for name in self.joint_names: v[self.robot.GetJoint(name).GetDOFIndex()] = self.joint_angles[self.joint_names.index(name)] self.robot.SetActiveDOFValues(v) pos_temp = [psm.pose.position.x, psm.pose.position.y, psm.pose.position.z] ori_temp = [psm.pose.orientation.x, psm.pose.orientation.y, psm.pose.orientation.z, psm.pose.orientation.w] self.goal = createBMatrix(pos_temp,ori_temp) self.goal_B_gripper = np.matrix([[ 0., 0., 1., 0.1], [ 0., 1., 0., 0.], [ -1., 0., 0., 0.], [ 0., 0., 0., 1.]]) self.pr2_B_goal = self.goal*self.goal_B_gripper self.sol = self.manip.FindIKSolution(np.array(self.pr2_B_goal), op.IkFilterOptions.CheckEnvCollisions) if self.sol is None: self.pub_feedback("Failed to find a good arm configuration for reaching.") return None rospy.loginfo("[%s] Got an IK solution: %s" % (rospy.get_name(), self.sol)) self.pub_feedback("Found a good arm configuration for reaching.") self.pub_feedback("Looking for path for arm to goal.") traj = None try: #self.res = self.manipprob.MoveToHandPosition(matrices=[np.array(self.pr2_B_goal)],seedik=10) # call motion planner with goal joint angles self.traj=self.manipprob.MoveManipulator(goal=self.sol,outputtrajobj=True) self.pub_feedback("Found a path to reach to the goal.") except: self.traj = None self.pub_feedback("Could not find a path to reach to the goal") return None tmp_traj = tmp_vel = tmp_acc = [] #np.zeros([self.traj.GetNumWaypoints(),7]) trajectory = JointTrajectory() for i in xrange(self.traj.GetNumWaypoints()): point = JointTrajectoryPoint() temp = [] for j in xrange(7): temp.append(float(self.traj.GetWaypoint(i)[j])) point.positions = temp #point.positions.append(temp) #point.accelerations.append([0.,0.,0.,0.,0.,0.,0.]) #point.velocities.append([0.,0.,0.,0.,0.,0.,0.]) trajectory.points.append(point) #tmp_traj.append(temp) #tmp_traj.append(list(self.traj.GetWaypoint(i))) #tmp_vel.append([0.,0.,0.,0.,0.,0.,0.]) #tmp_acc.append([0.,0.,0.,0.,0.,0.,0.]) #print 'tmp_traj is: \n', tmp_traj #for j in xrange(7): #tmp_traj[i,j] = float(self.traj.GetWaypoint(i)[j]) #trajectory = JointTrajectory() #point = JointTrajectoryPoint() #point.positions.append(tmp_traj) #point.velocities.append(tmp_vel) #point.accelerations.append(tmp_acc) #point.velocities=[[0.,0.,0.,0.,0.,0.,0.]] #point.accelerations=[[0.,0.,0.,0.,0.,0.,0.]] trajectory.joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] #trajectory.points.append(point) #self.mpc_weights_pub.publish(self.weights) self.goal_traj_pub.publish(trajectory) self.pub_feedback("Reaching to location")
samplePoints = [] for num in range(len(splinex2values)): samplePoints.append( [float(splinex2values[num]), float(spliney2values[num])]) print( "########################Now showing spline in rviz##########################" ) while not rospy.is_shutdown(): ###################10-1############################ #initialisation for marker1: spline1 marker1 = Marker() marker1.type = marker1.LINE_STRIP marker1.action = marker1.ADD marker1.header.frame_id = "map" marker1.header.stamp = rospy.Time() marker1.ns = "marker_1" marker1.id = 1 marker1.scale.x = 0.03 marker1.pose.position.x = 0.0 marker1.pose.position.y = 0.0 marker1.pose.position.z = 0.0 marker1.pose.orientation.x = 0.0 marker1.pose.orientation.y = 0.0 marker1.pose.orientation.z = 0.0 marker1.pose.orientation.w = 1.0 marker1.color.a = 1.0
def select_base_client(): angle = -m.pi/2 pr2_B_head1 = np.matrix([[ m.cos(angle), -m.sin(angle), 0., 0.], [ m.sin(angle), m.cos(angle), 0., 2.5], [ 0., 0., 1., 1.1546], [ 0., 0., 0., 1.]]) an = -m.pi/2 pr2_B_head2 = np.matrix([[ m.cos(an), 0., m.sin(an), 0.], [ 0., 1., 0., 0.], [ -m.sin(an), 0., m.cos(an), 0.], [ 0., 0., 0., 1.]]) pr2_B_head=pr2_B_head1*pr2_B_head2 pos_goal = [pr2_B_head[0,3],pr2_B_head[1,3],pr2_B_head[2,3]] ori_goal = tr.matrix_to_quaternion(pr2_B_head[0:3,0:3]) psm_head = PoseStamped() psm_head.header.frame_id = '/base_link' psm_head.pose.position.x=pos_goal[0] psm_head.pose.position.y=pos_goal[1] psm_head.pose.position.z=pos_goal[2] psm_head.pose.orientation.x=ori_goal[0] psm_head.pose.orientation.y=ori_goal[1] psm_head.pose.orientation.z=ori_goal[2] psm_head.pose.orientation.w=ori_goal[3] head_pub = rospy.Publisher("/haptic_mpc/head_pose", PoseStamped, latch=True) head_pub.publish(psm_head) rospack = rospkg.RosPack() pkg_path = rospack.get_path('hrl_base_selection') marker = Marker() marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time() marker.ns = "base_service_wc_model" marker.id = 0 #marker.type = Marker.SPHERE marker.type = Marker.MESH_RESOURCE; marker.action = Marker.ADD marker.pose.position.x = pos_goal[0] marker.pose.position.y = pos_goal[1] marker.pose.position.z = 0 marker.pose.orientation.x = ori_goal[0] marker.pose.orientation.y = ori_goal[1] marker.pose.orientation.z = ori_goal[2] marker.pose.orientation.w = ori_goal[3] marker.scale.x = .025 marker.scale.y = .025 marker.scale.z = .025 marker.color.a = 1. marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 #only if using a MESH_RESOURCE marker type: marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae"#~/git/gt-ros-pkg.hrl-assistive/hrl_base_selection/models/ADA_Wheelchair.dae" # ''.join([pkg_path, '/models/ADA_Wheelchair.dae']) #"package://pr2_description/meshes/base_v0/base.dae" vis_pub.publish( marker ) print 'I think I just published the wc model \n' goal_angle = 0#m.pi/2 pr2_B_goal = np.matrix([[ m.cos(goal_angle), -m.sin(goal_angle), 0., 2.6], [ m.sin(goal_angle), m.cos(goal_angle), 0., .3], [ 0., 0., 1., 1.2], [ 0., 0., 0., 1.]]) pos_goal = [pr2_B_goal[0,3],pr2_B_goal[1,3],pr2_B_goal[2,3]] ori_goal = tr.matrix_to_quaternion(pr2_B_goal[0:3,0:3]) psm_goal = PoseStamped() psm_goal.header.frame_id = '/base_link' psm_goal.pose.position.x=pos_goal[0] psm_goal.pose.position.y=pos_goal[1] psm_goal.pose.position.z=pos_goal[2] psm_goal.pose.orientation.x=ori_goal[0] psm_goal.pose.orientation.y=ori_goal[1] psm_goal.pose.orientation.z=ori_goal[2] psm_goal.pose.orientation.w=ori_goal[3] goal_pub = rospy.Publisher("/arm_reacher/goal_pose", PoseStamped, latch=True) goal_pub.publish(psm_goal) # task = 'shaving' task = 'yogurt' model = 'chair' start_time = time.time() rospy.wait_for_service('select_base_position') try: #select_base_position = rospy.ServiceProxy('select_base_position', BaseMove) #response = select_base_position(psm_goal, psm_head) select_base_position = rospy.ServiceProxy('select_base_position', BaseMove_multi) response = select_base_position(task, model) print 'response is: \n', response print 'Total time for service to return the response was: %fs' % (time.time()-start_time) return response.base_goal, response.configuration_goal#, response.ik_solution except rospy.ServiceException, e: print "Service call failed: %s"%e
def rrt(): global x_goal global y_goal global wrapper global epsilon global max_distance_neighbors global final_path_pub global done global x_origin global y_origin global instance number_iteration=10000 nodes_origin_pub = rospy.Publisher('/nodes_origin', Marker, queue_size=10) tree_origin_pub = rospy.Publisher('/branches_origin', Marker, queue_size=10) nodes_goal_pub = rospy.Publisher('/nodes_goal', Marker, queue_size=10) tree_goal_pub = rospy.Publisher('/branches_goal', Marker, queue_size=10) rate = rospy.Rate(10) marker_nodes_origin=Marker() marker_nodes_origin.type=8 marker_nodes_origin.header.frame_id='odom' marker_nodes_origin.scale.x=0.2 marker_nodes_origin.scale.y=0.2 marker_nodes_origin.color.r = 1.0 marker_nodes_origin.color.a = 1.0 marker_nodes_goal=Marker() marker_nodes_goal.type=8 marker_nodes_goal.header.frame_id='odom' marker_nodes_goal.scale.x=0.2 marker_nodes_goal.scale.y=0.2 marker_nodes_goal.color.b = 1.0 marker_nodes_goal.color.a = 1.0 marker_branches_origin=Marker() marker_branches_origin.type=5 marker_branches_origin.header.frame_id='odom' marker_branches_origin.scale.x=0.2 marker_branches_origin.color.b=1.0 marker_branches_origin.color.a=1.0 marker_branches_goal=Marker() marker_branches_goal.type=5 marker_branches_goal.header.frame_id='odom' marker_branches_goal.scale.x=0.2 marker_branches_goal.color.r=1.0 marker_branches_goal.color.a=1.0 # RRT Inicijaliziranje stabala origin_point = Node((x_origin, y_origin)) goal_point = Node((float(x_goal), float(y_goal))) tree_origin = Tree((origin_point)) tree_goal = Tree((goal_point)) point_link_origin = tree_origin.list_of_points[0] origin_exploitation = point_link_origin index_origin = 0 point_link_goal = tree_goal.list_of_points[0] goal_exploitation = point_link_goal index_goal = 0 path_length = wrapper.length_m * wrapper.width_m output_data = [] for i in range(number_iteration): nodes_origin=Point() nodes_goal = Point() exploration_duration = 0 exploration = random.uniform(0.0, 1.0) if exploration < 0.8: start_time = time.time() rand=(random.uniform(float(wrapper.origin_x),float(wrapper.length_m/2)),random.uniform(float(wrapper.origin_y),float(wrapper.width_m/2))) distance, index = spatial.KDTree(tree_origin.list_of_points).query(rand) point_link_origin = tree_origin.list_of_points[index] new_point_origin = join_rrt(point_link_origin, rand) if new_point_origin != None: indices_of_neighboring_points_origin = spatial.KDTree(tree_origin.list_of_points).query_ball_point(new_point_origin, max_distance_neighbors) new_point_origin_distance_traveled_min = 1600.0 index_min = index for index_neigboring_points_origin in indices_of_neighboring_points_origin: neighboring_node_origin = tree_origin.list_of_nodes[index_neigboring_points_origin] distance_neighbors_origin = euclidian_distance(new_point_origin, neighboring_node_origin.data) if join_prm(new_point_origin, neighboring_node_origin.data) is not None and neighboring_node_origin.distance_traveled + distance_neighbors_origin < new_point_origin_distance_traveled_min: index_min = index_neigboring_points_origin new_point_origin_distance_traveled_min = neighboring_node_origin.distance_traveled + distance_neighbors_origin new_node_origin = Node(new_point_origin, tree_origin.list_of_nodes[index_min]) tree_origin.dodaj(new_node_origin) nodes_origin.x = new_point_origin[0] nodes_origin.y = new_point_origin[1] marker_nodes_origin.points.append(nodes_origin) nodes_origin_pub.publish(marker_nodes_origin) for index_neigboring_points_origin in indices_of_neighboring_points_origin: neighboring_node_origin = tree_origin.list_of_nodes[index_neigboring_points_origin] distance_neighbors_origin = euclidian_distance(new_node_origin.data, neighboring_node_origin.data) if neighboring_node_origin.distance_traveled > new_node_origin.distance_traveled + distance_neighbors_origin and distance_neighbors_origin <= max_distance_neighbors: linking_point_origin = join_rrt(new_node_origin.data, neighboring_node_origin.data) if linking_point_origin is not None and euclidian_distance(linking_point_origin, neighboring_node_origin.data) <= 2 * epsilon: previous_parent_index = tree_origin.list_of_nodes.index(neighboring_node_origin.parent) neighboring_node_origin.change_parent(new_node_origin, previous_parent=tree_origin.list_of_nodes[previous_parent_index]) distance_origin_goal, index_origin_goal = spatial.KDTree(tree_goal.list_of_points).query(new_point_origin) if (distance_origin_goal + new_node_origin.distance_traveled + tree_goal.list_of_nodes[index_origin_goal].distance_traveled < path_length): origin_exploitation = new_point_origin index_origin = len(tree_origin.list_of_points)-1 goal_exploitation = tree_goal.list_of_points[index_origin_goal] index_goal = index_origin_goal marker_branches_origin.points = [] for node_a in tree_origin.list_of_nodes: a = Point() a.x = node_a.data[0] a.y = node_a.data[1] for node_b in node_a.children: b = Point() b.x = node_b.data[0] b.y = node_b.data[1] marker_branches_origin.points.append(a) marker_branches_origin.points.append(b) tree_origin_pub.publish(marker_branches_origin) rand1=(random.uniform(float(wrapper.origin_x),float(wrapper.length_m/2)),random.uniform(float(wrapper.origin_y),float(wrapper.width_m/2))) distance1, index1 = spatial.KDTree(tree_goal.list_of_points).query(rand1) point_link_goal = tree_goal.list_of_points[index1] new_point_goal = join_rrt(point_link_goal, rand1) if new_point_goal != None: indices_of_neighboring_points_goal = spatial.KDTree(tree_goal.list_of_points).query_ball_point(new_point_goal, max_distance_neighbors) new_point_goal_distance_traveled_min = 1600.0 index1_min = index1 for index_neigboring_points_goal in indices_of_neighboring_points_goal: neighboring_node_goal = tree_goal.list_of_nodes[index_neigboring_points_goal] distance_neighbors_goal = euclidian_distance(new_point_goal, neighboring_node_goal.data) if join_prm(new_point_goal, neighboring_node_goal.data) is not None and neighboring_node_goal.distance_traveled + distance_neighbors_goal < new_point_goal_distance_traveled_min: index1_min = index_neigboring_points_goal new_point_goal_distance_traveled_min = neighboring_node_goal.distance_traveled + distance_neighbors_goal new_node_goal = Node(new_point_goal, tree_goal.list_of_nodes[index1_min]) tree_goal.dodaj(new_node_goal) nodes_goal.x = new_point_goal[0] nodes_goal.y = new_point_goal[1] marker_nodes_goal.points.append(nodes_goal) nodes_goal_pub.publish(marker_nodes_goal) for index_neigboring_points_goal in indices_of_neighboring_points_goal: neighboring_node_goal = tree_goal.list_of_nodes[index_neigboring_points_goal] distance_neighbors_goal = euclidian_distance(new_node_goal.data, neighboring_node_goal.data) if neighboring_node_goal.distance_traveled > new_node_goal.distance_traveled + distance_neighbors_goal and distance_neighbors_goal <= max_distance_neighbors: linking_point_goal = join_rrt(new_node_goal.data, neighboring_node_goal.data) if linking_point_goal is not None and euclidian_distance(linking_point_goal, neighboring_node_goal.data) <= 2 * epsilon: previous_parent_index = tree_goal.list_of_nodes.index(neighboring_node_goal.parent) neighboring_node_goal.change_parent(new_node_goal, previous_parent=tree_goal.list_of_nodes[previous_parent_index]) distance_goal_origin, index_goal_origin = spatial.KDTree(tree_origin.list_of_points).query(new_point_goal) if (distance_goal_origin + new_node_goal.distance_traveled + tree_origin.list_of_nodes[index_goal_origin].distance_traveled < path_length): goal_exploitation = new_point_goal index_goal = len(tree_goal.list_of_points)-1 origin_exploitation = tree_origin.list_of_points[index_goal_origin] index_origin = index_goal_origin marker_branches_goal.points = [] for node_a in tree_goal.list_of_nodes: a = Point() a.x = node_a.data[0] a.y = node_a.data[1] for node_b in node_a.children: b = Point() b.x = node_b.data[0] b.y = node_b.data[1] marker_branches_goal.points.append(a) marker_branches_goal.points.append(b) tree_goal_pub.publish(marker_branches_goal) exploration_duration = time.time() - start_time else: new_point_pk = join_prm(origin_exploitation, goal_exploitation) if new_point_pk != None: send_origin = [node.data for node in tree_origin.list_of_nodes[index_origin]] send_origin = send_origin[::-1] send_goal = [node.data for node in tree_goal.list_of_nodes[index_goal]] send = [] send.extend(send_origin) send.extend(send_goal) gotovo(send) path_length = euclidian_distance(origin_exploitation, goal_exploitation) + \ tree_origin.list_of_nodes[tree_origin.list_of_points.index(origin_exploitation)].distance_traveled + \ tree_goal.list_of_nodes[tree_goal.list_of_points.index(goal_exploitation)].distance_traveled #print "---" #print "END" #break print "-----" print "I: ", i print "CP:", len(tree_origin.list_of_nodes) print "CK:", len(tree_goal.list_of_nodes) print "D: ", path_length print "t: ", exploration_duration output_data.append([i, len(tree_origin.list_of_nodes), len(tree_goal.list_of_nodes), path_length, exploration_duration]) rate.sleep() write_data_to_file(output_data, output_file+"rrt_star_connect_"+str(instance)+".txt") instance += 1
def update_wc(self,msg): v = self.robot.GetActiveDOFValues() for name in self.joint_names: v[self.robot.GetJoint(name).GetDOFIndex()] = self.joint_angles[self.joint_names.index(name)] self.robot.SetActiveDOFValues(v) rospy.loginfo("I have got a wc location!") pos_temp = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z] ori_temp = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w] self.pr2_B_wc = createBMatrix(pos_temp, ori_temp) psm = PoseStamped() psm.header.stamp = rospy.Time.now() #self.goal_pose_pub.publish(psm) self.pub_feedback("Reaching toward goal location") wheelchair_location = self.pr2_B_wc * self.corner_B_head.I self.wheelchair.SetTransform(np.array(wheelchair_location)) pos_goal = wheelchair_location[:3,3] ori_goal = tr.matrix_to_quaternion(wheelchair_location[0:3,0:3]) marker = Marker() marker.header.frame_id = "base_link" marker.header.stamp = rospy.Time() marker.ns = "arm_reacher_wc_model" marker.id = 0 marker.type = Marker.MESH_RESOURCE; marker.action = Marker.ADD marker.pose.position.x = pos_goal[0] marker.pose.position.y = pos_goal[1] marker.pose.position.z = pos_goal[2] marker.pose.orientation.x = ori_goal[0] marker.pose.orientation.y = ori_goal[1] marker.pose.orientation.z = ori_goal[2] marker.pose.orientation.w = ori_goal[3] marker.scale.x = 0.0254 marker.scale.y = 0.0254 marker.scale.z = 0.0254 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 #only if using a MESH_RESOURCE marker type: marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae" self.vis_pub.publish( marker ) v = self.robot.GetActiveDOFValues() for name in self.joint_names: v[self.robot.GetJoint(name).GetDOFIndex()] = self.joint_angles[self.joint_names.index(name)] self.robot.SetActiveDOFValues(v) goal_angle = 0#m.pi/2 self.goal = np.matrix([[ m.cos(goal_angle), -m.sin(goal_angle), 0., .5], [ m.sin(goal_angle), m.cos(goal_angle), 0., 0], [ 0., 0., 1., 1.2], [ 0., 0., 0., 1.]]) #self.goal = copy.copy(self.pr2_B_wc) #self.goal[0,3]=self.goal[0,3]-.2 #self.goal[1,3]=self.goal[1,3]-.3 #self.goal[2,3]= 1.3 print 'goal is: \n', self.goal self.goal_B_gripper = np.matrix([[ 0., 0., 1., 0.1], [ 0., 1., 0., 0.], [ -1., 0., 0., 0.], [ 0., 0., 0., 1.]]) self.pr2_B_goal = self.goal*self.goal_B_gripper self.sol = self.manip.FindIKSolution(np.array(self.pr2_B_goal), op.IkFilterOptions.CheckEnvCollisions) if self.sol is None: self.pub_feedback("Failed to find a good arm configuration for reaching.") return None rospy.loginfo("[%s] Got an IK solution: %s" % (rospy.get_name(), self.sol)) self.pub_feedback("Found a good arm configuration for reaching.") self.pub_feedback("Looking for path for arm to goal.") traj = None try: #self.res = self.manipprob.MoveToHandPosition(matrices=[np.array(self.pr2_B_goal)],seedik=10) # call motion planner with goal joint angles self.traj=self.manipprob.MoveManipulator(goal=self.sol,outputtrajobj=True) self.pub_feedback("Found a path to reach to the goal.") except: self.traj = None self.pub_feedback("Could not find a path to reach to the goal") return None tmp_traj = tmp_vel = tmp_acc = [] #np.zeros([self.traj.GetNumWaypoints(),7]) trajectory = JointTrajectory() for i in xrange(self.traj.GetNumWaypoints()): point = JointTrajectoryPoint() temp = [] for j in xrange(7): temp.append(float(self.traj.GetWaypoint(i)[j])) point.positions = temp #point.positions.append(temp) #point.accelerations.append([0.,0.,0.,0.,0.,0.,0.]) #point.velocities.append([0.,0.,0.,0.,0.,0.,0.]) trajectory.points.append(point) #tmp_traj.append(temp) #tmp_traj.append(list(self.traj.GetWaypoint(i))) #tmp_vel.append([0.,0.,0.,0.,0.,0.,0.]) #tmp_acc.append([0.,0.,0.,0.,0.,0.,0.]) #print 'tmp_traj is: \n', tmp_traj #for j in xrange(7): #tmp_traj[i,j] = float(self.traj.GetWaypoint(i)[j]) #trajectory = JointTrajectory() #point = JointTrajectoryPoint() #point.positions.append(tmp_traj) #point.velocities.append(tmp_vel) #point.accelerations.append(tmp_acc) #point.velocities=[[0.,0.,0.,0.,0.,0.,0.]] #point.accelerations=[[0.,0.,0.,0.,0.,0.,0.]] trajectory.joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] #trajectory.points.append(point) #self.mpc_weights_pub.publish(self.weights) self.moving=True self.goal_traj_pub.publish(trajectory) self.pub_feedback("Reaching to location")
def node(): global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count rospy.init_node('filter', anonymous=False) # 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_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 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 frontiers = copy(centroids) # ------------------------------------------------------------------------- # 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 # ------------------------------------------------------------------------- # 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 visualization(): r = 0.05 markerArray = MarkerArray() for i in range(p.shape[1]): if i == current_pos_number: marker = Marker() marker.header.frame_id = "global_tank" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = r * 2 # r*2 of distance to camera from tag_14 marker.scale.y = r * 2 marker.scale.z = r * 2 marker.color.r = 1 marker.color.a = 1 # transparency marker.pose.orientation.w = 1.0 marker.pose.position.x = p[1, i] # x marker.pose.position.y = p[2, i] # y marker.pose.position.z = wanted_z_position # z markerArray.markers.append(marker) else: marker = Marker() marker.header.frame_id = "global_tank" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = r # r*2 of distance to camera from tag_14 marker.scale.y = r marker.scale.z = r marker.color.g = 1 marker.color.a = 1 # transparency marker.pose.orientation.w = 1.0 marker.pose.position.x = p[1, i] # x marker.pose.position.y = p[2, i] # y marker.pose.position.z = wanted_z_position # z markerArray.markers.append(marker) for i in range(len(current_path[0])): marker = Marker() marker.header.frame_id = "global_tank" marker.id = i + p.shape[1] marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = r # r*2 of distance to camera from tag_14 marker.scale.y = r marker.scale.z = r marker.color.g = 1 marker.color.a = 1 # transparency marker.pose.orientation.w = 1.0 marker.pose.position.x = current_path[0, i] # x marker.pose.position.y = current_path[1, i] # y marker.pose.position.z = wanted_z_position # z markerArray.markers.append(marker) marker = Marker() marker.header.frame_id = "global_tank" marker.id = 200 marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = r # r*2 of distance to camera from tag_14 marker.scale.y = r marker.scale.z = r marker.color.g = 1 marker.color.r = 1 marker.color.a = 1 # transparency marker.pose.orientation.w = 1.0 marker.pose.position.x = current_path[0, 10 / 2] # x marker.pose.position.y = current_path[1, 10 / 2] # y marker.pose.position.z = wanted_z_position # z markerArray.markers.append(marker) publisher_marker.publish(markerArray)
def publish_cpi_markers(self, collisions): """ Publishes a string for each ClosestPointInfo in the dict. If the distance is below the threshold, the string is colored red. If it is below threshold*2 it is yellow. If it is below threshold*3 it is green. Otherwise no string will be published. :type collisions: Collisions """ m = Marker() m.header.frame_id = self.get_god_map().get_data(identifier.map_frame) m.action = Marker.ADD m.type = Marker.LINE_LIST m.id = 1337 m.ns = self.name_space m.scale = Vector3(0.003, 0, 0) m.pose.orientation.w = 1 if len(collisions.items()) > 0: for collision in collisions.items(): # type: Collision red_threshold = 0.05 # TODO don't hardcode this yellow_threshold = red_threshold * 2 green_threshold = yellow_threshold * 2 contact_distance = collision.get_contact_distance() if contact_distance < green_threshold: m.points.append( Point(*collision.get_position_on_a_in_map())) m.points.append( Point(*collision.get_position_on_b_in_map())) m.colors.append(ColorRGBA(0, 1, 0, 1)) m.colors.append(ColorRGBA(0, 1, 0, 1)) if contact_distance < yellow_threshold: m.colors[-2] = ColorRGBA(1, 1, 0, 1) m.colors[-1] = ColorRGBA(1, 1, 0, 1) if contact_distance < red_threshold: m.colors[-2] = ColorRGBA(1, 0, 0, 1) m.colors[-1] = ColorRGBA(1, 0, 0, 1) ma = MarkerArray() ma.markers.append(m) self.pub_collision_marker.publish(ma)
def callback(data, list): pub, ekf, publisher_marker = list # print("Start of current picture") # print(ekf.get_x_est()) # print(ekf.get_p_mat()) # print(image.encoding) # brige = CvBridge() # try: # frame = brige.imgmsg_to_cv2(image, "passthrough") # # frame = brige.compressed_imgmsg_to_cv2(image, "passthrough") # except CvBridgeError as e: # print(e) # # print(frame.shape) # point_data_3d = np.zeros((frame.shape[0] * frame.shape[1], 3)) # print(point_data_3d.shape) # for i in range(point_data_3d.shape[0]): # # print(i) # # print(np.mod(i, 480)) # # print(i/640) # point_data_3d[i, 0] = np.mod(i, 480)/300.0 # point_data_3d[i, 1] = i/640/300.0 # point_data_3d[i, 2] = frame[np.mod(i, 480), i/640] # point_data_3d = np.float32(point_data_3d) # #print(point_data_3d[1000,:]) pc = ros_numpy.numpify(data) points = np.zeros((pc.shape[0], 3)) points[:, 0] = pc['x'] points[:, 1] = pc['y'] points[:, 2] = pc['z'] # print(points.shape) points = points[::10, :] # print(points.shape) points = np.float32(points) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0) K = 10 ret, label, center = cv2.kmeans(points, K, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS) # print(center) best_match = np.argmin(abs(center[:, 2] - 1)) # print(best_match) A = points[label.ravel() == best_match] # B = points[label.ravel() == 1] # C = points[label.ravel() == 2] # print(A.shape) A = A[::10, :] # print(A.shape) points = [] lim = 8 # publish point cloud for i in range(A.shape[0]): x = A[i, 2] y = -A[i, 0] z = -A[i, 1] r = 255 g = 0 b = 0 a = 150 rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] pt = [x, y, z, rgb] points.append(pt) fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), # PointField('rgb', 12, PointField.UINT32, 1), PointField('rgba', 12, PointField.UINT32, 1), ] # print points header = Header() header.frame_id = "d435i_link" pc2 = point_cloud2.create_cloud(header, fields, points) pub.publish(pc2) ekf.prediction() print("EKF Update:") ekf.update(A) current_state_ekf = ekf.get_x_est() print("x_est", current_state_ekf) # calculat 3 vectors for ebene representation such that p=s_v+mu*r_v_1+theta*r_v_1 s_v = np.asarray([[0, 0, current_state_ekf[3] / current_state_ekf[2]]], dtype="float32") r_v_1 = np.asarray([[ 0.5, 0, (current_state_ekf[3] - 0.5 * current_state_ekf[0]) / current_state_ekf[2] ]], dtype="float32") r_v_2 = np.cross(r_v_1, np.transpose(current_state_ekf[0:3])) r_v_2 = np.asarray(r_v_2, dtype="float32") r_v_2 = normalize_vector(r_v_2) r_v_1 = normalize_vector(r_v_1) print("s_v", s_v) print("r_v_1", r_v_1) print("r_v_2", r_v_2) # print(ekf.get_z_est(0,-0.5)) # print(ekf.get_z_est(0,0)) # print(ekf.get_z_est(0,0.5)) # print(ekf.get_x_est()) # a = ekf.get_x_est()[0] # b = ekf.get_x_est()[1] # c = ekf.get_x_est()[2] # distance_to_net = PoseStamped() # distance_to_net.header.stamp = rospy.Time.now() # distance_to_net.header.frame_id = "boat_to_net" # ned # distance_to_net.pose.position.x = a # distance_to_net.pose.position.y = b # distance_to_net.pose.position.z = c # # publisher_distance_net.publish(distance_to_net) # rviz = True if rviz: markerArray = MarkerArray() i = 1 for mu in np.linspace(-1, 1, 10): for theta in np.linspace(-1, 1, 10): current_point = np.transpose(s_v) + mu * np.transpose( r_v_1) + theta * np.transpose(r_v_2) r = 0.02 marker = Marker() marker.header.frame_id = "d435i_link" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = r * 2 # r*2 marker.scale.y = r * 2 marker.scale.z = r * 2 marker.color.r = 1 marker.color.g = 1 marker.color.a = 1 # transparency marker.pose.orientation.w = 1.0 marker.pose.position.x = current_point[2] # x marker.pose.position.y = -current_point[0] # y marker.pose.position.z = -current_point[1] # z markerArray.markers.append(marker) i = i + 1 publisher_marker.publish(markerArray)
def publish(self, tracks, header): markers_msg = MarkerArray() for track in tracks: if track.is_confirmed(): if track.has_shape() and track.is_located(): for shape_idx, shape in enumerate(track.shapes): marker = Marker() marker_name = track.id + ":" + str(shape_idx) if marker_name not in self.marker_id_map: self.marker_id_map[ marker_name] = self.last_marker_id self.last_marker_id += 1 marker_id = self.marker_id_map[marker_name] marker.id = marker_id marker.ns = track.id marker.action = Marker.MODIFY marker.header = header node_pose = track.pose shape_pose = shape.pose final_pose = node_pose + shape_pose position = final_pose.position() orientation = final_pose.quaternion() marker.pose.position.x = position.x marker.pose.position.y = position.y marker.pose.position.z = position.z 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.color.r = shape.color[0] marker.color.g = shape.color[1] marker.color.b = shape.color[2] if track.label in ["person", "face"]: marker.color.a = 0.2 else: marker.color.a = shape.color[3] if track.is_static() is True: marker.color.a = 1.0 if shape.is_cylinder(): marker.type = Marker.CYLINDER marker.scale = Vector3(x=shape.width(), y=shape.width(), z=shape.height()) elif shape.is_sphere(): marker.type = Marker.SPHERE marker.scale = Vector3(x=shape.width(), y=shape.width(), z=shape.width()) elif shape.is_box(): marker.type = Marker.CUBE marker.scale = Vector3(x=shape.x, y=shape.y, z=shape.z) elif shape.is_mesh(): if shape.mesh_resource != "": marker.type = Marker.MESH_RESOURCE marker.mesh_resource = shape.mesh_resource marker.mesh_use_embedded_materials = True else: marker.type = Marker.TRIANGLE_LIST marker.points = shape.vertices marker.scale = Vector3(x=shape.scale.x, y=shape.scale.y, z=shape.scale.z) else: raise NotImplementedError("Shape not implemented") marker.lifetime = rospy.Duration(3.0) markers_msg.markers.append(marker) self.publisher.publish(markers_msg)
def visualize_field(x_vec_arr, y_vec_arr, ranges, theta_arr): """ This function displays the vector field into a pose array that can be viewed in RVIZ. This should only be used for debugging purposes as it can be costly to run RVIZ while the kart is actually running. """ # Create a publisher for the rostopic 'vector_field' # this will publish the marker array messages publisher = rospy.Publisher('vector_field', MarkerArray) vec_field = MarkerArray() # Variable used to ID each marker in the array i = 0 # Iterate through the following arrays and add markers to the marker array # message to be sent for r, theta, x_or, y_or in np.nditer( [ranges, theta_arr, x_vec_arr, y_vec_arr]): # Initialize a marker message _marker = Marker() # Populate the pose field of the marker message _marker.pose.position.x = r * math.cos(theta + math.pi) _marker.pose.position.y = r * math.sin(theta + math.pi) _marker.pose.position.z = 0 _marker.pose.orientation = Quaternion( *(quaternion_from_euler(0, 0, math.atan2(y_or, x_or) + math.pi, 'sxyz'))) # Populate the scale field of the marker message _marker.scale.x = SCALE_FACTOR * x_or _marker.scale.y = SCALE_FACTOR * y_or _marker.scale.z = 0.0001 # Populate the color field of the marker message -> teal _marker.color.r = 0 _marker.color.g = 128 _marker.color.b = 128 _marker.color.a = 255 # Populate the header of the marker message _marker.header.stamp.secs = rospy.Time.now().secs _marker.header.stamp.nsecs = rospy.Time.now().nsecs _marker.header.frame_id = "base_link" _marker.header.seq = i _marker.ns = "cur_marker" _marker.id = i i += 1 _marker.type = 0 _marker.lifetime.secs = UPDATE_RATE _marker.frame_locked = False # Append the marker message to the list of markers vec_field.markers.append(_marker) # Publish the marker array publisher.publish(vec_field)
def node(): rospy.init_node('rrt_frontier_detector', anonymous=False) # fetching all parameters eta = rospy.get_param('~eta', 0.7) init_map_x = rospy.get_param('~init_map_x', 20.0) init_map_y = rospy.get_param('~init_map_y', 20.0) ns = rospy.get_namespace() map_topic = rospy.get_param('~map_topic', ns + 'map') base_frame_topic = rospy.get_param('~base_frame_topic', 'base_link') #------------------------------------------- global mapData exploration_goal = Point() point = Point() rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) targetspub = rospy.Publisher('/exploration_goals', Point, queue_size=10) pub = rospy.Publisher('shapes', Marker, queue_size=10) pointspub = rospy.Publisher('/points', Point, queue_size=10) rate = rospy.Rate(100) # wait until map is received, when a map is received, mapData.header.seq will not be < 1 while mapData.header.seq < 1 or len(mapData.data) < 1: pass listener = tf.TransformListener() listener.waitForTransform(mapData.header.frame_id, ns + base_frame_topic, rospy.Time(0), rospy.Duration(10.0)) try: (trans, rot) = listener.lookupTransform(mapData.header.frame_id, ns + base_frame_topic, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): trans = [0, 0] xinx = trans[0] xiny = trans[1] x_init = array([xinx, xiny]) V = array([x_init]) i = 1.0 E = concatenate((x_init, x_init)) 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 = mapData.header.frame_id points.header.stamp = line.header.stamp = rospy.Time.now() points.ns = line.ns = "markers" points.id = 0 line.id = 1 points.type = Marker.POINTS line.type = Marker.LINE_LIST #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 = line.scale.y = 0.06 points.scale.x = points.scale.y = 0.3 points.scale.y = 0.3 line.color.r = 0.0 #9.0/255.0 line.color.g = 0.0 #91.0/255.0 line.color.b = 0.0 #236.0/255.0 points.color.r = 255.0 / 255.0 points.color.g = 0.0 / 255.0 points.color.b = 0.0 / 255.0 points.color.a = 1 line.color.a = 1 #0.6; points.lifetime = line.lifetime = rospy.Duration() p = Point() p.x = x_init[0] p.y = x_init[0] p.z = 0 pp = [] pl = [] pp.append(copy(p)) frontiers = [] xdim = mapData.info.width ydim = mapData.info.height resolution = mapData.info.resolution Xstartx = mapData.info.origin.position.x Xstarty = mapData.info.origin.position.y #-------------------------------RRT------------------------------------------ while not rospy.is_shutdown(): # Sample free xr = (random() * init_map_x) - (init_map_x * 0.5) yr = (random() * init_map_y) - (init_map_y * 0.5) x_rand = array([xr, yr]) # Nearest x_nearest = V[Nearest(V, x_rand), :] # Steer x_new = Steer(x_nearest, x_rand, eta) # ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle checking = ObstacleFree2(x_nearest, x_new, mapData) if checking == -1: exploration_goal.x = x_new[0] exploration_goal.y = x_new[1] exploration_goal.z = 0.0 targetspub.publish(exploration_goal) (trans, rot) = listener.lookupTransform(mapData.header.frame_id, ns + base_frame_topic, rospy.Time(0)) xinx = trans[0] xiny = trans[1] x_init = array([xinx, xiny]) V = array([x_init]) E = concatenate((x_init, x_init)) pp = [] pl = [] elif checking == 1: V = vstack((V, x_new)) point.x = x_new[0] point.y = x_new[1] pointspub.publish(point) temp = concatenate((x_nearest, x_new)) E = vstack((E, temp)) #Plotting points.points = [exploration_goal] pl = prepEdges(E) line.points = pl pub.publish(line) pub.publish(points) raw_input("") rate.sleep()
def make_individual_markers(self, msg): lpost = Marker() lpost.type = Marker.CYLINDER lpost.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_HEIGHT) lpost.color.r = 1.0 lpost.color.g = 1.0 lpost.color.b = 1.0 lpost.color.a = 0.4 lpost.pose.position = Point(0, GOAL_WIDTH / 2, GOAL_HEIGHT / 2) rpost = Marker() rpost.type = Marker.CYLINDER rpost.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_HEIGHT) rpost.color.r = 1.0 rpost.color.g = 1.0 rpost.color.b = 1.0 rpost.color.a = 0.4 rpost.pose.position = Point(0, -GOAL_WIDTH / 2, GOAL_HEIGHT / 2) bar = Marker() bar.type = Marker.CYLINDER bar.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_WIDTH) bar.color.r = 1.0 bar.color.g = 1.0 bar.color.b = 1.0 bar.color.a = 0.4 bar.pose.position = Point(0, 0, GOAL_HEIGHT) bar.pose.orientation = Quaternion( math.sqrt(2) / 2, 0, 0, math.sqrt(2) / 2) return (lpost, rpost, bar)
def sync_callback(msg1, msg2): # msg1: /image_raw # msg2: /velodyne_points: velodyne_points func_start = time.time() timestamp1 = msg1.header.stamp.to_nsec() print('image_callback: msg : seq=%d, timestamp=%19d' % (msg1.header.seq, timestamp1)) timestamp2 = msg2.header.stamp.to_nsec() print('velodyne_callback: msg : seq=%d, timestamp=%19d' % (msg2.header.seq, timestamp2)) arr = msg_to_arr(msg2) lidar = np.array([[item[0], item[1], item[2], item[3]] for item in arr]) camera_image = bridge.imgmsg_to_cv2(msg1, "bgr8") print("camera_image is {}".format(camera_image.shape)) top_view = point_cloud_2_top(lidar, res=0.2, zres=0.5, side_range=(-45, 45), fwd_range=(-45, 45), height_range=(-3, 0.5)) top_image = draw_top_image(top_view[:, :, -1]) if 0: # if show the images cemara_show_image = cv2.resize( camera_image, (camera_image.shape[1] // 2, camera_image.shape[0] // 2)) top_show_image_width = camera_image.shape[0] // 2 top_show_image = cv2.resize( top_image, (top_show_image_width, top_show_image_width)) show_image = np.concatenate((top_show_image, cemara_show_image), axis=1) cv2.imshow("top", show_image) cv2.waitKey(1) # use test data until round2 pipeline is ok np_reshape = lambda np_array: np_array.reshape(1, *(np_array.shape)) top_view = np_reshape(top) front_view = np_reshape(front) rgb_view = np_reshape(rgb) np.save(os.path.join(sys.path[0], "../MV3D/data/", "top.npy"), top_view) np.save(os.path.join(sys.path[0], "../MV3D/data/", "rgb.npy"), rgb_view) start = time.time() boxes3d = rpc.predict() end = time.time() print("predict boxes len={} use predict time: {} seconds.".format( len(boxes3d), end - start)) if len(boxes3d) > 0: translation, size, rotation = boxes3d_decompose(np.array(boxes3d)) # publish (boxes3d) to tracker_node markerArray = MarkerArray() for i in range(len(boxes3d)): m = Marker() m.type = Marker.CUBE m.header.frame_id = "velodyne" m.header.stamp = msg2.header.stamp m.scale.x, m.scale.y, m.scale.z = size[i][0], size[i][1], size[i][ 2] m.pose.position.x, m.pose.position.y, m.pose.position.z = \ translation[i][0], translation[i][1], translation[i][2] m.pose.orientation.x, m.pose.orientation.y, m.pose.orientation.z, m.pose.orientation.w = \ rotation[i][0], rotation[i][1], rotation[i][2], 0. m.color.a, m.color.r, m.color.g, m.color.b = \ 1.0, 0.0, 1.0, 0.0 markerArray.markers.append(m) pub.publish(markerArray) func_end = time.time() print("sync_callback use {} seconds".format(func_end - func_start))
def add_grasp_marker(self, frame_id, radius, height, init_position): # Descripcion basica int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id int_marker.pose.position = init_position int_marker.scale = max(radius * 2, height) + 0.02 int_marker.name = "grasp_marker" int_marker.description = "Graspable object" # Geometria marker = Marker() marker.type = Marker.CYLINDER marker.scale.x = radius * 2 marker.scale.y = radius * 2 marker.scale.z = height marker.color.r = random() marker.color.g = random() marker.color.b = random() marker.color.a = 1.0 # Control 6DOF control = InteractiveMarkerControl() control.always_visible = True control.markers.append(marker) int_marker.controls.append(control) int_marker.controls[ 0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D # Control roll control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "roll" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # Movimiento en X control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) # Control yaw control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "yaw" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # Movimiento en Z control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) # Control pitch control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "pitch" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # Movimiento en y control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) self.server.insert(int_marker, self.process_feedback) self.menu_handler.apply(self.server, int_marker.name)
path_len = 0 # with open(ROS_HOME + "/straight.txt") as f: with open(ROS_HOME + "/paths/t/surface.txt") as f: for line in f.readlines(): x = float(line.strip().split()[0]) y = float(line.strip().split()[1]) path_x.append(x) path_y.append(y) path_len += 1 #print(x, y) rospy.sleep(1) while path_len > count: marker = Marker() marker.header.frame_id = "/base_link" marker.type = marker.SPHERE 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 = 1.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = path_x[count] marker.pose.position.y = path_y[count]
def __init__(self): rospy.init_node("show_robocup_objects") # todo make dyn reconf able # todo add line markers self.marker_publisher = rospy.Publisher("/robocup_markers", Marker, queue_size=10) # object properties self.ball_diameter = 0.13 self.ball_lifetime = int(5 * (10**9)) self.post_diameter = 0.10 self.post_height = 1.10 self.goal_lifetime = int(5 * (10**9)) self.obstacle_height = 1.0 self.obstacle_lifetime = int(5 * (10**9)) self.obstacle_def_width = 0.3 # --- initilize message objects --- # Most of the message information stay the same. It is more performant to set them just one time # ball self.marker_ball_rel = Marker() # type: Marker self.marker_ball_rel.ns = "rel_ball" self.marker_ball_rel.id = 0 self.marker_ball_rel.type = Marker.SPHERE self.marker_ball_rel.action = Marker.MODIFY self.ball_pose = Pose() scale = Vector3(self.ball_diameter, self.ball_diameter, self.ball_diameter) self.marker_ball_rel.scale = scale self.ball_color = ColorRGBA() self.ball_color.r = 1.0 self.ball_color.a = 1.0 self.marker_ball_rel.color = self.ball_color self.marker_ball_rel.lifetime = rospy.Duration( nsecs=self.ball_lifetime) # goal # -post 1 self.marker_goal_rel1 = Marker() # type:Marker self.marker_goal_rel1.ns = "rel_goal" self.marker_goal_rel1.id = 0 self.marker_goal_rel1.type = Marker.CYLINDER self.marker_goal_rel1.action = Marker.MODIFY self.goal_post1_pose = Pose() scale = Vector3(self.post_diameter, self.post_diameter, self.post_height) self.marker_goal_rel1.scale = scale self.post1_color = ColorRGBA() self.post1_color.r = 1.0 self.post1_color.g = 1.0 self.post1_color.b = 1.0 self.post1_color.a = 1.0 self.marker_goal_rel1.color = self.post1_color self.marker_goal_rel1.lifetime = rospy.Duration( nsecs=self.goal_lifetime) # -post 2 self.marker_goal_rel2 = Marker() # type:Marker self.marker_goal_rel2.ns = "rel_goal" self.marker_goal_rel2.id = 1 self.marker_goal_rel2.type = Marker.CYLINDER self.marker_goal_rel2.action = Marker.MODIFY self.goal_post2_pose = Pose() scale = Vector3(self.post_diameter, self.post_diameter, self.post_height) self.marker_goal_rel2.scale = scale self.post2_color = ColorRGBA() self.post2_color.r = 1.0 self.post2_color.g = 1.0 self.post2_color.b = 1.0 self.post2_color.a = 1.0 self.marker_goal_rel2.color = self.post2_color self.marker_goal_rel2.lifetime = rospy.Duration( nsecs=self.goal_lifetime) # obstacle self.marker_obstacle = Marker() self.marker_obstacle.lifetime = rospy.Duration( nsecs=self.obstacle_lifetime) self.marker_obstacle.ns = "rel_obstacle" self.obstacle_color = ColorRGBA() self.obstacle_pose = Pose() self.marker_obstacle.type = Marker.CUBE # todo also display data from world model rospy.Subscriber("/ball_relative", BallRelative, self.ball_cb, queue_size=10) rospy.Subscriber("/goal_relative", GoalRelative, self.goal_cb, queue_size=10) rospy.Subscriber("/obstacles_relative", ObstaclesRelative, self.obstacle_cb, queue_size=10) # we do everything in the callbacks rospy.spin()
def visualize_groups_and_obstacles(self): marker_list = [] dummy_id = 0 # Groups for grp in self.groups: for pt in grp.points: marker = Marker() marker.id = dummy_id marker.header = self.header marker.type = Marker.SPHERE marker.action = 0 # 0 add/modify an object, 1 (deprecated), 2 deletes an object, 3 deletes all objects marker.color.a = 0.5 marker.color.r = 0.0 marker.color.g = 0.9 marker.color.b = 0.0 marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 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.pose.position.x = pt.x marker.pose.position.y = pt.y marker.pose.position.z = 0.0 marker_list.append(marker) if dummy_id not in self._all_marker_IDs: self._all_marker_IDs.append(dummy_id) dummy_id += 1 # Obstacles represented by lines for grp in self.obstacles_lines: marker = Marker() marker.id = dummy_id marker.header = self.header marker.type = Marker.LINE_STRIP marker.action = 0 # 0 add/modify an object, 1 (deprecated), 2 deletes an object, 3 deletes all objects marker.color.a = 0.8 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.scale.x = 0.05 marker.points = grp.best_fit_line.endpoints marker.points[0].z, marker.points[ 1].z = 0.035, 0.035 # based on diameter of sphere markers 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_list.append(marker) if dummy_id not in self._all_marker_IDs: self._all_marker_IDs.append(dummy_id) dummy_id += 1 # Obstacles represented by circles for grp in self.obstacles_circles: marker = Marker() marker.id = dummy_id marker.header = self.header marker.type = Marker.CYLINDER marker.action = 0 # 0 add/modify an object, 1 (deprecated), 2 deletes an object, 3 deletes all objects marker.color.a = 0.8 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.scale.x, marker.scale.y = float( grp.best_fit_circle.radius * 2), float( grp.best_fit_circle.radius * 2) # set different xy values for ellipse marker.scale.z = 0.1 marker.pose.position.x = grp.best_fit_circle.center.x marker.pose.position.y = grp.best_fit_circle.center.y marker.pose.position.z = marker.scale.z / 2 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_list.append(marker) if dummy_id not in self._all_marker_IDs: self._all_marker_IDs.append(dummy_id) dummy_id += 1 # delete detections from previous message that are not present in current message for id_to_del in self._all_marker_IDs[dummy_id:]: marker = Marker() marker.id = id_to_del marker.action = Marker.DELETE marker_list.append(marker) self._all_marker_IDs = self._all_marker_IDs[:dummy_id] # publish marker_array = MarkerArray() marker_array.markers = marker_list return marker_array
def callback(self): #continue listening to the publisher #create publisher object with topic name, message type, and buffer size pub_obj = rospy.Publisher('cmd_vel', Twist, queue_size=1) #odom_data = rospy.Subscriber('odom',Odometry,callback2) pub_obj2 = rospy.Publisher('/myvalues', numpy_msg(Floats), queue_size=1) #initialize the publisher object with its name and anonymous type #rospy.init_node('robot_talker',anonymous=True) #set the frequency in which to publish rate = rospy.Rate(10) #point_coordinates = Pose() point_coordinates_list = [] point_coordinates = [] #create the twist object twist_obj = Twist() odometry_obj = Odometry() laserscan_data = LaserScan() while not rospy.is_shutdown(): range_values = [] range_values = self.ranges #stopflag = 0 twist_obj1 = Twist() stop = 1 direction = 0 if len(self.ranges) > 0: for i in range(0, 359): laserscan_data = self value_x = self.ranges[i] * math.cos(self.angle_min + self.angle_increment) value_y = self.ranges[i] * math.sin(self.angle_min + self.angle_increment) point_coordinates.append(value_x) point_coordinates.append(value_y) coordinates_numpy = numpy.array(point_coordinates, dtype=numpy.float32) pub_obj2.publish(coordinates_numpy) #randomly select 2 points from the array #do the following for k iterations k = 0 #while (k<5): while (True): random_point = random.randint(0, len(point_coordinates) / 2) if (random_point % 2 == 0): #it is an x coordinate x_coordinate1 = point_coordinates[random_point] y_coordinate1 = point_coordinates[random_point + 1] else: #it is a y coordinate y_coordinate1 = point_coordinates[random_point] x_coordinate1 = point_coordinates[random_point - 1] random_point2 = random.randint(0, len(point_coordinates) / 2) if (random_point2 % 2 == 0): #it is an x coordinate x_coordinate2 = point_coordinates[random_point] y_coordinate2 = point_coordinates[random_point + 1] else: #it is a y coordinate y_coordinate2 = point_coordinates[random_point] x_coordinate2 = point_coordinates[random_point - 1] if (x_coordinate2 - x_coordinate1) <> 0: break #line is represented by ax+by+c=0 c1 = (y_coordinate2 - y_coordinate1) / (x_coordinate2 - x_coordinate1) a1 = c1 b = -1 counter = 0 i1 = 0 while (counter < len(point_coordinates)): i1 = counter numerator = (c1 * point_coordinates[counter]) + ( -1 * point_coordinates[counter + 1]) + (y_coordinate1 - c1 * x_coordinate1) #numerator_sq = numerator * numerator numerator = abs(numerator) denom = math.sqrt((c1 * c1) + 1) distance = numerator / denom if (distance < 0.1): #adding the selected point to the list of inliers inlier.append(point_coordinates[counter]) inlier.append(point_coordinates[counter + 1]) else: outlier.append(point_coordinates[counter]) outlier.append(point_coordinates[counter + 1]) counter += 2 #o = numpy.array(outlier, dtype=numpy.float32) i = numpy.array(inlier, dtype=numpy.float32) #pub_obj2.publish(i) counter2 = 0 line_marker_array = MarkerArray() publisher_obj3 = rospy.Publisher('lines', MarkerArray, queue_size=1) while (counter2 < len(inlier)): """ box_marker=Marker() box_marker.header.frame_id = "base_link" box_marker.action = Marker.ADD box_marker.type = Marker.CUBE box_marker.header.stamp = rospy.Time.now() box_marker.scale.x = 0.45 box_marker.scale.y = 0.45 box_marker.scale.z = 0.45 box_marker.color.r = 0.0 box_marker.color.g = 0.5 box_marker.color.b = 0.5 box_marker.color.a = 1.0 box_marker.lifetime = 0 break line_marker.action = Marker.ADD line_marker.pose.orientation.w = 1 """ line_marker = Marker() id1 = 0 line_marker.header.frame_id = "base_link" line_marker.type = Marker.LINE_STRIP line_marker.header.stamp = rospy.Time.now() line_marker.color.r = 0 line_marker.color.g = 1 line_marker.color.a = 1 line_marker.scale.x = 1 line_marker.scale.y = 1 line_marker.scale.z = 1 line_marker.pose.position.x = inlier[counter2] line_marker.pose.position.y = inlier[counter2 + 1] line_marker.pose.position.z = 1 line_marker.lifetime = rospy.Duration(0) line_marker_array.markers.append(line_marker) line_marker.id = id1 id1 += 1 counter += 2 publisher_obj3.publish(line_marker_array) return
def scan_callback(self, scan_msg): print('-----------------------------------------') start_time = time.time() # process scan message pose = self.pose.copy() bearings = self.bearings.copy() ranges = np.array(scan_msg.ranges) inf_flag = (-1 * np.isinf(ranges).astype(int) + 1) ranges = np.nan_to_num(ranges) * inf_flag euc_coord_x = pose[0] + np.cos(bearings + pose[2]) * ranges euc_coord_y = pose[1] + np.sin(bearings + pose[2]) * ranges dist_flag = np.where( (euc_coord_x-pose[0])**2 + \ (euc_coord_y-pose[1])**2 != 0.0)[0] points = np.array([euc_coord_x, euc_coord_y]).T points = points[dist_flag] self.obsv = [] if len(points) > 0: brc = Birch(n_clusters=None, threshold=0.05) brc.fit(points) labels = brc.predict(points) u_labels = np.unique(labels) for l in u_labels: seg_idx = np.where(labels==l) seg = points[seg_idx] if seg.shape[0] <= 1: fit_cov = 10 else: fit_cov = np.trace(np.cov(seg.T)) if fit_cov < 0.001 and seg.shape[0]>=4: lm = seg.mean(axis=0) self.obsv.append(lm.copy()) print('odom: {}\nlandmarks:\n{}'.format(pose, self.obsv)) # publish observed landmarks cube_list = Marker() cube_list.header.frame_id = 'odom' cube_list.header.stamp = rospy.Time.now() cube_list.ns = 'landmark_point' cube_list.action = Marker.ADD cube_list.pose.orientation.w = 1.0 cube_list.id = 0 cube_list.type = Marker.CUBE_LIST cube_list.scale.x = 0.05 cube_list.scale.y = 0.05 cube_list.scale.z = 0.5 cube_list.color.b = 1.0 cube_list.color.a = 1.0 for landmark in self.obsv: p = Point() p.x = landmark[0] p.y = landmark[1] p.z = 0.25 cube_list.points.append(p) self.obsv_pub.publish(cube_list) print('elasped time: {}'.format(time.time()-start_time))
def polyCb(self,data): if self.polydone == True: for i in range(self.count_id-1): marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time.now() marker.ns = "points_and_lines" marker.id = i if i == 0: marker.type = marker.LINE_STRIP else: marker.type = marker.SPHERE marker.action = marker.DELETE self.line_pub.publish(marker) self.polydone = False self.poly = [] self.polynum = 0 self.polypath = [] self.polynum = 0 self.count_id = 1 x = data.point.x y = data.point.y rospy.loginfo([x,y]) marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time.now() marker.ns = "points_and_lines" marker.id = self.count_id marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.position.x = x marker.pose.position.y = y marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.g = 1.0 marker.color.a = 1.0 self.line_pub.publish(marker) self.count_id += 1 self.polynum += 1 if self.polynum > 3: closepnt = self.poly[0] if calc_distance([x,y],closepnt) < 1: rospy.loginfo('Boundary done') self.polypath = mpltPath.Path(np.array(self.poly)) marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time.now() marker.ns = "points_and_lines" marker.id = self.count_id - 1 marker.type = marker.SPHERE marker.action = marker.DELETE self.line_pub.publish(marker) self.polydone = True self.send = True self.sendGoal(self.currentpos) if self.polydone == False: self.poly.append([x,y]) if len(self.poly) > 1: newMarker = Marker() newMarker.header.frame_id = "/map" newMarker.header.stamp = rospy.Time.now() newMarker.ns = "points_and_lines" newMarker.id = 0 newMarker.type = newMarker.LINE_STRIP newMarker.action = newMarker.ADD newMarker.pose.orientation.w = 1 newMarker.scale.x = 0.05 newMarker.color.b = 1.0 newMarker.color.a = 1.0 for p in self.poly: pnt = Point() pnt.x = p[0] pnt.y = p[1] newMarker.points.append(pnt) if self.polydone == True: pnt = Point() pnt.x = self.poly[0][0] pnt.y = self.poly[0][1] newMarker.points.append(pnt) newMarker.color.b = 0 newMarker.color.r = 1.0 self.line_pub.publish(newMarker)
def ctrl_callback(self, ctrl_flag_msg): idx = self.log['count'] % 10 pose = self.pose.copy() self.old_obsv = np.array(self.curr_obsv, copy=True) self.curr_obsv = np.array(self.obsv, copy=True) print('flag test: ', np.array_equal(self.old_obsv, self.curr_obsv)) print(self.obsv) self.erg_ctrl.barr.update_obstacles(self.obsv) _, ctrl_seq = self.erg_ctrl(pose.copy(), seq=True) if idx == 0: self.ctrl_seq = ctrl_seq.copy() ctrl = self.ctrl_seq[idx] ctrl_lin = ctrl[0] ctrl_ang = ctrl[1] vel_msg = Twist() vel_msg.linear.x = ctrl_lin vel_msg.linear.y = 0.0 vel_msg.linear.z = 0.0 vel_msg.angular.x = 0.0 vel_msg.angular.y = 0.0 vel_msg.angular.z = ctrl_ang self.ctrl_pub.publish(vel_msg) else: ctrl = self.ctrl_seq[idx] ctrl_lin = ctrl[0] ctrl_ang = ctrl[1] vel_msg = Twist() vel_msg.linear.x = ctrl_lin vel_msg.linear.y = 0.0 vel_msg.linear.z = 0.0 vel_msg.angular.x = 0.0 vel_msg.angular.y = 0.0 vel_msg.angular.z = ctrl_ang self.ctrl_pub.publish(vel_msg) # log self.log['count'] += 1 self.log['traj'].append(pose.copy()) self.log['ctrls'].append(ctrl.copy()) # publish predicted trajectory self.path_msg = Path() self.path_msg.header = copy(self.odom_header) dummy_pose = pose.copy() for i in range(idx, 80): dummy_ctrl = self.ctrl_seq[i] dummy_pose += 0.1 * np.array([cos(dummy_pose[2])*dummy_ctrl[0], sin(dummy_pose[2])*dummy_ctrl[0], dummy_ctrl[1]]) pose_msg = PoseStamped() pose_msg.header = copy(self.odom_header) pose_msg.pose.position.x = dummy_pose[0] pose_msg.pose.position.y = dummy_pose[1] self.path_msg.poses.append(copy(pose_msg)) self.path_pub.publish(self.path_msg) # landmarks table test for olm in np.array(self.obsv): tid = 0 tflag = 1 for tlm in self.lm_table: if np.array_equal(olm, tlm): print('exactly the same landmark') tflag = 0 elif np.sum((olm-tlm)**2) < 0.01: self.lm_table[tid] = olm tflag = 0 else: pass tid += 1 if tflag == 1: self.lm_table.append(np.array(olm)) cube_list = Marker() cube_list.header.frame_id = 'odom' cube_list.header.stamp = rospy.Time.now() cube_list.ns = 'landmark_map' cube_list.action = Marker.ADD cube_list.pose.orientation.w = 1.0 cube_list.id = 0 cube_list.type = Marker.CUBE_LIST cube_list.scale.x = 0.05 cube_list.scale.y = 0.05 cube_list.scale.z = 0.5 cube_list.color.r = 1.0 cube_list.color.g = 1.0 cube_list.color.a = 1.0 for landmark in self.lm_table: p = Point() p.x = landmark[0] p.y = landmark[1] p.z = 0.25 cube_list.points.append(p) self.map_pub.publish(cube_list)
def init_marker(index, x, y, symbole): marker_object = Marker() marker_object.header.frame_id = "/map" marker_object.header.stamp = rospy.get_rostime() marker_object.ns = "haro" marker_object.id = index marker_object.type = Marker.SPHERE marker_object.action = Marker.ADD #position my_point = Point() my_point.x = x my_point.y = y my_point.z = 0 #orientation marker_object.pose.position = my_point marker_object.pose.orientation.x = 0 marker_object.pose.orientation.y = 0 marker_object.pose.orientation.z = 0.0 marker_object.pose.orientation.w = 1.0 #size marker_object.scale.x = 0.2 marker_object.scale.y = 0.2 marker_object.scale.z = 0.2 if symbole == 'Biohazard': marker_object.color.r = 1.0 marker_object.color.g = 1.0 marker_object.color.b = 0.0 # This has to be, otherwise it will be transparent marker_object.color.a = 1.0 if symbole == 'Danger': marker_object.color.r = 1.0 marker_object.color.g = 1.0 marker_object.color.b = 1.0 # This has to be, otherwise it will be transparent marker_object.color.a = 1.0 if symbole == 'Fire': marker_object.color.r = 1.0 marker_object.color.g = 0.5 marker_object.color.b = 0.0 # This has to be, otherwise it will be transparent marker_object.color.a = 1.0 if symbole == 'Radioactive': marker_object.color.r = 204 / 255 marker_object.color.g = 255 / 255 marker_object.color.b = 153 / 255 # This has to be, otherwise it will be transparent marker_object.color.a = 1.0 if symbole == 'no smoking': marker_object.color.r = 153 / 255 marker_object.color.g = 76 / 255 marker_object.color.b = 0 / 255 # This has to be, otherwise it will be transparent marker_object.color.a = 1.0 if symbole == 'toxic': marker_object.color.r = 153 / 255 marker_object.color.g = 51 / 255 marker_object.color.b = 255 / 255 # This has to be, otherwise it will be transparent marker_object.color.a = 1.0 if symbole == 'alive_worker': marker_object.color.r = 0.0 marker_object.color.g = 1.0 marker_object.color.b = 0.0 # This has to be, otherwise it will be transparent marker_object.color.a = 1.0 if symbole == 'dead_worker': marker_object.color.r = 1.0 marker_object.color.g = 0.0 marker_object.color.b = 0.0 # This has to be, otherwise it will be transparent marker_object.color.a = 1.0 # If we want it for ever, 0, otherwise seconds before desapearing marker_object.lifetime = rospy.Duration(0) return marker_object