class Laser2PC(): def __init__(self): self.laserProj = LaserProjection() self.pcPub = rospy.Publisher("/prometheus/sensors/pcl2", pc2, queue_size=1) self.laserSub = rospy.Subscriber("/prometheus/sensors/2Dlidar_scan", LaserScan, self.laserCallback) def laserCallback(self, data): # cc = LaserScan() # cc.header = data.header # cc.angle_max = data.angle_max # cc.angle_min = data.angle_min # cc.angle_increment = data.angle_increment # cc.time_increment = data.time_increment # cc.scan_time = data.scan_time # cc.range_max = data.range_max # cc.range_min = data.range_min # cc.ranges = data.ranges # cc.intensities = data.intensities # a = len(data.ranges) # for i in range(0,a): # if data.ranges[i] != float("inf"): # if data.ranges[i] < 0.4: # cc.ranges[i] = 0.41 cloud_out = self.laserProj.projectLaser(data) self.pcPub.publish(cloud_out)
class Laser2PC(): def __init__(self): self.laserProj = LaserProjection() self.pcPub = rospy.Publisher("/laserPointCloud", PointCloud2, queue_size=1) self.laserSub = rospy.Subscriber("/scan", LaserScan, self.laserCallback, queue_size=1) def laserCallback(self, msg): pc2_msg = self.laserProj.projectLaser(msg) self.pcPub.publish(pc2_msg) # Convert it to a generator of the individual points point_generator = pc2.read_points(pc2_msg) # Access a generator in a loop sum_points = 0.0 num = 0 for point in point_generator: if not np.isnan(point[2]): sum_points += point[2] num += 1 # Calculate the average z value for example print("Average height: " + str(sum_points / num))
class Laser2PC(): #translate laser to pc and publish point cloud to /cloud_in def __init__(self): self._measurements = [float('inf')] self.laserProj = LaserProjection() self.pcPub = rospy.Publisher('/point_cloud2', pc2, queue_size=5) self.laserSub = rospy.Subscriber('/scan', LaserScan, self.laserCallBack) def laserCallBack(self, data): # rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data) cloud_out = self.laserProj.projectLaser(data) cloud_out.header.seq = data.header.frame_id cloud_out.header.stamp = data.header.stamp cloud_out.header.frame_id = 'laser0_frame' self._measurements = cloud_out self.pcPub.publish(cloud_out) @property def ready(self): return not np.isnan(self._measurements) @property def measurements(self): return self._measurements
class Laser2PC(): def __init__(self): self.laserProj = LaserProjection() self.pcPub = rospy.Publisher("/laserPointCLoud",pc2, queue_size=1) self.laserSub = rospy.Subscriber("scan",LaserScan,self.laserCallback) def laserCallback(self,data): cloud_out = self.laserProj.projectLaser(data) self.pcPub.publish(cloud_out)
class LaserScanToPointCloud: def __init__(self): self.laserProj = LaserProjection() self.pointCloudPublisher = rospy.Publisher("/points", PointCloud2, queue_size = 1) self.laserScanSubscriber = rospy.Subscriber("/scan", LaserScan, self.laserScanCallback) def laserScanCallback(self, data): self.pointCloudPublisher.publish(self.laserProj.projectLaser(data))
class Laser2PC(): def __init__(self): self.laserProj= LaserProjection() self.pcPub = rospy.Publisher('Pointy_Cloud', PointCloud2, queue_size=10) self.laserSub = rospy.Subscriber("lidar", LaserScan, self.laserCallback) def laserCallback(self,msg): point_cloud = self.laserProj.projectLaser(msg) self.pcPub.publish(point_cloud)
class Laser2pc(object): def __init__(self,robotname): self.robotname= robotname self.laserProj=LaserProjection() self.pcPub= rospy.Publisher("/{}/LaserPointCloud".format(self.robotname),pc2, queue_size=1) self.lsersub=rospy.Subscriber("/{}/laser_scan".format(self.robotname),LaserScan,self.laserCallback) def laserCallback(self,data): cloud_out=self.laserProj.projectLaser(data) self.pcPub.publish(cloud_out)
def readscan(self): laser_projector = LaserProjection() for topic, msg, time_stamp in self.bag.read_messages( topics=[self.scan_topic]): cloud = laser_projector.projectLaser(msg) frame_points = np.zeros([0, 2]) for p in pc2.read_points(cloud, field_names=("x", "y", "z"), skip_nans=True): p2d = np.array([p[0], p[1]]) frame_points = np.vstack([frame_points, p2d]) self.points.append([time_stamp, frame_points])
class Laser2PC(): def __init__(self): self.laserProj = LaserProjection() self.pcPub = rospy.Publisher("/prometheus/sensors/pcl2", pc2, queue_size=1) self.laserSub = rospy.Subscriber("/prometheus/sensors/2Dlidar_scan", LaserScan, self.laserCallback) def laserCallback(self, data): cloud_out = self.laserProj.projectLaser(data) self.pcPub.publish(cloud_out)
class LaserTurn: def __init__(self,state): rospy.loginfo("Scan done") self.state = state self.done = False self.laser_msg = rospy.Subscriber("/scan", LaserScan, self.update_laser) self.lp = LaserProjection() pc = self.lp.projectLaser(scan) pc.rangemin = targetdistance def act(self): spo = " "
class laser2pc(): def __init__(self): self.laserProj = LaserProjection() self.pcPub = rospy.Publisher("/laser_pointcloud", pc2, queue_size=1) self.lasersub = rospy.Subscriber("/scan", LaserScan, self.laserCallback) def laserCallback(self, data): cloud_out = self.laserProj.projectLaser(data) #convert from point cloud 2 to PCL #pc = ros_numpy.numpify(cloud_out) #points=np.zeros((pc.shape[0],3)) #points[:,0]=pc['x'] #points[:,1]=pc['y'] #points[:,2]=pc['z'] #p = pcl.PointCloud(np.array(points, dtype=np.float32)) self.pcPub.publish(cloud_out)
class Laser2Cloud: def __init__(self): self.tfr = TransformerROS() self.projector = LaserProjection() self.outputTopic = rospy.get_param('output_topic', 'cloud') self.inputTopic = rospy.get_param('input_topic', 'scan') self.outputFrame = rospy.get_param('output_frame', 'base_link') self.publisher = rospy.Publisher(self.outputTopic, PointCloud2, queue_size=10) self.subscriber = rospy.Subscriber(self.inputTopic, LaserScan, self.laser_callback) def laser_callback(self, data): cloud = self.projector.projectLaser(data) self.publisher.publish(cloud)
def data_preprocess(self, msg): laser_projector = LaserProjection() cloud = laser_projector.projectLaser(msg.scan) pts = list( pc2.read_points(cloud, field_names=("x", "y", "z"), skip_nans=True)) scan = np.array(pts)[:, :2] qx = msg.odom.pose.pose.orientation.x qy = msg.odom.pose.pose.orientation.y qz = msg.odom.pose.pose.orientation.z qw = msg.odom.pose.pose.orientation.w odom = tf.transformations.quaternion_matrix((qx, qy, qz, qw)) odom[0, 3] = msg.odom.pose.pose.position.x odom[1, 3] = msg.odom.pose.pose.position.y odom[2, 3] = msg.odom.pose.pose.position.z return scan, odom
def readscan(self): laser_projector = LaserProjection() for topic, msg, time_stamp in self.bag.read_messages( topics=[self.scan_topic]): cloud = laser_projector.projectLaser(msg) frame_points = np.zeros([0, 2]) for p in pc2.read_points(cloud, field_names=("x", "y", "z"), skip_nans=True): #theta = math.atan2(p[0] , p[1]) #laser_range = np.pi #if theta > np.pi/2 + np.pi/3 + laser_range/2 or theta < np.pi/2 + np.pi/3 - laser_range/2: # continue #d = math.sqrt(p[0]*p[0] + p[1]*p[1]) #if d > 1.5 and theta > np.pi/2: # continue p2d = np.array([p[0], p[1]]) frame_points = np.vstack([frame_points, p2d]) self.points.append([time_stamp, frame_points])
class Lidar(): def __init__(self): rospy.loginfo("Initializing Lidar class. Creating subscriber") self.scan_sub = rospy.Subscriber('/base_scan', LaserScan, self.on_scan) self.xyz_points = None rospy.loginfo("Initializing Lidar class. Creating LaserProjection") self.laser_projector = LaserProjection() def on_scan(self, scan): rospy.loginfo("Got scan, projecting") cloud = self.laser_projector.projectLaser(scan) gen = pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z")) self.xyz_points = np.array(list(gen)) rospy.sleep(1/Me.CMD_FREQ) def show_point_plot(self): x = self.xyz_points[:,0] y = self.xyz_points[:,1] plt.scatter(x, y) plt.show()
class Laser2PC(): def __init__(self): self.laserProj = LaserProjection() self.pcPub = rospy.Publisher("/laserPointCloud", pc, queue_size=1) self.laserSub = rospy.Subscriber("/scan", LaserScan, self.laserCallback, queue_size=1) def laserCallback(self, data): #print(data) cloud2_out = self.laserProj.projectLaser(data) #print(cloud2_out) transformed_cloud = pc() transformed_cloud.header.seq = cloud2_out.header.seq transformed_cloud.header.stamp = cloud2_out.header.stamp transformed_cloud.header.frame_id = '/link_chassis' points_from_cloud2 = point_cloud2.read_points(cloud2_out) for p in points_from_cloud2: laser_point_msg = PointStamped() laser_point_msg.header.frame_id = '/laser_frame' laser_point_msg.header.stamp = cloud2_out.header.stamp laser_point_msg.header.seq = cloud2_out.header.seq laser_point_msg.point = Point(p[0], p[1], p[2]) #print(laser_point_msg) #here it shows points #print(point) #print(type(laser_point_msg.point)) #listener = tf.TransformListener() try: transformedPoint = listener.transformPoint( '/link_chassis', laser_point_msg) #print(transformedPoint) #print(transformed_cloud) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue transformed_cloud.points.append(transformedPoint.point) #listener.waitForTransform('/link_chassis', '/laser_frame',) #transformed_cloud.channels = cloud_out.channels #print(transformed_cloud) self.pcPub.publish(transformed_cloud)
class Laser2PC(): def __init__(self): self.laserProj = LaserProjection() # self.inMsg = rospy.get_param('~inMsg') # self.outMsg = rospy.get_param('~outMsg') self.inMsg = '/scan' self.outMsg = '/converted_pc' self.pcPub = rospy.Publisher(self.outMsg, pc2, queue_size=1) self.laserSub = rospy.Subscriber(self.inMsg, LaserScan, self.laserCallback) self.count = 0 def laserCallback(self, data): cloud_out = self.laserProj.projectLaser(data) cloud_out.header = data.header cloud_out.header.frame_id = self.outMsg self.pcPub.publish(cloud_out) self.count += 1 print("Publish complete!" + str(self.count))
class tb3_mapping_node(): def __init__(self): # Variables self.laserProj = LaserProjection() # Publishers self.pcWorldPub = rospy.Publisher("/world_pc", pc2, queue_size=1) #self.pcWaterPub = rospy.Publisher("/water_pc", pc2, queue_size=1) self.mkArrayPub = rospy.Publisher("/marker_array_water_recoloured", MarkerArray, queue_size=1) # Subscribers rospy.Subscriber("/scan", LaserScan, self.callback_lidar_scan) rospy.Subscriber("/marker_array_water", MarkerArray, self.callback_recolour) rospy.spin() # Publish pc2 of world for the 1st Octomap server to use for mapping. def callback_lidar_scan(self, data): cloud_out = self.laserProj.projectLaser(data) self.pcWorldPub.publish(cloud_out) # Listen to markerarray from 2nd Octomap server and modify to distinguish water from the world. def callback_recolour(self, data): tempMarker = data.markers[ len(list(data.markers)) - 1] # Only final marker element contains the markers. tempMarker.header.stamp = rospy.Time( ) # Do not set Time.now() here, it can cause issues for marker being accepted. tempMarker.color.r = 0.0 tempMarker.color.g = 0.9167 tempMarker.color.b = 1.0 tempMarker.color.a = 1.0 tempMarker.scale.z = 0.051 # Slightly larger than map resolution to make water voxels cover world voxels of same location. tempMarker.colors.append(tempMarker.color) self.mkArrayPub.publish([tempMarker ]) # Publish the marker as a MarkerArray
class Laser2PC(): def __init__(self): self.laserProj = LaserProjection() self.pcPub = rospy.Publisher("/laserPCL", pc2, queue_size=1) self.laserSub = rospy.Subscriber("/scan", LaserScan, self.laserCallback) def laserCallback(self, data): cloud_out = self.laserProj.projectLaser(data) Xg = pc2c.read_points(cloud_out, skip_nans=True, field_names=("x", "y", "z")) cloud_points = np.empty((cloud_out.width, 2)) a = 0 for p in Xg: #rospy.loginfo(p) #rospy.loginfo(p[0]) cloud_points[a, 0] = p[0] cloud_points[a, 1] = p[1] #cloud_points[a, 2] = p[2] #rospy.loginfo(cloud_points[a,:]) a = a + 1 #for d in cloud_out.data: # print(d<<4) #rospy.loginfo(cloud_points) # Cluster PCL: #scaler = StandardScaler() #scaler.fit(cloud_points) dbscan = DBSCAN() clusters = dbscan.fit_predict(cloud_points) #print("Cluster Membership:\n{}".format(clusters)) self.pcPub.publish(cloud_out) rospy.loginfo("Start Clusters") rospy.loginfo(clusters) rospy.loginfo("Start Clusters")
class Laser2pc(): #lst = [] def __init__(self): pass def subscriber(self): rospy.Subscriber("/laserscan", LaserScan, self.callbac) rospy.spin() def callbac(self, msg): lst = [] self.converter = LaserProjection() cloud = self.converter.projectLaser(msg) self.pub = rospy.Publisher("/pc", pc2, queue_size=1) self.pub.publish(cloud) for p in pc22.read_points(cloud, field_names=("x", "y", "z"), skip_nans=True): lst.append((p[0], p[1], p[2])) print(lst) print("") print("-------------------") print("")
def callback_scan(self, data): try: transform = self.tfBuffer.lookup_transform("odom", "base_link", rospy.Time()) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): return trans = transform.transform.translation rot = transform.transform.rotation eular = tf.transformations.euler_from_quaternion( (rot.x, rot.y, rot.z, rot.w), "szxy") quat = tf.transformations.quaternion_from_euler(eular[0], 0, 0, "szxy") transform.transform.translation.z = 0 transform.transform.rotation = Quaternion(quat[0], quat[1], quat[2], quat[3]) self.tf_robot_position.sendTransform( (trans.x, trans.y, 0), (quat[0], quat[1], quat[2], quat[3]), rospy.Time.now(), 'robot_position', 'odom') scan = LaserScan() scan = data projector = LaserProjection() cloud = projector.projectLaser(scan) fixed_frame_cloud = do_transform_cloud(cloud, transform) points = pc2.read_points(fixed_frame_cloud, field_names=['x', 'y', 'z'], skip_nans=True) pole = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] data = [] for x, y, z in points: if x > 5 and x < 7 and y > -2 and y < 2: data.append([x, y, z, 0xff0000]) pole[0][0] += x pole[0][1] += y pole[0][2] += 1 elif x > 2 and x < 4 and y > -2 and y < 0: data.append([x, y, z, 0x00ff00]) pole[1][0] += x pole[1][1] += y pole[1][2] += 1 elif x > 2 and x < 4 and y > 0 and y < 2: data.append([x, y, z, 0x0000ff]) pole[2][0] += x pole[2][1] += y pole[2][2] += 1 else: data.append([x, y, z, 0xffff00]) for p in pole: if p[2] != 0: p[0] /= p[2] p[1] /= p[2] if pole[0][2] < 5 or (pole[1][2] < 5 and pole[2][2] < 5): return trans_diff = [6.0 - pole[0][0], 0.0 - pole[0][1]] if pole[1][2] != 0: rot_diff = math.atan2(-1, -3) - math.atan2(pole[1][1] - pole[0][1], pole[1][0] - pole[0][0]) elif pole[2][2] != 0: rot_diff = math.atan2(1, -3) - math.atan2(pole[2][1] - pole[0][1], pole[2][0] - pole[0][0]) # data_pole = [] for x, y, z, color in data: tx = x + trans_diff[0] - 6.0 ty = y + trans_diff[1] rx = tx * math.cos(rot_diff) - ty * math.sin(rot_diff) + 6.0 ry = tx * math.sin(rot_diff) + ty * math.cos(rot_diff) if random.random() < 0.01: self.data_pole.append([rx, ry, z, color]) # print data_pole HEADER = Header(frame_id='/odom') FIELDS = [ PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1), ] filterd_cloud = pc2.create_cloud(HEADER, FIELDS, self.data_pole) self.pc_pub.publish(filterd_cloud)
def compare_val(arr_1,arr_2): # arr_1 = array1[:] # arr_2 = array2[:] obs_list = [] # contains list of points which are same objects for i in range(len(arr_1)-1): empty_list = [] # make a circular array #s = arr_1[i] - arr_2[i] # compare 2 - 1 # print(s) increment = 0.400000095367 j = 0 # print("arr2",arr_2) # print("arr1",arr_1) if arr_1[i] == "nan" or arr_2[i] == "nan": continue # print(len(arr_1) == len(arr_2)) # print(str(arr_2[i]-arr_1[i+1])[2]) # print(str(arr_2[i+1]-arr_1[i])[2]) # laser1 = LaserScan() laser1.header = "laser1" # laser2 = LaserScan() laser2.header = "laser2" x1 = [] x2 = [] for l1 in range(len(arr_2)): x1.append(0) x2.append(0) #making range entries equal to 0 laser1.ranges = x1[:] laser2.ranges = x1[:] while (str(arr_2[i]-arr_1[i+1])[2]) == '3' and (str(arr_2[i+1]-arr_1[i])[2]) == '4': # print("in loop") # empty_list.append(arr_1[i]) #this just makes list # empty_list.append(arr_2[i]) x1[i] = arr_1[i] x2[i] = arr_2[i] i = i + 1 if i-2 > range(len(arr_1)): break if arr_1[i] == "nan" or arr_2[i] == 'nan': i = i + 1 # if len(empty_list) != 0: # obs_list.append(empty_list) laser1.ranges = x1[:] laser2.ranges = x2[:] # print(x1) # print(x2) # print(laser1) # print(laser2) laserproj1 = LaserProjection() laserproj2 = LaserProjection() cloud1 = laserproj1.projectLaser(laser1) cloud2 = laserproj2.projectLaser(laser2) # print(cloud1) # print(cloud2) xyz_array1 = list(point_cloud2.read_points(cloud1, skip_nans=True, field_names = ("x", "y", "z"))) xyz_array2 = list(point_cloud2.read_points(cloud2, skip_nans=True, field_names = ("x", "y", "z"))) if len(xyz_array1) != 0: print("1 ") print(xyz_array1) ## # print("obs_list") # print(obs_list) rate = rospy.Rate(1000) rate.sleep()
class baseScan: def __init__(self, verbose=False): self.rangeData = LaserScan() self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.callback_laser) rospy.sleep(1) self.listener = tf.TransformListener() self.laser_projector = LaserProjection() self.pc = [] self.leg1 = [] self.leg2 = [] self.br = tf.TransformBroadcaster() self.rate = rospy.Rate(10.0) self.rate_measurements = rospy.Rate(0.5) self.priorOri_in_base = [] self.priorLeft_in_base = [] self.priorRight_in_base = [] self.odomL = [] self.odomR = [] self.priorAvailable = False self.newObsWeight = 0.2 self.offsetXY = [0.0, 0.0] self.binOffset = 0.02 #self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10) self.reCalibrationCount = 4 self.tolerance = 0.1 self.updateRounds = 100 self.asyncRate = 20 self.limitInitX = True self.xLimit = 0.1 self.rp = rospkg.RosPack() self.writeFile = False self.calculateTF = False self.sendTF = False self._ready = False self.radius_th_leg = 0.1 self.n_samples = 5 self.sleep_between_scans = 2 while not rospy.is_shutdown(): try: self._shelfHeight = rospy.get_param( "/apc/shelf_calibration/height") self._shelfRoll = rospy.get_param( "/apc/shelf_calibration/roll") self._shelfPitch = rospy.get_param( "/apc/shelf_calibration/pitch") break except: rospy.sleep(1.0) continue self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty, self.start_srv_callback) def start_srv_callback(self, req): while not self._ready: rospy.logwarn('Shelf published waiting for scan callback') rospy.sleep(1.0) rospy.loginfo('[shelf_publisher]: starting shelf publisher') self.calculateTF = True return EmptyResponse() def callback_laser(self, data): self._ready = True self.rangeData = data def getCloud(self): cloud2 = self.laser_projector.projectLaser(self.rangeData) xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z")) self.pc = [] while not rospy.is_shutdown(): try: self.pc.append(xyz.next()) except: break return self.pc def findLegsOnce(self): pc = self.getCloud() x = [] y = [] for i in range(len(pc)): x.append(pc[i][0]) y.append(pc[i][1]) radius = [] if self.limitInitX: y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit] x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit] for i in range(len(x)): radius.append(math.sqrt(x[i]**2 + y[i]**2)) n = radius.index(min(radius)) x2 = [ x[i] for i in range(len(x)) if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) > 0.7 ] y2 = [ y[i] for i in range(len(y)) if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) > 0.7 ] radius2 = [] for i in range(len(x2)): radius2.append(math.sqrt(x2[i]**2 + y2[i]**2)) n2 = radius2.index(min(radius2)) leg1_p = [x[n], y[n]] leg2_p = [x2[n2], y2[n2]] x1_avg = [ x[i] for i in range(len(x)) if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) < self.radius_th_leg ] y1_avg = [ y[i] for i in range(len(y)) if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) < self.radius_th_leg ] x2_avg = [ x[i] for i in range(len(x)) if math.sqrt((x[i] - x[n2])**2 + (y[i] - y[n2])**2) < self.radius_th_leg ] y2_avg = [ y[i] for i in range(len(y)) if math.sqrt((x[i] - x[n2])**2 + (y[i] - y[n2])**2) < self.radius_th_leg ] leg1 = [ numpy.mean(x1_avg) + self.offsetXY[0], numpy.mean(y1_avg) + self.offsetXY[1] ] leg2 = [ numpy.mean(x2_avg) + self.offsetXY[0], numpy.mean(y2_avg) + self.offsetXY[1] ] #leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]] #leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]] return [leg1, leg2] # left, right def findLegs(self): leg1_x = numpy.zeros(self.n_samples) leg1_y = numpy.zeros(self.n_samples) leg2_x = numpy.zeros(self.n_samples) leg2_y = numpy.zeros(self.n_samples) for i in range(0, self.n_samples): rospy.loginfo('taking laser scan: ') rospy.loginfo(i) legs_sample = self.findLegsOnce() leg1_x[i] = legs_sample[0][0] leg1_y[i] = legs_sample[0][1] leg2_x[i] = legs_sample[1][0] leg2_y[i] = legs_sample[1][1] time.sleep(self.sleep_between_scans) #self.rate_measurements.sleep() leg1_m = [numpy.mean(leg1_x), numpy.mean(leg1_y)] leg2_m = [numpy.mean(leg2_x), numpy.mean(leg2_y)] rospy.loginfo('done scan') rospy.loginfo(leg1_m) rospy.loginfo(leg2_m) legs = [leg1_m, leg2_m] if legs[0][1] < legs[1][1]: legs[0], legs[1] = legs[1], legs[0] self.leg1 = legs[0] self.leg2 = legs[1] return [self.leg1, self.leg2] # left, right def getShelfFrame(self): legs = self.findLegs() ori_x = (legs[0][0] + legs[1][0]) / 2. ori_y = (legs[0][1] + legs[1][1]) / 2. left_leg = legs[0] if legs[0][1] < legs[1][1]: left_leg = legs[1] rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y) return [ori_x, ori_y], rotAngle, legs def tf2PoseStamped(self, xy, ori): shelfPoseMsg = PoseStamped() shelfPoseMsg.pose.position.x = xy[0] shelfPoseMsg.pose.position.y = xy[1] shelfPoseMsg.pose.position.z = 0.0 shelfPoseMsg.pose.orientation.x = ori[0] shelfPoseMsg.pose.orientation.y = ori[1] shelfPoseMsg.pose.orientation.z = ori[2] shelfPoseMsg.pose.orientation.w = ori[3] shelfPoseMsg.header.frame_id = 'base' shelfPoseMsg.header.stamp = rospy.Time.now() return shelfPoseMsg def estimateShelfFrame(self): while not rospy.is_shutdown(): self.rate.sleep() t_init = rospy.Time.now() if self.calculateTF: try: shelfOri, shelfRot, legs = self.getShelfFrame() except: rospy.logerr("Shelf calibration is not getting points") continue self.calculateTF = False self.sendTF = True self.writeFile = True if self.sendTF: # F1 = kdl.Frame(kdl.Rotation.RPY(0, 0, shelfRot), # kdl.Vector(shelfOri[0], shelfOri[0], self._shelfHeight)) # F2 = kdl.Frame(kdl.Rotation.RPY(self._shelfRoll, self._shelfPitch, 0), kdl.Vector(0,0,0)) # F3 = F1*F2 # position_xyz3 = F3.p # orientation_xyzw3 = F3.M.GetQuaternion() # self.br.sendTransform((position_xyz3[0], position_xyz3[1], position_xyz3[2]), # orientation_xyzw3, # rospy.Time.now(), # "/shelf_front", # child # "/base" # parent # ) self.br.sendTransform( (shelfOri[0], shelfOri[1], self._shelfHeight), tf.transformations.quaternion_from_euler( self._shelfRoll, self._shelfPitch, shelfRot), rospy.Time.now(), "/shelf_front", # child "/base" # parent ) self.br.sendTransform((legs[0][0], legs[0][1], self._shelfHeight), \ tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot), rospy.Time.now(), "/left_leg", # child "/base" # parent ) self.br.sendTransform((legs[1][0], legs[1][1], self._shelfHeight), \ tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot), rospy.Time.now(), "/right_leg", # child "/base" # parent ) #self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot))) if self.writeFile: file_path = self.rp.get_path('calibration_data') f = open(file_path + "/extrinsics/shelf_front.txt", 'w') f.write(str(shelfOri[0]) + '\t') f.write(str(shelfOri[1]) + '\t') f.write(str(0.0) + '\t') quaternion_shelf = tf.transformations.quaternion_from_euler( 0, 0, shelfRot) f.write(str(quaternion_shelf[0]) + '\t') f.write(str(quaternion_shelf[1]) + '\t') f.write(str(quaternion_shelf[2]) + '\t') f.write(str(quaternion_shelf[3]) + '\n') f.close() self.writeFile = False
class baseScan: def __init__(self, verbose=False): self.rangeData = LaserScan() self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback) self.listener = tf.TransformListener() self.laser_projector = LaserProjection() self.pc = [] self.leg1 = [] self.leg2 = [] self.br = tf.TransformBroadcaster() self.rate = rospy.Rate(20.0) self.calibrated = False self.reCalibration = False self.priorOri_in_base_laser_link = [] # in base_laser_link frame self.priorLeft_in_base_laser_link = [] # in base_laser_link frame self.priorRight_in_base_laser_link = [] # in base_laser_link frame self.odomL = [] # in odom_combined frame self.odomR = [] # in odom_combined frame self.priorAvailable = False self.newObsWeight = 0.2 self.offsetXY = [-0.044, 0] self.binOffset = 0.02 self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped) self.reCalibrationCount = 4 self.tolerance = 0.1 self.updateRounds = 100 self.asyncRate = 20 self.limitInitX = True self.xLimit = 0.1 self.emergencyThreshold = 30*20 self.emergencyTimeWindow = 60 while not rospy.is_shutdown(): try: self.offsetXY = rospy.get_param('/base_scan/offset') break except: rospy.loginfo('[shelf publisher]: waiting for base scan offset param') rospy.sleep(random.uniform(0,1)) continue self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty, self.start_srv_callback) self._start = False # backup human supervised info for emergency rp = rospkg.RosPack() try: dict_fp = rp.get_path('amazon_challenge_grasping') except: rospy.logerr('[shelf_publisher]: emergency file not found!') sys.exit(1) self.fileName = dict_fp + '/config/shelf_emergency.dat' self.emergency = {} def start_srv_callback(self, req): rospy.loginfo('[shelf_publisher]: starting shelf publisher!') if self._start: rospy.logwarn('[shelf_publisher]: already started!!!!') self._start = True return EmptyResponse() def raw_input_with_timeout(prompt, timeout=1.0): print prompt, timer = threading.Timer(timeout, thread.interrupt_main) astring = None try: timer.start() astring = raw_input(prompt) except KeyboardInterrupt: pass timer.cancel() return astring def callback(self,data): self.rangeData = data def refreshRangeData(self): self.rangeData = LaserScan() # flush while len(self.rangeData.ranges) == 0 and not rospy.is_shutdown(): rospy.sleep(0.04) def getStatus(self): return self.calibrated def getCloud(self): # self.refreshRangeData() cloud2 = self.laser_projector.projectLaser(self.rangeData) xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z")) self.pc = [] while not rospy.is_shutdown(): try: self.pc.append(xyz.next()) except: break return self.pc def findLegsOnce(self): pc = self.getCloud() x = [] y= [] for i in range(len(pc)): x.append(pc[i][0]) y.append(pc[i][1]) radius = [] if self.calibrated or self.reCalibration: # use prior to find legs for i in range(len(x)): radius.append(math.sqrt((x[i]-self.priorLeft_in_base_laser_link[0])**2 + (y[i] - self.priorLeft_in_base_laser_link[1])**2)) n1 = radius.index(min(radius)) radius = [] for i in range(len(x)): radius.append(math.sqrt((x[i]-self.priorRight_in_base_laser_link[0])**2 + (y[i] - self.priorRight_in_base_laser_link[1])**2)) n2 = radius.index(min(radius)) leg1 = [x[n1], y[n1]] leg2 = [x[n2], y[n2]] else: # Assuming there is nothing between the robot and the shelf if self.limitInitX: y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit] x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit] for i in range(len(x)): radius.append(math.sqrt(x[i]**2 + y[i]**2)) n = radius.index(min(radius)) x2 = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) > 0.7] y2 = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) > 0.7] radius2 = [] for i in range(len(x2)): radius2.append(math.sqrt(x2[i]**2 + y2[i]**2)) n2 = radius2.index(min(radius2)) leg1 = [x[n], y[n]] leg2 = [x2[n2], y2[n2]] leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]] leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]] return [leg1, leg2] # left, right def findLegs(self): ''' accumulate new observations ''' L1x = 0 L1y = 0 L2x = 0 L2y = 0 legs = self.findLegsOnce() if legs[0][1] < legs[1][1]: legs[0], legs[1] = legs[1], legs[0] if self.calibrated: self.leg1[0] = self.leg1[0] * (1-self.newObsWeight) + legs[0][0] * self.newObsWeight self.leg1[1] = self.leg1[1] * (1-self.newObsWeight) + legs[0][1] * self.newObsWeight self.leg2[0] = self.leg2[0] * (1-self.newObsWeight) + legs[1][0] * self.newObsWeight self.leg2[1] = self.leg2[1] * (1-self.newObsWeight) + legs[1][1] * self.newObsWeight else: self.leg1 = legs[0] self.leg2 = legs[1] return [self.leg1, self.leg2] # left, right def getShelfFrame(self): # with respect to the frame of /base_scan legs = self.findLegs() ori_x = (legs[0][0] + legs[1][0]) / 2. ori_y = (legs[0][1] + legs[1][1]) / 2. left_leg = legs[0] if legs[0][1] < legs[1][1]: left_leg = legs[1] rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y) return [ori_x, ori_y], rotAngle, legs def tf2PoseStamped(self, xy, ori): shelfPoseMsg = PoseStamped() shelfPoseMsg.pose.position.x = xy[0] shelfPoseMsg.pose.position.y = xy[1] shelfPoseMsg.pose.position.z = 0.0 shelfPoseMsg.pose.orientation.x = ori[0] shelfPoseMsg.pose.orientation.y = ori[1] shelfPoseMsg.pose.orientation.z = ori[2] shelfPoseMsg.pose.orientation.w = ori[3] shelfPoseMsg.header.frame_id = 'base_laser_link' shelfPoseMsg.header.stamp = rospy.Time.now() return shelfPoseMsg def saveEmergency(self): with open(self.fileName, 'wb') as handle: pickle.dump(self.emergency, handle) def loadEmergency(self): with open(self.fileName, 'rb') as handle: self.emergency = pickle.load(handle) def publish2TF(self): answer = 'n' ask = True u = 0 shelfN = 0 recalibrateCount = 0 emergencyCount = 0 t_init = rospy.Time.now() while not rospy.is_shutdown(): self.rate.sleep() # check if human calibration is done shelfOri, shelfRot, legs = self.getShelfFrame() if self.reCalibration: u = 0 if not self.calibrated: self.br.sendTransform((shelfOri[0], shelfOri[1], 0), tf.transformations.quaternion_from_euler(0, 0, shelfRot), rospy.Time.now(), "/shelf_frame", # child "/base_laser_link" # parent ) self.br.sendTransform((legs[0][0], legs[0][1], 0), \ tf.transformations.quaternion_from_euler(0, 0, shelfRot), \ rospy.Time.now(), \ "/left_leg", \ "/base_laser_link") self.br.sendTransform((legs[1][0], legs[1][1], 0), \ tf.transformations.quaternion_from_euler(0, 0, shelfRot), \ rospy.Time.now(), \ "/right_leg", \ "/base_laser_link") if self.priorAvailable: while not rospy.is_shutdown(): try: shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0)) self.priorLeft_in_base_laser_link, dummy = self.listener.lookupTransform("/base_laser_link", "/odomL", rospy.Time(0)) self.priorRight_in_base_laser_link, dummy = self.listener.lookupTransform("/base_laser_link", "/odomR", rospy.Time(0)) break except: continue else: try: shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0)) except Exception, e: # print e continue if self.reCalibration and math.sqrt((shelf_in_odom[0]-self.priorOri_in_odom[0]) **2 + (shelf_in_odom[1]-self.priorOri_in_odom[1]) **2) <= self.tolerance: # rospy.sleep(2) recalibrateCount += 1 if recalibrateCount == self.reCalibrationCount: # take recalibration only if it's stable rospy.loginfo('reCalibrated!') recalibrateCount = 0 u = 0 while not rospy.is_shutdown(): # make sure the odomL and odomR are updated try: ######## self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0)) ######## self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0)) ######## self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0)) self.calibrated = True self.reCalibration = False rospy.loginfo("Prior origin in odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1])) break except: continue if not self.calibrated and ask: emergencyCount += 1 if emergencyCount == self.emergencyThreshold: self.loadEmergency() self.odomL = self.emergency.odomL self.odomL_rot = self.emergency.odomL_rot self.odomR = self.emergency.odomR self.odomR_rot = self.emergency.odomR_rot emergency_timestamp = self.emergency.timestamp if (rospy.Time.now() - emergency_timestamp).to_sec() > self.emergencyTimeWindow: print '' rospy.logwarn('***********************************************************************') rospy.logwarn('[shelf_publisher] NOT ABLE TO RECOVER FROM CRASHING, GAME OVER') rospy.logwarn('***********************************************************************') continue ask = False self.calibrated = True self.priorAvailable = True self.priorOri_in_odom = shelf_in_odom self.priorRot_in_odom = shelf_rot_in_odom self.priorLeft_in_base_laser_link = legs[0] self.priorRight_in_base_laser_link = legs[1] print '' rospy.logwarn('***********************************************************************') rospy.logwarn('[shelf_publisher] EMERGENCY RECOVERY CALLED!!!!!!!!!!!!!!!!!!!!!!') rospy.logwarn('***********************************************************************') else: sys.stdout.write("\r [ROS time: %s] Is the current shelf pose estimation good? (y/n)" % rospy.get_time() ) sys.stdout.flush() if self._start: answer = 'y' if answer == 'y' or answer == 'yes': self.calibrated = True ask = False self.priorAvailable = True self.priorOri_in_odom = shelf_in_odom self.priorRot_in_odom = shelf_rot_in_odom self.priorLeft_in_base_laser_link = legs[0] self.priorRight_in_base_laser_link = legs[1] print "" rospy.loginfo("Human calibration finished") rospy.loginfo("Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1])) while not rospy.is_shutdown(): try: self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0)) self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0)) break except: pass self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now()) self.saveEmergency() else: continue # check in the odometry frame if self.priorAvailable: # print 'pub odom' self.br.sendTransform(self.odomL, \ self.odomL_rot, \ rospy.Time.now(),\ "/odomL", \ "/odom_combined") self.br.sendTransform(self.odomR, \ self.odomR_rot, \ rospy.Time.now(),\ "/odomR", \ "/odom_combined") if self.calibrated: u += 1 shelfN += 1 if math.sqrt((shelf_in_odom[0] - self.priorOri_in_odom[0]) **2 + (shelf_in_odom[1] - self.priorOri_in_odom[1]) **2) > self.tolerance: rospy.logwarn('something is wrong with shelf pose estimation!!!!!!!!!! RECALIBRATING') recalibrateCount = 0 u = 0 self.calibrated = False self.reCalibration = True continue else: self.br.sendTransform((shelfOri[0], shelfOri[1], 0), tf.transformations.quaternion_from_euler(0, 0, shelfRot), rospy.Time.now(), "/shelf_frame", # child "/base_laser_link" # parent ) self.br.sendTransform((legs[0][0], legs[0][1], 0), \ tf.transformations.quaternion_from_euler(0, 0, shelfRot), \ rospy.Time.now(),\ "/left_leg", \ "/base_laser_link") self.br.sendTransform((legs[1][0], legs[1][1], 0), \ tf.transformations.quaternion_from_euler(0, 0, shelfRot), \ rospy.Time.now(),\ "/right_leg", \ "/base_laser_link") self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot))) if u == self.updateRounds: while not rospy.is_shutdown(): # make sure the odomL and odomR are updated try: self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0)) self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0)) self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0)) rospy.loginfo("Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1])) u = 0 break except: continue if (rospy.Time.now()-t_init).to_sec()>1.0: t_init = rospy.Time.now() self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now()) self.saveEmergency() # print 'pub' shelfN = 0 self.br.sendTransform((0, 0.1515 + self.binOffset, 1.21), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_A", # child "/shelf_frame" # parent ) self.br.sendTransform((0, - 0.1515 + self.binOffset, 1.21), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_B", # child "/shelf_frame" # parent ) self.br.sendTransform((0, - 0.4303 + self.binOffset, 1.21), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_C", # child "/shelf_frame" # parent ) self.br.sendTransform((0, 0.1515 + self.binOffset, 1), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_D", # child "/shelf_frame" # parent ) self.br.sendTransform((0, - 0.1515 + self.binOffset, 1), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_E", # child "/shelf_frame" # parent ) self.br.sendTransform((0, - 0.4303 + self.binOffset, 1), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_F", # child "/shelf_frame" # parent ) self.br.sendTransform((0, 0.1515 + self.binOffset, 0.78), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_G", # child "/shelf_frame" # parent ) self.br.sendTransform((0, - 0.1515 + self.binOffset, 0.78), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_H", # child "/shelf_frame" # parent ) self.br.sendTransform((0, - 0.4303 + self.binOffset, 0.78), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_I", # child "/shelf_frame" # parent ) self.br.sendTransform((0, 0.1515 + self.binOffset, 0.51), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_J", # child "/shelf_frame" # parent ) self.br.sendTransform((0, - 0.1515 + self.binOffset, 0.51), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_K", # child "/shelf_frame" # parent ) self.br.sendTransform((0, - 0.4303 + self.binOffset, 0.51), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_L", # child "/shelf_frame" # parent )
class Explore: def __init__(self): """ Class constructor """ rospy.init_node('explore', anonymous=True) self.laser = LaserScan() self.laser_proj = LaserProjection() self.pc_pub = rospy.Publisher('/laserPointCloud', PointCloud2, queue_size=1) self.sacn_subscriber = rospy.Subscriber('/scan', LaserScan, self.callback) self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self.front_thresh = 0.3 def callback(self, msg): 'inside callback' ranges = list(msg.ranges) cloud_out = self.laser_proj.projectLaser(msg) # front_thresh = 0.27 side_thresh = 0.5 tolerance = 0.38 right_dist_average = 0 left_dist_average = 0 front_dist = 100 if len(ranges) != 0: right_dist = ranges[45:134] left_dist = ranges[225:314] if ranges[0] < 100000: front_dist_list = ranges[349:359] front_dist_list.extend(ranges[0:10]) front_dist = sum(front_dist_list) / len(front_dist_list) right_dist_average = sum(right_dist) / len(right_dist) left_dist_average = sum(left_dist) / len(left_dist) diff = abs(right_dist_average - left_dist_average) right = False left = False if right_dist_average > left_dist_average and diff > tolerance: left = True if left_dist_average > right_dist_average and diff > tolerance: right = True if (right_dist_average > side_thresh) and (left_dist_average > side_thresh): left = True right = True #------------------------------------------------------------------------------------------- if front_dist > self.front_thresh: print 'drive straight', front_dist, self.front_thresh if front_dist > 1: self.front_thresh = 0.27 self.send_speed(1, 0) else: if left and (not right): self.front_thresh = front_dist - 0.05 print 'turn left', front_dist, self.front_thresh self.rotate(pi / 2, 1) if right and (not left): self.front_thresh = front_dist - 0.05 print 'turn right', front_dist, self.front_thresh self.rotate(pi / 2, -1) if (not right) and (not left): self.front_thresh = front_dist - 0.05 print 'drive back', front_dist, self.front_thresh self.rotate(pi, 1) if left and right: x = random.randint(0, 1) if x == 0: print 'turn left rand', front_dist, self.front_thresh self.front_thresh = front_dist - 0.05 self.rotate(pi / 2, 1) else: print 'turn right rand', right_dist_average, left_dist_average, diff self.front_thresh = front_dist - 0.05 self.rotate(pi / 2, -1) def send_speed(self, linear_speed, angular_speed): """ Sends the speeds to the motors. :param linear_speed [float] [m/s] The forward linear speed. :param angular_speed [float] [rad/s] The angular speed for rotating around the body center. """ # REQUIRED CREDIT # Make a new Twist message # TODO self.msg_cmd_vel = Twist() #Linear velocity self.msg_cmd_vel.linear.x = linear_speed self.msg_cmd_vel.linear.y = 0.0 self.msg_cmd_vel.linear.z = 0.0 #Angular Velocity self.msg_cmd_vel.angular.x = 0.0 self.msg_cmd_vel.angular.y = 0.0 if angular_speed < 0: clockwise = True else: clockwise = False if clockwise: self.msg_cmd_vel.angular.z = -abs(angular_speed) else: self.msg_cmd_vel.angular.z = abs(angular_speed) # Publish the message # TODO self.velocity_publisher.publish(self.msg_cmd_vel) def rotate(self, angle, aspeed): """ Rotates the robot around the body center by the given angle. :param angle [float] [rad] The distance to cover. :param angular_speed [float] [rad/s] The angular speed. """ # REQUIRED CREDIT init_time = rospy.Time.now().to_sec() current_angle = 0 while current_angle < abs(angle): self.send_speed(0, aspeed) time = rospy.Time.now().to_sec() if aspeed > 0: current_angle = aspeed * (time - init_time) else: current_angle = -aspeed * (time - init_time) self.msg_cmd_vel.angular.z = 0 self.velocity_publisher.publish(self.msg_cmd_vel) # new_laser = LaserScan() # self.callback(new_laser) def smooth_drive(self, distance, linear_speed): """ Drives the robot in a straight line by changing the actual speed smoothly. :param distance [float] [m] The distance to cover. :param linear_speed [float] [m/s] The maximum forward linear speed. """ # rospy.loginfo("------- Smooth drive starting --------") initial_pose = [self.odom_x, self.odom_y] traveled = math.sqrt((initial_pose[0] - self.odom_x)**2 + (initial_pose[1] - self.odom_y)**2) tolerance = linear_speed / 50.0 coeff = 1 self.send_speed(0, 0) while traveled <= distance - tolerance: # accelerate linearly while traveled < 0.2: self.send_speed(traveled + .2, 0) # decelerate with sin() curve if distance - traveled < linear_speed + 3: linear_speed = 1 coeff = math.sin(math.pi / 2 + (math.pi / 2 * (traveled / distance))) scaled_speed = linear_speed * coeff if scaled_speed < 0.1: scaled_speed = 0.1 self.send_speed(scaled_speed, 0) rospy.sleep(.05) traveled = math.sqrt((initial_pose[0] - self.odom_x)**2 + (initial_pose[1] - self.odom_y)**2) # rospy.loginfo("Scaled speed: " + str(scaled_speed)) # rospy.loginfo("Total distance traveled: " + str(traveled)) self.send_speed(0, 0) # rospy.sleep(1) stopped_at = traveled traveled = math.sqrt((initial_pose[0] - self.odom_x)**2 + (initial_pose[1] - self.odom_y)**2) # rospy.loginfo("------- Smooth drive finished --------") # rospy.loginfo("Desired distance: " + str(distance)) # rospy.loginfo("Total distance after stopping: " + str(traveled)) # rospy.loginfo("Additional distance after stopping: " + str(traveled - stopped_at)) rospy.loginfo("Distance error: " + str(traveled - distance)) def run(self): self.callback(LaserScan()) rospy.spin()
class UWTeleop(object): def __init__(self): rospy.init_node("uw_teleop") self.vel_pub1 = rospy.Publisher('/dataNavigator1', Odometry, queue_size=10) self.vel_pub2 = rospy.Publisher('/dataNavigator2', Odometry, queue_size=10) self.rate = rospy.Rate(20) self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_msg_callback) self.bf1_odom_sub = rospy.Subscriber("/uwsim/BlueFox1Odom", Odometry, self.pose1_callback) self.bf2_odom_sub = rospy.Subscriber("/uwsim/BlueFox2Odom", Odometry, self.pose2_callback) ####### Make a lidar by stacking multibeam sensors ################## self.bf1_laser_sub = rospy.Subscriber("/BlueFox1/multibeam", LaserScan, self.laser_callback) self.bf1_laser1_sub = rospy.Subscriber("/BlueFox1/multibeam1", LaserScan, self.laser1_callback) self.bf1_laser2_sub = rospy.Subscriber("/BlueFox1/multibeam2", LaserScan, self.laser2_callback) self.bf1_laser3_sub = rospy.Subscriber("/BlueFox1/multibeam3", LaserScan, self.laser3_callback) self.bf1_laser4_sub = rospy.Subscriber("/BlueFox1/multibeam4", LaserScan, self.laser4_callback) self.bf1_laser5_sub = rospy.Subscriber("/BlueFox1/multibeam5", LaserScan, self.laser5_callback) self.bf1_laser6_sub = rospy.Subscriber("/BlueFox1/multibeam6", LaserScan, self.laser6_callback) self.bf1_laser7_sub = rospy.Subscriber("/BlueFox1/multibeam7", LaserScan, self.laser7_callback) self.bf1_laser8_sub = rospy.Subscriber("/BlueFox1/multibeam8", LaserScan, self.laser8_callback) self.bf1_laser9_sub = rospy.Subscriber("/BlueFox1/multibeam9", LaserScan, self.laser9_callback) self.bf1_laser10_sub = rospy.Subscriber("/BlueFox1/multibeam10", LaserScan, self.laser10_callback) self.bf1_laser11_sub = rospy.Subscriber("/BlueFox1/multibeam11", LaserScan, self.laser11_callback) self.laser_data = {} self.combined_laserscan = LaserScan() ####################################################################### self.bf2_laser_sub = rospy.Subscriber("/BlueFox2/multibeam", LaserScan, self.laser2_callback) # messages required by /ndt_mapping self.lidar_pc2_pub = rospy.Publisher('velodyne_points', PointCloud2, queue_size=10) self.imu_pub = rospy.Publisher("/imu_raw", Imu, queue_size=10) self.vehicle_odom_pub = rospy.Publisher('/odom_pose', Odometry, queue_size=10) self.joy_data = Joy() self.vel_cmd1 = Odometry() self.vel_cmd2 = Odometry() self.bf1_pose = None self.bf2_pose = None self.odom_hz = 10 self.bf1_vel = None self.bf2_vel = None self.bf1_laser = LaserScan() self.bf2_laser = LaserScan() self.joy_button = None self.lidar_counter = 0 # this is a hack to make lidar spin self.laser_projection = LaserProjection() self.velodyne_mapped_laserscan = LaserScan() self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) def pose1_callback(self, data): pose = data.pose.pose self.bf1_pose = pose self.bf1_vel = data.twist.twist # publish a /odom_pose topic odom = Odometry() odom.pose = data.pose odom.twist = self.vel_cmd1.twist self.vehicle_odom_pub.publish(odom) # Publish a simulated Imu topic # imu = Imu() # imu.orientation = data.pose.pose.orientation def pose2_callback(self, data): pose = data.pose.pose self.bf2_pose = pose self.bf2_vel = data.twist.twist def laser2_callback(self, data): self.bf2_laser = data def get_relative_pose(self, source, target): ''' returns a vector pointing from source to target ''' source_pos = np.array( [source.position.x, source.position.y, source.position.z]) target_pos = np.array( [target.position.x, target.position.y, target.position.z]) source_ori = source.orientation target_ori = target.orientation source_quater = (source_ori.x, source_ori.y, source_ori.z, source_ori.w) T_source = tf.transformations.quaternion_matrix(source_quater) T_source[:3, 3] = source_pos target_quater = (target_ori.x, target_ori.y, target_ori.z, target_ori.w) T_target = tf.transformations.quaternion_matrix(target_quater) T_target[:3, 3] = target_pos T_rel = np.dot(np.linalg.inv(T_source), T_target) return T_rel def get_planar_pose_follow_cmd(self, source, target, desired_dist=3.0): ''' source vehicle follows target vehicle keeping a distance of 3.0 meters and maintaining the elevation ''' T_rel = self.get_relative_pose(source, target) roll_rel, pitch_rel, yaw_rel = tf.transformations.euler_from_matrix( T_rel, axes='sxyz') rel_pos = T_rel[:3, 3] current_dist = np.linalg.norm(rel_pos[:2]) unit_rel_pos_xy = rel_pos[:2] / np.linalg.norm(rel_pos[:2]) # q_rel = tf.transformations.quaternion_from_matrix(T_rel) vel_cmd = Odometry() p = 0.1 vel_cmd.twist.twist.linear.z = p * rel_pos[2] if abs(np.linalg.norm(rel_pos[:2]) - desired_dist) < 0.01: vel_cmd.twist.twist.linear.x = 0 vel_cmd.twist.twist.linear.y = 0 elif np.linalg.norm(rel_pos[:2]) > desired_dist: vel_cmd.twist.twist.linear.x = p * unit_rel_pos_xy[0] vel_cmd.twist.twist.linear.y = p * unit_rel_pos_xy[1] else: vel_cmd.twist.twist.linear.x = -p * unit_rel_pos_xy[0] vel_cmd.twist.twist.linear.y = -p * unit_rel_pos_xy[1] vel_cmd.twist.twist.angular.x = 2.0 * p * roll_rel vel_cmd.twist.twist.angular.y = 2.0 * p * pitch_rel vel_cmd.twist.twist.angular.z = 2.0 * p * yaw_rel return vel_cmd def get_planar_vel_follow_cmd(self, target_vel): ''' source vehicle copies target vehicle's velocity''' vel_cmd = Odometry() vel_cmd.twist.twist = target_vel return vel_cmd def get_obs_avoidance_cmd(self, source_id, scale=0.01): if source_id == 1: laser = self.bf1_laser pose = self.bf1_pose if source_id == 2: laser = self.bf2_laser pose = self.bf2_pose angle = laser.angle_min angle_inc = laser.angle_increment for i, r in enumerate(laser.ranges): r_vec = scale * np.array([np.cos(angle), np.sin(angle)]) / r if i == 0: vel_vec = r_vec else: vel_vec = np.vstack([vel_vec, r_vec]) vel_tot = np.sum(vel_vec, axis=0) vel_cmd = Odometry() vel_cmd.twist.twist.linear.x = -vel_tot[0] vel_cmd.twist.twist.linear.y = -vel_tot[1] return vel_cmd def joy_msg_callback(self, data): ''' joy_data.axes = [+left, +forward, +yaw, +up, +pitch, +roll] joy_data.buttons = [take_pictures, ...] ''' self.joy_data = data if self.joy_data.buttons[1] == 0: self.joy_button = 0 self.vel_cmd1.twist.twist.linear.x = self.joy_data.axes[1] * .125 self.vel_cmd1.twist.twist.linear.y = -self.joy_data.axes[0] * .125 self.vel_cmd1.twist.twist.linear.z = -self.joy_data.axes[3] * .125 self.vel_cmd1.twist.twist.angular.x = self.joy_data.axes[4] * .125 self.vel_cmd1.twist.twist.angular.y = self.joy_data.axes[5] * .125 self.vel_cmd1.twist.twist.angular.z = -self.joy_data.axes[2] * .125 if self.joy_data.buttons[1] == 1: self.joy_button = 1 self.vel_cmd2.twist.twist.linear.x = self.joy_data.axes[1] * .125 self.vel_cmd2.twist.twist.linear.y = -self.joy_data.axes[0] * .125 self.vel_cmd2.twist.twist.linear.z = -self.joy_data.axes[3] * .125 self.vel_cmd2.twist.twist.angular.x = self.joy_data.axes[4] * .125 self.vel_cmd2.twist.twist.angular.y = self.joy_data.axes[5] * .125 self.vel_cmd2.twist.twist.angular.z = -self.joy_data.axes[2] * .125 def start_teleop(self): while not rospy.is_shutdown(): if self.joy_button == 0: # user is controlling BF1, BF2 is following follow_cmd = self.get_planar_pose_follow_cmd( source=self.bf2_pose, target=self.bf1_pose, desired_dist=3.0) obs_cmd = self.get_obs_avoidance_cmd(source_id=1, scale=0.01) self.vel_cmd2 = follow_cmd self.vel_cmd2.twist.twist.linear.x += obs_cmd.twist.twist.linear.x self.vel_cmd2.twist.twist.linear.y += obs_cmd.twist.twist.linear.y # self.vel_cmd2 = self.get_planar_vel_follow_cmd(target_vel=self.bf1_vel) # if self.joy_button == 1: # user is controlling BF1, BF2 is following # rel_pos = self.get_relative_pose(source=self.bf1_pose, target=self.bf2_pose) # #print(rel_pos) # source_pos = np.array([self.bf1_pose.position.x, self.bf1_pose.position.y, self.bf1_pose.position.z]) # target_pos = np.array([self.bf2_pose.position.x, self.bf2_pose.position.y, self.bf2_pose.position.z]) # self.vel_cmd1 = self.get_planar_pose_follow_cmd(source_pos=source_pos, target_pos=target_pos,rel_pos=rel_pos,desired_dist=3.0) self.vel_pub1.publish(self.vel_cmd1) self.vel_pub2.publish(self.vel_cmd2) self.rate.sleep() def lidar_callback(self, data): T = self.tf_buffer.lookup_transform("lidar", "velodyne", rospy.Time()) data.intensities = [1] * len(data.ranges) pc = self.laser_projection.projectLaser( data, channel_options=self.laser_projection.ChannelOption.INTENSITY) pc_transformed = do_transform_cloud(pc, T) pc_transformed.header.frame_id = "velodyne" pc_transformed.is_dense = True self.lidar_pc2_pub.publish(pc_transformed) def laser1_callback(self, data): self.bf1_laser = data
class Laser2PC(): def __init__(self): self.laserProj = LaserProjection() #self.pub = rospy.Publisher("/own/true/sonar_PC2",PointCloud2, queue_size=1) #self.pub1 = rospy.Publisher("/own/simulated/sonar_PC2",PointCloud2, queue_size=1) self.pub2 = rospy.Publisher("/sonar/PC2", PointCloud2, queue_size=1) #self.sub = rospy.Subscriber("/desistek_saga/sonar",LaserScan, self.callback) #self.sub1 = rospy.Subscriber("/own/simulated/sonar_LS",LaserScan, self.callback1) self.sub2 = rospy.Subscriber("/sonar/LS", LaserScan, self.callback2) """ def callback(self,data): laser = LaserScan() laser.header = data.header laser.angle_min = data.angle_min laser.angle_max = data.angle_max laser.angle_increment = data.angle_increment laser.time_increment = data.time_increment laser.scan_time = data.scan_time laser.range_min = data.range_min laser.range_max = data.range_max laser.intensities = data.intensities laser.ranges = data.ranges cloud = self.laserProj.projectLaser(laser) self.pub.publish(cloud) def callback1(self,arg): laser = LaserScan() laser.header = arg.header laser.angle_min = arg.angle_min laser.angle_max = arg.angle_max laser.angle_increment = arg.angle_increment laser.time_increment = arg.time_increment laser.scan_time = arg.scan_time laser.range_min = arg.range_min laser.range_max = arg.range_max laser.intensities = arg.intensities laser.ranges = arg.ranges cloud = self.laserProj.projectLaser(laser) self.pub1.publish(cloud) """ def callback2(self, arg): laser = LaserScan() laser.header = arg.header laser.angle_min = arg.angle_min laser.angle_max = arg.angle_max laser.angle_increment = arg.angle_increment laser.time_increment = arg.time_increment laser.scan_time = arg.scan_time laser.range_min = arg.range_min laser.range_max = arg.range_max laser.intensities = arg.intensities laser.ranges = arg.ranges cloud = self.laserProj.projectLaser(laser) self.pub2.publish(cloud)
class Localization_quality_estimation(): def __init__(self): rospy.init_node('map_to_pointcloud') self.br = tf.TransformBroadcaster() self.transform = TransformStamped() self.laser_footprint_transform = TransformStamped() self.tf_listener = tf.TransformListener() self.ts_old = rospy.Time.now() self.index_old = 0 self.tf_listener.waitForTransform("/pr1/base_footprint", "pr1/left_laser_link", rospy.Time(0), rospy.Duration(1)) (self.trans, self.rot) = self.tf_listener.lookupTransform("/pr1/base_footprint", "pr1/left_laser_link",rospy.Time(0)) self.laser_footprint_transform.transform.translation.x = self.trans[0] self.laser_footprint_transform.transform.translation.y = self.trans[1] self.laser_footprint_transform.transform.rotation.x = self.rot[0] self.laser_footprint_transform.transform.rotation.y = self.rot[1] self.laser_footprint_transform.transform.rotation.z = self.rot[2] self.laser_footprint_transform.transform.rotation.w = self.rot[3] #rospy.Subscriber("/pr1/amcl_pose", PoseWithCovarianceStamped, self.robot_pose_cb) rospy.Subscriber("map", OccupancyGrid, self.map_cb) rospy.sleep(1) self.laser_projection = LaserProjection() rospy.Subscriber("/pr1/l_scan", LaserScan, self.laser_cb) self.pc2_pub = rospy.Publisher("pointcloud", pc2, queue_size=1) rospy.spin() def map_cb(self, msg = OccupancyGrid()): map_metadata = msg.info map_data = msg.data map_width = map_metadata.width map_resolution = map_metadata.resolution map_origin = map_metadata.origin self.obstacles_x = [] self.obstacles_y = [] for i in range(0,len(map_data)): if map_data[i] == 100 and self.index_old != i-1: x = (i % map_width) * map_resolution + map_origin.position.x y = (i // map_width) * map_resolution + map_origin.position.y self.obstacles_x.append(x) self.obstacles_y.append(y) self.index_old = i # br = tf.TransformBroadcaster() # br2= tf2_ros.TransformBroadcaster() # t = TransformStamped() # rospy.sleep(5) # for i in range(0,len(self.obstacles_x),10): # #print("index: ", i) # status = br.sendTransform((self.obstacles_x[i], self.obstacles_y[i], 0), # tf.transformations.quaternion_from_euler(0, 0, 0), # rospy.Time.now(), # "obstacle_" + str(i), # "map") # t.header.stamp = rospy.Time.now() # t.header.frame_id = "map" # t.child_frame_id = "obstacle_" + str(i) # t.transform.translation.x = self.obstacles_x[0] # t.transform.translation.y = self.obstacles_y[0] # status2 = br2.sendTransform(t) # print(status, status2) def laser_cb(self, msg = LaserScan()): now = rospy.Time.now() if now - self.ts_old > rospy.Duration(0.05): cloud_out = self.laser_projection.projectLaser(msg) #print(rospy.Time.now()) (trans, rot) = self.tf_listener.lookupTransform("/map", "pr1/left_laser_link",rospy.Time(0)) self.transform.transform.translation.x = trans[0] self.transform.transform.translation.y = trans[1] self.transform.transform.rotation.x = rot[0] self.transform.transform.rotation.y = rot[1] self.transform.transform.rotation.z = rot[2] self.transform.transform.rotation.w = rot[3] cloud_transformed = do_transform_cloud(cloud_out, self.transform) #self.pc2_pub.publish(cloud_transformed) pc_np_transformed = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(cloud_transformed, remove_nans=True) # for i in range(0,len(pc_np_transformed),15): # #print("index: ", i) # status = self.br.sendTransform((pc_np_transformed[i][0], pc_np_transformed[i][1], 0), # tf.transformations.quaternion_from_euler(0, 0, 0), # rospy.Time.now(), # "scan" + str(i), # "map") matches = 0 for i in range(0,len(pc_np_transformed),5): for j in range(0,len(self.obstacles_x),6): if (abs(pc_np_transformed[i][0] - self.obstacles_x[j]) < 0.15) and (abs(pc_np_transformed[i][1] - self.obstacles_y[j]) < 0.15): matches += 1 # self.br.sendTransform((pc_np_transformed[i][0], pc_np_transformed[i][1], 0), # tf.transformations.quaternion_from_euler(0, 0, 0), # rospy.Time.now(), # "match_" + str(i), # "map") break print(matches) #print(self.obstacles_x, self.obstacles_y) now2 = rospy.Time.now() print("rechenzeit: ", (now2 - now)* 0.000001) self.ts_old = now
class baseScan: def __init__(self, verbose=False): self.rangeData = LaserScan() self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.callback_laser) rospy.sleep(1) self.listener = tf.TransformListener() self.laser_projector = LaserProjection() self.pc = [] self.leg1 = [] self.leg2 = [] self.br = tf.TransformBroadcaster() self.rate = rospy.Rate(10.0) self.rate_measurements = rospy.Rate(0.5) self.priorOri_in_base = [] self.priorLeft_in_base = [] self.priorRight_in_base = [] self.odomL = [] self.odomR = [] self.priorAvailable = False self.newObsWeight = 0.2 self.offsetXY = [0.0, 0.0] self.binOffset = 0.02 #self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10) self.reCalibrationCount = 4 self.tolerance = 0.1 self.updateRounds = 100 self.asyncRate = 20 self.limitInitX = True self.xLimit = 0.0 self.rp = rospkg.RosPack() self.writeFile = False self.calculateTF = False self.sendTF = False self._ready = False self.radius_th_leg = 0.1 self.n_samples = 5 self.sleep_between_scans = 2 while not rospy.is_shutdown(): try: self._shelfHeight = rospy.get_param("/apc/shelf_calibration/height") self._shelfRoll = rospy.get_param("/apc/shelf_calibration/roll") self._shelfPitch = rospy.get_param("/apc/shelf_calibration/pitch") break except: rospy.sleep(1.0) continue self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty, self.start_srv_callback) def start_srv_callback(self, req): while not self._ready: rospy.logwarn('Shelf published waiting for scan callback') rospy.sleep(1.0) rospy.loginfo('[shelf_publisher]: starting shelf publisher') self.calculateTF = True return EmptyResponse() def callback_laser(self, data): self._ready = True self.rangeData = data def getCloud(self): cloud2 = self.laser_projector.projectLaser(self.rangeData) xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z")) self.pc = [] while not rospy.is_shutdown(): try: self.pc.append(xyz.next()) except: break return self.pc def findLegsOnce(self): pc = self.getCloud() x = [] y = [] for i in range(len(pc)): x.append(pc[i][0]) y.append(pc[i][1]) radius = [] if self.limitInitX: y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit] x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit] for i in range(len(x)): radius.append(math.sqrt(x[i]**2 + y[i]**2)) n = radius.index(min(radius)) x1 = [x[i] for i in range(len(x)) if y[i] > 0.0] y1 = [y[i] for i in range(len(y)) if y[i] > 0.0] radius2 = [] x2 = [x[i] for i in range(len(x)) if y[i] < 0.0] y2 = [y[i] for i in range(len(y)) if y[i] < 0.0] for i in range(len(x2)): radius2.append(math.sqrt(x2[i]**2 + y2[i]**2)) n2 = radius2.index(min(radius2)) leg1_p = [x[n], y[n]] leg2_p = [x2[n2], y2[n2]] x1_avg = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) < self.radius_th_leg] y1_avg = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) < self.radius_th_leg] x2_avg = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n2])**2 + (y[i]-y[n2])**2 ) < self.radius_th_leg] y2_avg = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n2])**2 + (y[i]-y[n2])**2 ) < self.radius_th_leg] leg1 = [numpy.mean(x1) + self.offsetXY[0], numpy.mean(y1) + self.offsetXY[1]] leg2 = [numpy.mean(x2) + self.offsetXY[0], numpy.mean(y2) + self.offsetXY[1]] #leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]] #leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]] return [leg1, leg2] # left, right def findLegs(self): leg1_x = numpy.zeros(self.n_samples) leg1_y = numpy.zeros(self.n_samples) leg2_x = numpy.zeros(self.n_samples) leg2_y = numpy.zeros(self.n_samples) for i in range(0, self.n_samples): rospy.loginfo('taking laser scan: ') rospy.loginfo(i) legs_sample = self.findLegsOnce() leg1_x[i] = legs_sample[0][0] leg1_y[i] = legs_sample[0][1] leg2_x[i] = legs_sample[1][0] leg2_y[i] = legs_sample[1][1] time.sleep(self.sleep_between_scans) #self.rate_measurements.sleep() leg1_m = [numpy.mean(leg1_x), numpy.mean(leg1_y)] leg2_m = [numpy.mean(leg2_x), numpy.mean(leg2_y)] rospy.loginfo('done scan') rospy.loginfo(leg1_m) rospy.loginfo(leg2_m) legs = [leg1_m, leg2_m] if legs[0][1] < legs[1][1]: legs[0], legs[1] = legs[1], legs[0] self.leg1 = legs[0] self.leg2 = legs[1] return [self.leg1, self.leg2] # left, right def getShelfFrame(self): legs = self.findLegs() ori_x = (legs[0][0] + legs[1][0]) / 2. ori_y = (legs[0][1] + legs[1][1]) / 2. left_leg = legs[0] if legs[0][1] < legs[1][1]: left_leg = legs[1] rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y) return [ori_x, ori_y], rotAngle, legs def tf2PoseStamped(self, xy, ori): shelfPoseMsg = PoseStamped() shelfPoseMsg.pose.position.x = xy[0] shelfPoseMsg.pose.position.y = xy[1] shelfPoseMsg.pose.position.z = 0.0 shelfPoseMsg.pose.orientation.x = ori[0] shelfPoseMsg.pose.orientation.y = ori[1] shelfPoseMsg.pose.orientation.z = ori[2] shelfPoseMsg.pose.orientation.w = ori[3] shelfPoseMsg.header.frame_id = 'base' shelfPoseMsg.header.stamp = rospy.Time.now() return shelfPoseMsg def estimateShelfFrame(self): while not rospy.is_shutdown(): self.rate.sleep() t_init = rospy.Time.now() if self.calculateTF: try: shelfOri, shelfRot, legs = self.getShelfFrame() except: rospy.logerr("Shelf calibration is not getting points") continue self.calculateTF = False self.sendTF = True self.writeFile = True if self.sendTF: # F1 = kdl.Frame(kdl.Rotation.RPY(0, 0, shelfRot), # kdl.Vector(shelfOri[0], shelfOri[0], self._shelfHeight)) # F2 = kdl.Frame(kdl.Rotation.RPY(self._shelfRoll, self._shelfPitch, 0), kdl.Vector(0,0,0)) # F3 = F1*F2 # position_xyz3 = F3.p # orientation_xyzw3 = F3.M.GetQuaternion() # self.br.sendTransform((position_xyz3[0], position_xyz3[1], position_xyz3[2]), # orientation_xyzw3, # rospy.Time.now(), # "/shelf_front", # child # "/base" # parent # ) self.br.sendTransform((shelfOri[0], shelfOri[1], self._shelfHeight), tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot), rospy.Time.now(), "/shelf_front", # child "/base" # parent ) self.br.sendTransform((legs[0][0], legs[0][1], self._shelfHeight), \ tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot), rospy.Time.now(), "/left_leg", # child "/base" # parent ) self.br.sendTransform((legs[1][0], legs[1][1], self._shelfHeight), \ tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot), rospy.Time.now(), "/right_leg", # child "/base" # parent ) #self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot))) if self.writeFile: file_path = self.rp.get_path('calibration_data') f = open(file_path + "/extrinsics/shelf_front.txt", 'w') f.write(str(shelfOri[0])+'\t') f.write(str(shelfOri[1])+'\t') f.write(str(0.0)+'\t') quaternion_shelf = tf.transformations.quaternion_from_euler(0, 0, shelfRot) f.write(str(quaternion_shelf[0])+'\t') f.write(str(quaternion_shelf[1])+'\t') f.write(str(quaternion_shelf[2])+'\t') f.write(str(quaternion_shelf[3])+'\n') f.close() self.writeFile = False
class mbotSimpleBehavior(object): ''' Exposes a behavior for the pioneer robot so that moves forward until it has an obstacle at 1.0 m then stops rotates for some time to the right and resumes motion. ''' def __init__(self): ''' Class constructor: will get executed at the moment of object creation ''' self.odom_msg_received = False self.laser_msg_received = False self.map_msg_received = False # register node in ROS network rospy.init_node('pioneer_smart_behavior', anonymous=False) self.INITIALPOSE_x = -1.39379392735 self.INITIALPOSE_y = -1.33716776885 self.INITIALPOSE_teta = 0 # send map as point cloud self.publish_map = rospy.Publisher("/wall_map", pc2, queue_size=1) # subscribe our laser point cloud self.laserProjection = LaserProjection() # puclish pointcloudlaser self.pcPub = rospy.Publisher("/laserPointCloud", pc2, queue_size=1) # self.matchinginfo = rospy.Publisher("/laserPointCloud", pc2, queue_size=1) # print message in terminal rospy.loginfo('Pioneer simple behavior started !') # subscribe to pioneer laser scanner topic rospy.Subscriber("/scan_combined", LaserScan, self.laserCallback, queue_size=1) # subscribe to pioneer odometry topic rospy.Subscriber("/odom", Odometry, self.odomCallback, queue_size=1) # subscribe to mbot map topic rospy.Subscriber("/map", OccupancyGrid, self.mapCallback, queue_size=1) rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.amclposeCallback, queue_size=1) # setup publisher to later on move the pioneer base #self.pub_cmd_vel = rospy.Publisher('robot_0/cmd_vel', Twist, queue_size=1) # define member variable and initialize with a big value # it will store the distance from the robot to the walls #self.distance = 10.0 # Last two odometry readings self.initpose_icp = np.array([[ np.cos(self.INITIALPOSE_teta), -np.sin(self.INITIALPOSE_teta), self.INITIALPOSE_x ], [ np.sin(self.INITIALPOSE_teta), np.cos(self.INITIALPOSE_teta), self.INITIALPOSE_y ], [0, 0, 1]]) self.varicp = 0 self.amcl = np.array([[self.INITIALPOSE_x], [self.INITIALPOSE_y]]) self.currentmsgodom = [ self.INITIALPOSE_x, self.INITIALPOSE_y, self.INITIALPOSE_teta ] self.currentmsgamcl = [ self.INITIALPOSE_x, self.INITIALPOSE_y, self.INITIALPOSE_teta ] self.odom = { 'x': self.INITIALPOSE_x, 'y': self.INITIALPOSE_y, 'teta': self.INITIALPOSE_teta, 'x_1': self.INITIALPOSE_x, 'y_1': self.INITIALPOSE_y, 'teta_1': self.INITIALPOSE_teta } self.odom_read = { 'x': self.INITIALPOSE_x, 'y': self.INITIALPOSE_y, 'teta': self.INITIALPOSE_teta, 'x_1': self.INITIALPOSE_x, 'y_1': self.INITIALPOSE_y, 'teta_1': self.INITIALPOSE_teta } # EKF pose predcition self.pose_prediction = { 'x': self.INITIALPOSE_x, 'y': self.INITIALPOSE_y, 'teta': self.INITIALPOSE_teta } self.init_pose_prediction = { 'x': self.INITIALPOSE_x, 'y': self.INITIALPOSE_y, 'teta': self.INITIALPOSE_teta } self.pose_estimation = { 'x': self.INITIALPOSE_x, 'y': self.INITIALPOSE_y, 'teta': self.INITIALPOSE_teta } # self.pose_prediction = {'x': 0,'y': 0, 'teta': 0} # Jacobian matrix g self.matrix_G = np.identity(3) self.matrix_H = np.identity(3) self.matrix_R = np.array([[0.9, 0, 0], [0, 1.1, 0], [0, 0, 100]]) # covariance of noise observation matrix self.matrix_Q = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # Covariance of instance t-1 #self.cov_matrix = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 0.5]]) # covariance of instance t (actual) self.prev_cov = np.identity(3) self.act_cov = np.identity(3) # predicted covariance self.pred_cov = np.identity(3) self.pred_update = np.array([0, 0, 0], dtype=float) self.z_measurements = np.array([0, 0, 0], dtype=float) self.map = [] self.width = 0 self.height = 0 self.resolution = 0 self.cloudpointmap = [] self.currentlaserdata = [] self.usingpointlaser = [] # defines the range threshold bellow which the robot should stop moving foward and rotate insteadnp.array([[np.cos(0),-np.sin(0),-1.37660027342],[np.sin(0),np.cos(0.003139031004),4.01820473614],[0,0,1]]) # if rospy.has_param('distance_threshold'): # # retrieves the threshold from the parameter server in the case where the parameter exists # self.distance_threshold = rospy.get_param('distance_threshold') # else: # self.distance_threshold = 1.0 def odomCallback(self, msg): ''' This function gets executed everytime a odomotry reading msg is received on the topic: /robot_0/odometry ''' self.odom_msg_received = True # print("ANGULO", self.quaternion_to_euler(msg)) # print("x", msg.pose.pose.position.x, "y", msg.pose.pose.position.y) self.currentmsgodom[ 0] = msg.pose.pose.position.x + self.init_pose_prediction[ 'x'] - 1.3108866698979635 self.currentmsgodom[ 1] = msg.pose.pose.position.y + self.init_pose_prediction[ 'y'] - 0.19804321874743439 self.currentmsgodom[2] = self.quaternion_to_euler( msg) + self.init_pose_prediction['teta'] + 2.401482407814359 def amclposeCallback(self, msg): #print('amcl pose x= ', msg.pose.pose.position.x, 'amcl pose y= ', msg.pose.pose.position.y, "amcl teta = ", self.quaternion_to_euler(msg)) self.amcl = np.hstack((self.amcl, np.array([[msg.pose.pose.position.x], [msg.pose.pose.position.y]]))) self.currentmsgamcl[0] = msg.pose.pose.position.x self.currentmsgamcl[1] = msg.pose.pose.position.y self.currentmsgamcl[2] = np.remainder(self.quaternion_to_euler(msg), 2 * np.pi) #print("amcl angulo", self.currentmsgamcl[2]) def quaternion_to_euler(self, msg): quaternion = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(quaternion) return yaw def laserCallback(self, msg): ''' This function gets executed everytime a laser scanner msg is received on the topic: /scan_combined ''' self.laser_msg_received = True temporaria = [] cloud_out = self.laserProjection.projectLaser(msg) for p in pcmapping.read_points(cloud_out, field_names=("x", "y", "z"), skip_nans=True): temporaria.append([p[0], p[1]]) self.currentlaserdata = temporaria #print("laser len", len(self.currentlaserdata)) #print("point _ point_cloud_laser=", self.currentlaserdata) def mapCallback(self, msg): self.map = msg.data self.width = msg.info.width self.height = msg.info.height self.resolution = msg.info.resolution i = 0 j = 0 for i in range(self.width): for j in range(self.height): if (self.map[i + j * self.width] > 0.0): self.cloudpointmap.append([ self.resolution * i + msg.info.origin.position.x, self.resolution * j + msg.info.origin.position.y ]) self.map_msg_received = True def ekf_predict(self): ''' perform one EKF predict ''' self.odom['x'] = self.currentmsgodom[0] self.odom['y'] = self.currentmsgodom[1] self.odom['teta'] = self.currentmsgodom[2] dx = self.odom['x'] - self.odom['x_1'] dy = self.odom['y'] - self.odom['y_1'] dtheta = self.odom['teta'] - self.odom['teta_1'] delta_trans = math.sqrt(dx**2 + dy**2) delta_rot1 = math.atan2(dy, dx) - self.odom['teta_1'] delta_rot2 = dtheta - delta_rot1 #self.prev_cov = self.act_cov self.pose_prediction['x'] = self.pose_estimation[ 'x'] + delta_trans * math.cos(self.pose_estimation['teta'] + delta_rot1) self.pose_prediction['y'] = self.pose_estimation[ 'y'] + delta_trans * math.sin(self.pose_estimation['teta'] + delta_rot1) self.pose_prediction[ 'teta'] = self.pose_estimation['teta'] + delta_rot1 + delta_rot2 self.pose_prediction['teta'] = np.remainder( self.pose_estimation['teta'], 2 * np.pi) #print(self.pose_prediction) #print("pose_prediction x =" ,self.pose_prediction['x'],"pose_prediction y =" ,self.pose_prediction['y'],"pose_prediction theta=" ,self.pose_prediction['teta'], "\n", "odom x =", self.odom['x'], "odom y =", self.odom['y'], "odom theta =", self.odom['teta']) self.matrix_G = np.array( [[ 1, 0, -delta_trans * math.sin(self.pose_estimation['teta'] + delta_rot1) ], [ 0, 1, delta_trans * math.cos(self.pose_estimation['teta'] + delta_rot1) ], [0, 0, 1]]) self.pred_cov = np.dot( self.matrix_G, np.dot(self.act_cov, self.matrix_G.transpose())) + self.matrix_R self.odom['x_1'] = self.odom['x'] self.odom['y_1'] = self.odom['y'] self.odom['teta_1'] = self.odom['teta'] def ekf_update(self): pose_estimation_update = np.array([0, 0, 0], dtype=float) self.pred_update[0] = self.pose_prediction['x'] self.pred_update[1] = self.pose_prediction['y'] self.pred_update[2] = self.pose_prediction['teta'] #print(self.pred_update) #print("pose_prediction x =" ,self.pose_prediction['x'],"pose_prediction y =" ,self.pose_prediction['y'],"pose_prediction theta=" ,self.pose_prediction['teta'], "\n", "odom x =", self.odom['x'], "odom y =", self.odom['y'], "odom theta =", self.odom['teta']) #Este Z deveria vir do ICP z = np.array([[self.currentmsgamcl[0]], [self.currentmsgamcl[1]], [self.currentmsgamcl[2]]]) #kalman gain k_gain = (self.pred_cov.dot(self.matrix_H.transpose())).dot( LA.inv((self.matrix_H.dot(self.pred_cov) ).dot(self.matrix_H.transpose()) + self.matrix_Q)) self.updatemeasurements_icp() pose_estimation_update = self.pred_update.reshape(3, 1) + k_gain.dot( (self.z_measurements.reshape(3, 1) - self.pred_update.reshape(3, 1))) #print(k_gain) #print(pose_estimation_update) #print(pose_estimation_update) self.pose_estimation['x'] = pose_estimation_update[0][0] self.pose_estimation['y'] = pose_estimation_update[1][0] self.pose_estimation['teta'] = pose_estimation_update[2][0] self.act_cov = (np.identity(3) - k_gain.dot(self.matrix_H)).dot( self.pred_cov) def updatemeasurements_icp(self): #print("pointcloudlaser" , self.point_cloud_laser) # x_max_pointcloud_laser = max(self.point_cloud_laser,key=itemgetter(1))[0] # y_max_pointcloud_laser = max(self.point_cloud_laser,key=itemgetter(1))[1] # x_min_pointcloud_laser = min(self.point_cloud_laser,key=itemgetter(1))[0] # y_min_pointcloud_laser = min(self.point_cloud_laser,key=itemgetter(1))[1] # # # resizedmap = [x for x in self.cloudpointmap if (x_min_pointcloud_laser - 0.5 <= x[0] <= x_max_pointcloud_laser +0.5 and y_min_pointcloud_laser - 0.5 <= x[1] <= y_max_pointcloud_laser +0.5) ] self.usingpointlaser = self.currentlaserdata # x_max_pointcloud_laser = max(self.usingpointlaser,key=itemgetter(1))[0] # y_max_pointcloud_laser = max(self.usingpointlaser,key=itemgetter(1))[1] # x_min_pointcloud_laser = min(self.usingpointlaser,key=itemgetter(1))[0] # y_min_pointcloud_laser = min(self.usingpointlaser,key=itemgetter(1))[1] if (self.varicp == 0): # resizedmap = [x for x in self.cloudpointmap if (-1.43967161853 + x_min_pointcloud_laser -1.2 <= x[0] <= -1.43967161853 + x_max_pointcloud_laser+1.2 or 4.02608497565 + y_min_pointcloud_laser - 1.2 <= x[1] <= 4.02608497565 + y_max_pointcloud_laser +1.2 ) ] T, distances, i = icp( np.array(self.usingpointlaser), np.array(self.cloudpointmap), np.array([[ np.cos(self.INITIALPOSE_teta), -np.sin(self.INITIALPOSE_teta), self.INITIALPOSE_x ], [ np.sin(self.INITIALPOSE_teta), np.cos(self.INITIALPOSE_teta), self.INITIALPOSE_y ], [0, 0, 1]])) self.z_measurements = np.array([[T[0][2]], [T[1][2]], [np.arctan2(T[1][0], T[0][0])]]) #print(T) #print("foi aqui") #print("mapa ",np.shape( np.array(self.cloudpointmap))) #print("laser", np.shape( np.array(self.usingpointlaser))) self.varicp = 1 else: #resizedmap = [x for x in self.cloudpointmap if (self.pred_update[0] + x_min_pointcloud_laser <= x[0] <= self.pred_update[0] + x_max_pointcloud_laser or self.pred_update[1] + y_min_pointcloud_laser <= x[1] <= self.pred_update[1] + y_max_pointcloud_laser ) ] #print("mapa ",np.shape( np.array(self.cloudpointmap))) #print("laser", np.shape( np.array(self.usingpointlaser))) T, distances, i = icp( np.array(self.usingpointlaser), np.array(self.cloudpointmap), np.array([[ np.cos(self.pred_update[2]), -np.sin(self.pred_update[2]), self.pred_update[0] ], [ np.sin(self.pred_update[2]), np.cos(self.pred_update[2]), self.pred_update[1] ], [0, 0, 1]])) #print(T) self.z_measurements[0] = T[0][2] self.z_measurements[1] = T[1][2] self.z_measurements[2] = np.arctan2(T[1][0], T[0][0]) #print(T) def run_behavior(self): pred = np.array([[self.pose_prediction['x']], [self.pose_prediction['y']], [self.pose_prediction['teta']]]) odometry = np.array([[self.odom_read['x_1']], [self.odom_read['y_1']], [self.odom_read['teta_1']]]) error = np.array([[0], [0], [0]]) while not rospy.is_shutdown(): # base needs this msg to be published constantly for the robot to keep moving so we publish in a loop # while the distance from the robot to the walls is bigger than the defined threshold keep moving forward ##if self.distance > self.distance_threshold: ## self.move_forward() ##else: # rotate for a certain angle ## self.rotate_right() # EKF - Predict r = rospy.Rate(5) while len(self.currentlaserdata ) == 0 or self.map_msg_received == False: continue #print("MAIN LASER", self.point_cloud_laser) if self.odom_msg_received == True: #rospy.loginfo('odom msg received!') self.odom_msg_received = False if self.laser_msg_received == True: #rospy.loginfo('laser msg received!') self.laser_msg_received == True total_time = 0 self.ekf_predict() start = time.time() self.ekf_update() end = time.time() #print(end-start) arr = np.array([[self.pose_estimation['x']], [self.pose_estimation['y']], [self.pose_estimation['teta']]]) #print("PRED=", pred) #print ("ARR = ", arr) pred = np.hstack((pred, arr)) error_array = np.array( [[self.pose_estimation['x'] - self.currentmsgamcl[0]], [self.pose_estimation['y'] - self.currentmsgamcl[1]], [self.pose_prediction['teta'] - self.currentmsgamcl[2]]]) error = np.hstack((error, error_array)) with open('pred.pkl', 'wb') as f: pickle.dump(pred, f) with open('amcl.pkl', 'wb') as f2: pickle.dump(self.amcl, f2) arr2 = np.array([[self.odom['x_1']], [self.odom['y_1']], [self.odom['teta_1']]]) odometry = np.hstack((odometry, arr2)) #plt.plot(odometry[0, :].flatten(), # odometry[1, :].flatten(), "-r") with open('rosbagbom.pkl', 'wb') as f: pickle.dump((error[0, :], error[1, :], error[2, :]), f) plt.plot(pred[0, :].flatten(), pred[1, :].flatten(), "-b") plt.plot(self.amcl[0, :].flatten(), self.amcl[1, :].flatten(), "-g") # plt.plot(error[0, :].flatten(), "-r") # plt.plot(error[1, :].flatten(), '-b') # sleep for a small amount of time plt.grid(True) plt.pause(0.00001) r.sleep()
class lineScan: def __init__(self, verbose=False): self.rangeData = LaserScan() self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback) self.listener = tf.TransformListener() self.laser_projector = LaserProjection() self.pc = [] self.leg1 = [] self.leg2 = [] self.br = tf.TransformBroadcaster() self.rate = rospy.Rate(100.0) self.calibrated = False self.reCalibration = False self.priorOri = [] self.priorLeft = [] self.priorRight = [] self.odomL = [] self.odomR = [] self.priorAvailable = False self.newObsWeight = 0.1 self.offsetXY = [-0.044, 0] def callback(self,data): self.rangeData = data def refreshRangeData(self): self.rangeData = LaserScan() # flush while len(self.rangeData.ranges) == 0: rospy.sleep(0.04) def getStatus(self): return self.calibrated def getCloud(self): self.refreshRangeData() cloud2 = self.laser_projector.projectLaser(self.rangeData) xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z")) self.pc = [] while True: try: self.pc.append(xyz.next()) except: break return self.pc def segmentLength(self, m, i1, i2): v1 = m[i1] v2 = m[i2] return np.linalg.norm(v1-v2) def cloud2Input(self): while not rospy.is_shutdown(): plt.clf() pc = self.getCloud() m = np.asarray(pc) segs = [] r = split_and_merge(m, 0.1) for i in range(1, len(r)): if r[i] - r[i-1] < 20: continue l = self.segmentLength(m, r[i], r[i-1]) if l > 0.8 and l < 0.9: print 'found' print l segs.append([r[i], r[i-1]]) # Plot found lines plt.plot(m[:, 0], m[:, 1], '.r') plt.plot(m[r, 0], m[r, 1], '-') plt.plot(m[r, 0], m[r, 1], '+') for s in segs: plt.plot(m[s, 0], m[s, 1], 'g-+', linewidth=5) plt.axis('equal') plt.draw() plt.title("Laser scan") plt.show(block=False)
class occupancy(): def __init__(self): ## SETUP ROS NODES rospy.init_node('occupancy', anonymous=False) rospy.loginfo("To stop occupancy CTRL + C") rospy.on_shutdown(self.shutdown) self.r = rospy.Rate(10) # 10hz # SETUP MAP self.range = 10.0 # meters self.pixels = 36 self.map = np.zeros((self.pixels, self.pixels)) self.pixwid = self.pixels / self.range print("pixel width", self.pixwid) self.offset = np.asarray([1.0, 5.0]) # control where 0,0 will map to # ~ x = np.linspace(0-self.offsets[0],self.range-self.offsets[0], self.pixels) # ~ y = np.linspace(0-self.offsets[1],self.range-self.offsets[1], self.pixels) # ~ xx,yy = np.meshgrid(x,y) self.points, self.edges = self.get_graph() self.heuristic = np.ones(len(self.points)) # MAP WORLD POINT TO MAP COORDINATE self.xmap = lambda a: np.minimum( np.maximum(0, ((a + self.offset[0]) * self.pixwid).astype(int)), self.pixels - 1) self.ymap = lambda a: np.minimum( np.maximum(0, ((a + self.offset[1]) * self.pixwid).astype(int)), self.pixels - 1) self.xinverse = lambda a: ((a + 0.0) / self.pixwid) - self.offset[0] self.yinverse = lambda a: ((a + 0.0) / self.pixwid) - self.offset[1] self.center = np.asarray([self.xmap(0), self.ymap(0)]) wy = range(0, self.pixels) # FOR RAYCASTING (head of the 'ray') behind = np.vstack([np.zeros_like(wy), wy]).transpose() right = np.vstack([wy, np.zeros_like(wy)]).transpose() left = np.vstack([wy, (self.pixels - 1) * np.ones_like(wy)]).transpose() front = np.vstack([(self.pixels - 1) * np.ones_like(wy), wy]).transpose() self.wp = np.vstack([left, right, front]) #SUBSCRIBER #rospy.Subscriber('/converted_pc', PointCloud2, self.parse_command) self.laser_projector = LaserProjection() self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.on_scan) # ~ target_string = "/new_target" target_string = "/tracked_target" self.target_sub = rospy.Subscriber(target_string, Marker, self.process_target) # Placeholder # ~ self.target = np.asarray( [28, 16] ) # ~ self.start = np.ravel_multi_index(self.center, (self.pixels,self.pixels) ) # ~ self.goal = np.ravel_multi_index(self.target, (self.pixels,self.pixels) ) # PUBLISHER # self.pub = rospy.Publisher('/go_nogo', ObjectVector, queue_size=1) # ~ self.pub = rospy.Publisher('/net_input', Float32MultiArray, queue_size=1) self.pub_path = rospy.Publisher('/path_to_target', Marker, queue_size=1) # ~ self.pub_new_target = rospy.Publisher('/new_target', Point, queue_size=1) # ~ self.pub_new_target = rospy.Publisher('/new_target', Marker, queue_size=1) rospy.sleep(0.01) rospy.loginfo("Waiting for topics...") try: rospy.wait_for_message("/scan", LaserScan, timeout=10.0) rospy.wait_for_message(target_string, Marker, timeout=10.0) except rospy.ROSException as e: rospy.logerr("Timeout while waiting for topics!") raise e print("Node started") while not rospy.is_shutdown(): self.r.sleep() self.update_map() # TODO: get heuristic self.start = np.ravel_multi_index(self.center, (self.pixels, self.pixels)) self.goal = np.ravel_multi_index(self.target, (self.pixels, self.pixels)) path = self.find_path(self.start, self.goal, heuristic=self.heuristic) x, y = np.unravel_index(path, (self.pixels, self.pixels)) self.map[x, y] = 2 if VIS_MAP: # ~ fig = plt.figure(1) # ~ plt.clf() # ~ #plt.plot(self.xy[:,0], self.xy[:,1], '.') # ~ plt.plot(self.xmap(self.xy[:,0]), self.xmap(self.xy[:,1]), '.') fig = plt.figure(2) plt.clf() plt.imshow(self.map) plt.show(block=False) plt.pause(0.0000000001) path_message = self.get_path_message(x, y) self.pub_path.publish(path_message) def shutdown(self): rospy.loginfo("Stop occupancy") # rospy.sleep(1) return 0 def process_target(self, data): x = self.xmap(data.pose.position.x) y = self.ymap(data.pose.position.y) # ~ print("points", x,y) self.target = np.asarray([x, y]) def on_scan(self, scan): cloud = self.laser_projector.projectLaser(scan) gen = pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z")) self.xyz_generator = gen self.xyz = np.asarray([[p[0], p[1], p[2]] for p in gen]) def update_map(self): # ~ https://www.redblobgames.com/articles/visibility/ # ~ 1. Calculate the angles where walls begin or end. # ~ 2. Cast a ray from the center along each angle. # ~ 3. Fill in the triangles generated by those rays. self.xy = deepcopy(self.xyz[:, :2]) xs = self.xmap(self.xy[:, 0]) ys = self.ymap(self.xy[:, 1]) # ~ print(self.xy[:,1]) # ~ print(ys) self.map = 0.5 * np.ones((self.pixels, self.pixels)) self.map[xs, ys] = 1 self.map[self.center[0], self.center[1]] = 0 # TODO: raytracing self.map = raycast(self.map, self.center, self.wp) self.map = np.maximum(self.map, gaussian_filter(self.map, sigma=2)) # https://raw.githubusercontent.com/pfirsich/Ray-casting-test/master/main.lua def get_graph(self): # GET GRAPH CORRESPONDING TO OCCUPANCY MAP # GET POINTS [time, forward, lateral] X = range(self.pixels) Y = range(self.pixels) Y, X = np.meshgrid(Y, X) # X,Y doesn't ravel correctly points = np.vstack([X.ravel(), Y.ravel()]).transpose() print("points shape", np.shape(points)) # GET EDGES edges = [] cost_edges = [] # [t,x,y]-> [t+1,x:x+2,y-1:y+1] # x can only take big steps (x+0 x+2) early on for p in range(len(points)): x, y = np.unravel_index( p, (self.pixels, self.pixels)) # self.x_num, self.y_num) ) # # CONNECTIVITY if x == 0: xs = [x, x + 1] elif x == self.pixels - 1: xs = [x - 1, x] else: xs = [x - 1, x, x + 1] if y == 0: ys = [y, y + 1] elif y == self.pixels - 1: ys = [y - 1, y] else: ys = [y - 1, y, y + 1] xgrid, ygrid = np.meshgrid(xs, ys) xlist = xgrid.ravel() ylist = ygrid.ravel() arr = np.array([xlist, ylist]).astype(int) inds = np.ravel_multi_index( arr, (self.pixels, self.pixels)) # order='F') edges.append(inds) return points, edges def find_path(self, start, goal, heuristic=0): # DIJKSTRA's ALGORITHM / A* node_num = len(self.points) nodedist = np.inf * np.ones(node_num) nodedist[start] = 0 visited = np.zeros(node_num) came_from = np.zeros(node_num) big_num = 10000 v = start for i in range(node_num): # STARTING NODE (working backwards) costs = nodedist + 10 * big_num * visited + heuristic v = np.argmin(costs) # unvisited and closest # print('v', v, np.min(costs)) if v == goal: break visited[v] = 1 # FIND NEIGHBORS # k = 30 + 1 # 1st is always self # dist, ind = self.tree.query([self.points[v,:]], k=k) # ind = ind[0] # dist = dist[0] v_neighbor = self.edges[v] # dist = self.weights[v] # WE USE THE DESTINATION COST (from occupancy map) AS THE EDGE COST # COST: cost to current + current to new cost for p in range(np.size(v_neighbor)): # ~ line = np.asarray(np.hstack((self.points[v,:],self.points[v_neighbor[p],:]))) new_dist = nodedist[v] + self.map.ravel()[v_neighbor[p]] # print('...', p, new_dist) if new_dist < nodedist[v_neighbor[p]]: nodedist[v_neighbor[p]] = new_dist came_from[v_neighbor[p]] = v # steps+=1 if v != goal: #>max_steps: print("No path found") return [] # print("costs",nodedist) self.node_dist = nodedist # RECOVER PATH self.path = [] self.path.append(v) # FIND NEXT SUBGOAL # if self.subgoal!=len(self.points)-1: ################################### if not finished self.subgoal = start while came_from[v] != self.subgoal: v = int(came_from[v]) self.path.append(v) self.path.append(self.subgoal) self.subgoal = v return self.path def get_path_message(self, x, y): X = self.xinverse(x) Y = self.yinverse(y) m = default_marker() m.lifetime = rospy.Duration(0.1) for i in range(len(X)): m.points.append(Point(x=X[i], y=Y[i], z=0)) return m
def test_project_laser(self): tolerance = 6 # decimal places projector = LaserProjection() ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0] intensities = np.arange(1.0, 6.0).tolist() min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0]) max_angles = -min_angles angle_increments = np.pi / np.array([180., 360., 720.]) scan_times = [rospy.Duration(1./i) for i in [40, 20]] for range_val, intensity_val, \ angle_min, angle_max, angle_increment, scan_time in \ product(ranges, intensities, min_angles, max_angles, angle_increments, scan_times): try: scan = build_constant_scan( range_val, intensity_val, angle_min, angle_max, angle_increment, scan_time) except BuildScanException: if (angle_max - angle_min)/angle_increment > 0: self.fail() cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INDEX) self.assertEquals(len(cloud_out.fields), 4, "PointCloud2 with channel INDEX: fields size != 4") cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INTENSITY) self.assertEquals(len(cloud_out.fields), 4, "PointCloud2 with channel INDEX: fields size != 4") cloud_out = projector.projectLaser(scan, -1.0) self.assertEquals(len(cloud_out.fields), 5, "PointCloud2 with channel INDEX: fields size != 5") cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX) self.assertEquals(len(cloud_out.fields), 5, "PointCloud2 with channel INDEX: fields size != 5") cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX | LaserProjection.ChannelOption.DISTANCE) self.assertEquals(len(cloud_out.fields), 6, "PointCloud2 with channel INDEX: fields size != 6") cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX | LaserProjection.ChannelOption.DISTANCE | LaserProjection.ChannelOption.TIMESTAMP) self.assertEquals(len(cloud_out.fields), 7, "PointCloud2 with channel INDEX: fields size != 7") valid_points = 0 for i in range(len(scan.ranges)): ri = scan.ranges[i] if (PROJECTION_TEST_RANGE_MIN <= ri and ri <= PROJECTION_TEST_RANGE_MAX): valid_points += 1 self.assertEqual(valid_points, cloud_out.width, "Valid points != PointCloud2 width") idx_x = idx_y = idx_z = 0 idx_intensity = idx_index = 0 idx_distance = idx_stamps = 0 i = 0 for f in cloud_out.fields: if f.name == "x": idx_x = i elif f.name == "y": idx_y = i elif f.name == "z": idx_z = i elif f.name == "intensity": idx_intensity = i elif f.name == "index": idx_index = i elif f.name == "distances": idx_distance = i elif f.name == "stamps": idx_stamps = i i += 1 i = 0 for point in pc2.read_points(cloud_out): ri = scan.ranges[i] ai = scan.angle_min + i * scan.angle_increment self.assertAlmostEqual(point[idx_x], ri * np.cos(ai), tolerance, "x not equal") self.assertAlmostEqual(point[idx_y], ri * np.sin(ai), tolerance, "y not equal") self.assertAlmostEqual(point[idx_z], 0, tolerance, "z not equal") self.assertAlmostEqual(point[idx_intensity], scan.intensities[i], tolerance, "Intensity not equal") self.assertAlmostEqual(point[idx_index], i, tolerance, "Index not equal") self.assertAlmostEqual(point[idx_distance], ri, tolerance, "Distance not equal") self.assertAlmostEqual(point[idx_stamps], i * scan.time_increment, tolerance, "Timestamp not equal") i += 1
class baseScan: def __init__(self, verbose=False): self.rangeData = LaserScan() self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback) self.listener = tf.TransformListener() self.laser_projector = LaserProjection() self.pc = [] self.leg1 = [] self.leg2 = [] self.br = tf.TransformBroadcaster() self.rate = rospy.Rate(20.0) self.calibrated = False self.reCalibration = False self.priorOri_in_base_laser_link = [] # in base_laser_link frame self.priorLeft_in_base_laser_link = [] # in base_laser_link frame self.priorRight_in_base_laser_link = [] # in base_laser_link frame self.odomL = [] # in odom_combined frame self.odomR = [] # in odom_combined frame self.priorAvailable = False self.newObsWeight = 0.2 self.offsetXY = [-0.044, 0] self.binOffset = 0.02 self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped) self.reCalibrationCount = 4 self.tolerance = 0.1 self.updateRounds = 100 self.asyncRate = 20 self.limitInitX = True self.xLimit = 0.1 self.emergencyThreshold = 30 * 20 self.emergencyTimeWindow = 60 while not rospy.is_shutdown(): try: self.offsetXY = rospy.get_param('/base_scan/offset') break except: rospy.loginfo( '[shelf publisher]: waiting for base scan offset param') rospy.sleep(random.uniform(0, 1)) continue self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty, self.start_srv_callback) self._start = False # backup human supervised info for emergency rp = rospkg.RosPack() try: dict_fp = rp.get_path('amazon_challenge_grasping') except: rospy.logerr('[shelf_publisher]: emergency file not found!') sys.exit(1) self.fileName = dict_fp + '/config/shelf_emergency.dat' self.emergency = {} def start_srv_callback(self, req): rospy.loginfo('[shelf_publisher]: starting shelf publisher!') if self._start: rospy.logwarn('[shelf_publisher]: already started!!!!') self._start = True return EmptyResponse() def raw_input_with_timeout(prompt, timeout=1.0): print prompt, timer = threading.Timer(timeout, thread.interrupt_main) astring = None try: timer.start() astring = raw_input(prompt) except KeyboardInterrupt: pass timer.cancel() return astring def callback(self, data): self.rangeData = data def refreshRangeData(self): self.rangeData = LaserScan() # flush while len(self.rangeData.ranges) == 0 and not rospy.is_shutdown(): rospy.sleep(0.04) def getStatus(self): return self.calibrated def getCloud(self): # self.refreshRangeData() cloud2 = self.laser_projector.projectLaser(self.rangeData) xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z")) self.pc = [] while not rospy.is_shutdown(): try: self.pc.append(xyz.next()) except: break return self.pc def findLegsOnce(self): pc = self.getCloud() x = [] y = [] for i in range(len(pc)): x.append(pc[i][0]) y.append(pc[i][1]) radius = [] if self.calibrated or self.reCalibration: # use prior to find legs for i in range(len(x)): radius.append( math.sqrt( (x[i] - self.priorLeft_in_base_laser_link[0])**2 + (y[i] - self.priorLeft_in_base_laser_link[1])**2)) n1 = radius.index(min(radius)) radius = [] for i in range(len(x)): radius.append( math.sqrt( (x[i] - self.priorRight_in_base_laser_link[0])**2 + (y[i] - self.priorRight_in_base_laser_link[1])**2)) n2 = radius.index(min(radius)) leg1 = [x[n1], y[n1]] leg2 = [x[n2], y[n2]] else: # Assuming there is nothing between the robot and the shelf if self.limitInitX: y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit] x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit] for i in range(len(x)): radius.append(math.sqrt(x[i]**2 + y[i]**2)) n = radius.index(min(radius)) x2 = [ x[i] for i in range(len(x)) if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) > 0.7 ] y2 = [ y[i] for i in range(len(y)) if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) > 0.7 ] radius2 = [] for i in range(len(x2)): radius2.append(math.sqrt(x2[i]**2 + y2[i]**2)) n2 = radius2.index(min(radius2)) leg1 = [x[n], y[n]] leg2 = [x2[n2], y2[n2]] leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]] leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]] return [leg1, leg2] # left, right def findLegs(self): ''' accumulate new observations ''' L1x = 0 L1y = 0 L2x = 0 L2y = 0 legs = self.findLegsOnce() if legs[0][1] < legs[1][1]: legs[0], legs[1] = legs[1], legs[0] if self.calibrated: self.leg1[0] = self.leg1[0] * ( 1 - self.newObsWeight) + legs[0][0] * self.newObsWeight self.leg1[1] = self.leg1[1] * ( 1 - self.newObsWeight) + legs[0][1] * self.newObsWeight self.leg2[0] = self.leg2[0] * ( 1 - self.newObsWeight) + legs[1][0] * self.newObsWeight self.leg2[1] = self.leg2[1] * ( 1 - self.newObsWeight) + legs[1][1] * self.newObsWeight else: self.leg1 = legs[0] self.leg2 = legs[1] return [self.leg1, self.leg2] # left, right def getShelfFrame(self): # with respect to the frame of /base_scan legs = self.findLegs() ori_x = (legs[0][0] + legs[1][0]) / 2. ori_y = (legs[0][1] + legs[1][1]) / 2. left_leg = legs[0] if legs[0][1] < legs[1][1]: left_leg = legs[1] rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y) return [ori_x, ori_y], rotAngle, legs def tf2PoseStamped(self, xy, ori): shelfPoseMsg = PoseStamped() shelfPoseMsg.pose.position.x = xy[0] shelfPoseMsg.pose.position.y = xy[1] shelfPoseMsg.pose.position.z = 0.0 shelfPoseMsg.pose.orientation.x = ori[0] shelfPoseMsg.pose.orientation.y = ori[1] shelfPoseMsg.pose.orientation.z = ori[2] shelfPoseMsg.pose.orientation.w = ori[3] shelfPoseMsg.header.frame_id = 'base_laser_link' shelfPoseMsg.header.stamp = rospy.Time.now() return shelfPoseMsg def saveEmergency(self): with open(self.fileName, 'wb') as handle: pickle.dump(self.emergency, handle) def loadEmergency(self): with open(self.fileName, 'rb') as handle: self.emergency = pickle.load(handle) def publish2TF(self): answer = 'n' ask = True u = 0 shelfN = 0 recalibrateCount = 0 emergencyCount = 0 t_init = rospy.Time.now() while not rospy.is_shutdown(): self.rate.sleep() # check if human calibration is done shelfOri, shelfRot, legs = self.getShelfFrame() if self.reCalibration: u = 0 if not self.calibrated: self.br.sendTransform( (shelfOri[0], shelfOri[1], 0), tf.transformations.quaternion_from_euler(0, 0, shelfRot), rospy.Time.now(), "/shelf_frame", # child "/base_laser_link" # parent ) self.br.sendTransform((legs[0][0], legs[0][1], 0), \ tf.transformations.quaternion_from_euler(0, 0, shelfRot), \ rospy.Time.now(), \ "/left_leg", \ "/base_laser_link") self.br.sendTransform((legs[1][0], legs[1][1], 0), \ tf.transformations.quaternion_from_euler(0, 0, shelfRot), \ rospy.Time.now(), \ "/right_leg", \ "/base_laser_link") if self.priorAvailable: while not rospy.is_shutdown(): try: shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform( "/odom_combined", "/shelf_frame", rospy.Time(0)) self.priorLeft_in_base_laser_link, dummy = self.listener.lookupTransform( "/base_laser_link", "/odomL", rospy.Time(0)) self.priorRight_in_base_laser_link, dummy = self.listener.lookupTransform( "/base_laser_link", "/odomR", rospy.Time(0)) break except: continue else: try: shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform( "/odom_combined", "/shelf_frame", rospy.Time(0)) except Exception, e: # print e continue if self.reCalibration and math.sqrt( (shelf_in_odom[0] - self.priorOri_in_odom[0])**2 + (shelf_in_odom[1] - self.priorOri_in_odom[1])**2) <= self.tolerance: # rospy.sleep(2) recalibrateCount += 1 if recalibrateCount == self.reCalibrationCount: # take recalibration only if it's stable rospy.loginfo('reCalibrated!') recalibrateCount = 0 u = 0 while not rospy.is_shutdown( ): # make sure the odomL and odomR are updated try: ######## self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0)) ######## self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0)) ######## self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0)) self.calibrated = True self.reCalibration = False rospy.loginfo( "Prior origin in odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1])) break except: continue if not self.calibrated and ask: emergencyCount += 1 if emergencyCount == self.emergencyThreshold: self.loadEmergency() self.odomL = self.emergency.odomL self.odomL_rot = self.emergency.odomL_rot self.odomR = self.emergency.odomR self.odomR_rot = self.emergency.odomR_rot emergency_timestamp = self.emergency.timestamp if (rospy.Time.now() - emergency_timestamp ).to_sec() > self.emergencyTimeWindow: print '' rospy.logwarn( '***********************************************************************' ) rospy.logwarn( '[shelf_publisher] NOT ABLE TO RECOVER FROM CRASHING, GAME OVER' ) rospy.logwarn( '***********************************************************************' ) continue ask = False self.calibrated = True self.priorAvailable = True self.priorOri_in_odom = shelf_in_odom self.priorRot_in_odom = shelf_rot_in_odom self.priorLeft_in_base_laser_link = legs[0] self.priorRight_in_base_laser_link = legs[1] print '' rospy.logwarn( '***********************************************************************' ) rospy.logwarn( '[shelf_publisher] EMERGENCY RECOVERY CALLED!!!!!!!!!!!!!!!!!!!!!!' ) rospy.logwarn( '***********************************************************************' ) else: sys.stdout.write( "\r [ROS time: %s] Is the current shelf pose estimation good? (y/n)" % rospy.get_time()) sys.stdout.flush() if self._start: answer = 'y' if answer == 'y' or answer == 'yes': self.calibrated = True ask = False self.priorAvailable = True self.priorOri_in_odom = shelf_in_odom self.priorRot_in_odom = shelf_rot_in_odom self.priorLeft_in_base_laser_link = legs[0] self.priorRight_in_base_laser_link = legs[1] print "" rospy.loginfo("Human calibration finished") rospy.loginfo( "Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1])) while not rospy.is_shutdown(): try: self.odomL, self.odomL_rot = self.listener.lookupTransform( "/odom_combined", "/left_leg", rospy.Time(0)) self.odomR, self.odomR_rot = self.listener.lookupTransform( "/odom_combined", "/right_leg", rospy.Time(0)) break except: pass self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now()) self.saveEmergency() else: continue # check in the odometry frame if self.priorAvailable: # print 'pub odom' self.br.sendTransform(self.odomL, \ self.odomL_rot, \ rospy.Time.now(),\ "/odomL", \ "/odom_combined") self.br.sendTransform(self.odomR, \ self.odomR_rot, \ rospy.Time.now(),\ "/odomR", \ "/odom_combined") if self.calibrated: u += 1 shelfN += 1 if math.sqrt((shelf_in_odom[0] - self.priorOri_in_odom[0])**2 + (shelf_in_odom[1] - self.priorOri_in_odom[1])**2) > self.tolerance: rospy.logwarn( 'something is wrong with shelf pose estimation!!!!!!!!!! RECALIBRATING' ) recalibrateCount = 0 u = 0 self.calibrated = False self.reCalibration = True continue else: self.br.sendTransform( (shelfOri[0], shelfOri[1], 0), tf.transformations.quaternion_from_euler( 0, 0, shelfRot), rospy.Time.now(), "/shelf_frame", # child "/base_laser_link" # parent ) self.br.sendTransform((legs[0][0], legs[0][1], 0), \ tf.transformations.quaternion_from_euler(0, 0, shelfRot), \ rospy.Time.now(),\ "/left_leg", \ "/base_laser_link") self.br.sendTransform((legs[1][0], legs[1][1], 0), \ tf.transformations.quaternion_from_euler(0, 0, shelfRot), \ rospy.Time.now(),\ "/right_leg", \ "/base_laser_link") self.pubShelfSep.publish( self.tf2PoseStamped( shelfOri, tf.transformations.quaternion_from_euler( 0, 0, shelfRot))) if u == self.updateRounds: while not rospy.is_shutdown( ): # make sure the odomL and odomR are updated try: self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform( "/odom_combined", "/shelf_frame", rospy.Time(0)) self.odomL, self.odomL_rot = self.listener.lookupTransform( "/odom_combined", "/left_leg", rospy.Time(0)) self.odomR, self.odomR_rot = self.listener.lookupTransform( "/odom_combined", "/right_leg", rospy.Time(0)) rospy.loginfo( "Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1])) u = 0 break except: continue if (rospy.Time.now() - t_init).to_sec() > 1.0: t_init = rospy.Time.now() self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now()) self.saveEmergency() # print 'pub' shelfN = 0 self.br.sendTransform( (0, 0.1515 + self.binOffset, 1.21), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_A", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, -0.1515 + self.binOffset, 1.21), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_B", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, -0.4303 + self.binOffset, 1.21), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_C", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, 0.1515 + self.binOffset, 1), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_D", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, -0.1515 + self.binOffset, 1), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_E", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, -0.4303 + self.binOffset, 1), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_F", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, 0.1515 + self.binOffset, 0.78), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_G", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, -0.1515 + self.binOffset, 0.78), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_H", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, -0.4303 + self.binOffset, 0.78), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_I", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, 0.1515 + self.binOffset, 0.51), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_J", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, -0.1515 + self.binOffset, 0.51), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_K", # child "/shelf_frame" # parent ) self.br.sendTransform( (0, -0.4303 + self.binOffset, 0.51), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/shelf_bin_L", # child "/shelf_frame" # parent )
class lidar_processing(): def __init__(self): ## SETUP ROS NODES rospy.init_node('lidar_processing', anonymous=False) rospy.loginfo("To stop vecs_to_gonogo CTRL + C") rospy.on_shutdown(self.shutdown) self.r = rospy.Rate(10) # 10hz self.coms = 0.0 self.vel = 0.0 new_pose = np.asarray([1, 0]) target_pose = np.asarray([1, 0]) #SUBSCRIBER #rospy.Subscriber('/converted_pc', PointCloud2, self.parse_command) self.laser_projector = LaserProjection() self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.on_scan) # target pose from filter # PUBLISHER # self.pub = rospy.Publisher('/go_nogo', ObjectVector, queue_size=1) # ~ self.pub = rospy.Publisher('/net_input', Float32MultiArray, queue_size=1) self.pub_hulls = rospy.Publisher('/convex_hulls', MarkerArray, queue_size=1) # ~ self.pub_new_target = rospy.Publisher('/new_target', Point, queue_size=1) self.pub_new_target = rospy.Publisher('/new_target', Marker, queue_size=1) rospy.sleep(0.01) rospy.loginfo("Waiting for topics...") try: rospy.wait_for_message("/scan", LaserScan, timeout=10.0) except rospy.ROSException as e: rospy.logerr("Timeout while waiting for topics!") raise e print("Node started") while not rospy.is_shutdown(): self.r.sleep() #self.xyz_generator # GET CLUSTERS points, labels, n_clusters = self.get_clusters() # GET HULLS and their COMs hulls = [] coms = [] for n in range(n_clusters): hull = ConvexHull(points[labels == n, :]) # ~ print("mean",np.mean(points[labels==n,:], 0) ) coms.append(np.mean(points[labels == n, :], 0)) # hull.vertices, hull.area # print(n, 'area', hull.area, np.std(points[labels==n,:],0)) hulls.append(hull) # GET TARGET POSE dists = np.sum((np.vstack(coms) - new_pose)**2, 1) self.new_id = np.argmin(dists) # PUBLISH HULLS marker = self.get_hull_markers(hulls, coms) self.pub_hulls.publish(marker) # PUBLISH NEW TARGET (for reset condition) nt = coms[self.new_id] # ~ matched = coms[self.cluster_id] # ~ print("new target", nt, nt[0], nt[1]) # ~ self.pub_new_target.publish(Pose(Point(x=nt[0], y=nt[1],z=0) ) ) m = default_marker(x=nt[0], y=nt[1], z=0) m.type = 2 m.color.r = 0 m.color.a = 1.0 m.scale.x = .30 m.scale.y = .30 m.scale.z = .30 self.pub_new_target.publish(m) if VIS_HULL: fig = plt.figure(1) plt.clf() plt.plot(points[:, 0], points[:, 1], 'o') for n in range(n_clusters): hull = hulls[n] pts = points[labels == n, :] for simplex in hull.simplices: if n == self.cluster_id: plt.plot(hull.points[simplex, 0], hull.points[simplex, 1], 'r-') else: plt.plot(hull.points[simplex, 0], hull.points[simplex, 1], 'k-') plt.axis([-2, 8, -4, 4]) plt.show(block=False) plt.pause(0.0000000001) # ~ self.pub.publish(self.data) # numpy in ros only handles 1d arrays def shutdown(self): rospy.loginfo("Stop lidar_processing") # rospy.sleep(1) return 0 def get_clusters(self): # ~ print(data.data) #for p in gen: # print " x : %f y: %f z: %f" %(p[0],p[1],p[2]) self.xy = deepcopy(self.xyz[:, :2]) db = DBSCAN(eps=0.15, min_samples=3).fit(self.xy) core_samples_mask = np.zeros_like(db.labels_, dtype=bool) core_samples_mask[db.core_sample_indices_] = True labels = db.labels_ # Number of clusters in labels, ignoring noise if present. n_clusters = len(set(labels)) - (1 if -1 in labels else 0) n_noise = list(labels).count(-1) self.labels = labels if VIS_CLUSTER: fig = plt.figure(1) plt.clf() for c in range(n_clusters): col = np.random.uniform(0, 1, (3)) # ~ print(np.shape(self.xy[labels==c,0])) plt.plot(self.xy[self.labels == c, 0], self.xy[self.labels == c, 1], '.', color=col) # ~ plt.plot(self.vel_hist, '.r') plt.plot(self.xy[self.labels == -1, 0], self.xy[self.labels == -1, 1], '.', color=[0, 0, 0]) plt.xlabel("Time") plt.ylabel("Speed (m/s)") plt.show(block=False) plt.pause(0.0000000001) # ~ print("clusters:",n_clusters) return self.xy, self.labels, n_clusters def on_scan(self, scan): #print scan #rospy.loginfo("Got scan, projecting") cloud = self.laser_projector.projectLaser(scan) #print cloud #rospy.loginfo("Printed cloud") gen = pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z")) self.xyz_generator = gen self.xyz = np.asarray([[p[0], p[1], p[2]] for p in gen]) # ~ print(np.shape(self.xyz)) #for p in gen: # print " x : %f y: %f z: %f" %(p[0],p[1],p[2]) def get_hull_markers(self, hulls, coms): marker = MarkerArray() ind = 0 for h in range(len(hulls)): m = default_marker() m.id = ind #car.id -- all car.ids are 0 m.lifetime = rospy.Duration(0.1) # ~ m.header = car.header # m.color.g = 0.72 # m.color.a = (car.id+0.0)/t # m.scale.x = 1.0 + 5*(car.id+0.0)/t # m.scale.y = 1.0 + 5*(car.id+0.0)/t # ~ print("hull",hull) m.points.append(Point(x=coms[h][0], y=coms[h][1], z=0)) for v in hulls[h].vertices: m.points.append( Point(x=hulls[h].points[v, 0], y=hulls[h].points[v, 1], z=0)) marker.markers.append(m) ind += 1 return marker