class ToolPose(object): def __init__(self): self.pose = PoseStamped() self._tf_listener = TransformListener() #self.pose_pub = rospy.Publisher('tool_pose', PoseStamped, queue_size=10) #self.joint_sub = rospy.Subscriber('joint_states', JointState, self.tool_pose_callback) self.joint_srv = rospy.Service('tool_pose', GetToolPose, self.tool_pose_callback) def tool_pose_callback(self, req): ''' get gripper link pose with respect to base_link''' res = GetToolPoseResponse() time = 0 trans = None rot = None self.pose.header.frame_id = 'base_link' while not rospy.is_shutdown(): try: time = self._tf_listener.getLatestCommonTime( 'base_link', 'wrist_roll_link') trans, rot = self._tf_listener.lookupTransform( 'base_link', 'wrist_roll_link', time) break except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: continue self.pose.pose.position.x = round(trans[0], 4) self.pose.pose.position.y = round(trans[1], 4) self.pose.pose.position.z = round(trans[2], 4) self.pose.pose.orientation.x = round(rot[0], 4) self.pose.pose.orientation.y = round(rot[1], 4) self.pose.pose.orientation.z = round(rot[2], 4) self.pose.pose.orientation.w = round(rot[3], 4) #self.pose_pub.publish(self.pose) res.tool_pose = self.pose return res
class RoadmapServer: def __init__(self, args): rospy.init_node('roadmap_server') self.optimization_distance = rospy.get_param('~optimization_distance', 10); self.tf = TransformListener() stereo_cam = camera.Camera((389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95)) self.skel = Skeleton(stereo_cam) # self.skel.node_vdist = 1 if False: self.skel.load(args[1]) self.skel.optimize() self.startframe = 100000 else: self.startframe = 0 self.frame_timestamps = {} self.vo = None self.pub = rospy.Publisher("roadmap", vslam.msg.Roadmap) time.sleep(1) #self.send_map(rospy.time(0)) self.wheel_odom_edges = set() rospy.Subscriber('/wide_stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000) def send_map(self, stamp): if len(self.skel.nodes) < 4: return TG = nx.MultiDiGraph() for i in self.skel.nodes: TG.add_node(i) for (a,b,p,c) in self.skel.every_edge + list(self.wheel_odom_edges): TG.add_edge(a, b, (p, c)) here = max(self.skel.nodes) # TG is the total graph, G is the local subgraph uTG = TG.to_undirected() print "uTG", uTG.nodes(), uTG.edges() close = set([here]) for i in range(self.optimization_distance): close |= set(nx.node_boundary(uTG, close)) print "close", close G = TG.subgraph(close) uG = uTG.subgraph(close) pg = TreeOptimizer3() def mk_covar(xyz, rp, yaw): return (1.0 / math.sqrt(xyz),1.0 / math.sqrt(xyz), 1.0 / math.sqrt(xyz), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(yaw)) weak = mk_covar(9e10,3,3) strong = mk_covar(0.0001, 0.000002, 0.00002) pg.initializeOnlineOptimization() def pgadd(p, n, data): relpose, strength = data if strength == 0.0: cov = weak else: cov = strong pg.addIncrementalEdge(p, n, relpose.xform(0,0,0), relpose.euler(), cov) Gedges = G.edges(data = True) revmap = {} map = {} for n in nx.dfs_preorder(uG, here): revmap[len(map)] = n map[n] = len(map) priors = [p for p in uG.neighbors(n) if p in map] if priors == []: print "START NODE", n, "as", map[n] else: print "NEXT NODE", n p = priors[0] if not G.has_edge(p, n): n,p = p,n data = G.get_edge_data(p, n)[0] Gedges.remove((p, n, data)) pgadd(map[p], map[n], data) for (p,n,d) in Gedges: pgadd(map[p], map[n], d) pg.initializeOnlineIterations() start_error = pg.error() for i in range(10): pg.iterate() print "Error from", start_error, "to", pg.error() pg.recomputeAllTransformations() print self.tf.getFrameStrings() target_frame = "base_link" t = self.tf.getLatestCommonTime("wide_stereo_optical_frame", target_frame) trans,rot = self.tf.lookupTransform(target_frame, "wide_stereo_optical_frame", t) xp = Pose() xp.fromlist(self.tf.fromTranslationRotation(trans,rot)) roadmap_nodes = dict([ (n, (30,0,0)) for n in TG.nodes() ]) nl = sorted(roadmap_nodes.keys()) print "nl", nl for i in sorted(map.values()): xyz,euler = pg.vertex(i) p = from_xyz_euler(xyz, euler) pose_in_target = xp * p x,y,z = pose_in_target.xform(0,0,0) euler = pose_in_target.euler() print x,y,z, euler roadmap_nodes[revmap[i]] = (x, y, euler[2]) roadmap_edges = [] for (p,n) in set(TG.edges()): print p,n best_length = max([ (confidence, pose.distance()) for (pose, confidence) in TG.get_edge_data(p, n).values()])[1] roadmap_edges.append((p, n, best_length)) msg = vslam.msg.Roadmap() msg.header.stamp = stamp msg.header.frame_id = "base_link" msg.nodes = [vslam.msg.Node(*roadmap_nodes[n]) for n in nl] print "(p,n)", [ (p,n) for (p,n,l) in roadmap_edges ] msg.edges = [vslam.msg.Edge(nl.index(p), nl.index(n), l) for (p, n, l) in roadmap_edges] msg.localization = nl.index(here) self.pub.publish(msg) if 1: import matplotlib.pyplot as plt fig = plt.figure(figsize = (12, 6)) plt.subplot(121) pos = nx.spring_layout(TG, iterations=1000) nx.draw(TG, pos,node_color='#A0CBE2') nx.draw_networkx_edges(G, pos, with_labels=False, edge_color='r', alpha=0.5, width=25.0, arrows = False) plt.subplot(122) nx.draw(G, pos = dict([(n, roadmap_nodes[n][:2]) for n in G.nodes()])) #plt.show() plt.savefig("/tmp/map_%d.png" % here) def handle_raw_stereo(self, msg): size = (msg.left_info.width, msg.left_info.height) if self.vo == None: cam = camera.StereoCamera(msg.right_info) self.fd = FeatureDetectorFast(300) self.ds = DescriptorSchemeCalonder() self.vo = VisualOdometer(cam, scavenge = False, inlier_error_threshold = 3.0, sba = None, inlier_thresh = 100, position_keypoint_thresh = 0.2, angle_keypoint_thresh = 0.15) self.vo.num_frames = self.startframe pair = [Image.fromstring("L", size, i.uint8_data.data) for i in [ msg.left_image, msg.right_image ]] af = SparseStereoFrame(pair[0], pair[1], feature_detector = self.fd, descriptor_scheme = self.ds) self.vo.handle_frame(af) self.frame_timestamps[af.id] = msg.header.stamp if self.skel.add(self.vo.keyframe): print "====>", self.vo.keyframe.id, self.frame_timestamps[self.vo.keyframe.id] #print self.tf.getFrameStrings() #assert self.tf.frameExists("wide_stereo_optical_frame") #assert self.tf.frameExists("odom_combined") N = sorted(self.skel.nodes) for a,b in zip(N, N[1:]): if (a,b) in self.skel.edges: t0 = self.frame_timestamps[a] t1 = self.frame_timestamps[b] if self.tf.canTransformFull("wide_stereo_optical_frame", t0, "wide_stereo_optical_frame", t1, "odom_combined"): t,r = self.tf.lookupTransformFull("wide_stereo_optical_frame", t0, "wide_stereo_optical_frame", t1, "odom_combined") relpose = Pose() relpose.fromlist(self.tf.fromTranslationRotation(t,r)) self.wheel_odom_edges.add((a, b, relpose, 1.0)) self.send_map(msg.header.stamp)