class Prediction: def __init__(self): rospy.init_node("onlinePrediction", anonymous=True) self.__controlSignal = False self.__controlState = 0 self.__isAngleValid = False self.__isImuValid = False self.__extract = Extractor() self.__data = [] self.__isPredictionValid = False self.__model = Model() self.__collectData = [] self.__isAngleInit = False self.__initAngleData = [] self.__angleOffset = 0 self.__pubFuture = rospy.Publisher("predicted_trajectory", FutureTrajectory, queue_size=4) self.__pubLocation = rospy.Publisher("nowLocation", Point, queue_size=4) rospy.Subscriber("robot_odm", Pose2D, self.odmCallback, queue_size=4) rospy.Subscriber("openpose_ros/human_depth_list", HumanDepthList, self.humanListCallback, queue_size=50) rospy.Subscriber("/robot/controlSignal", Bool, self.controlSignalCallback) rospy.Subscriber("/imu", Imu, self.imuCallback, queue_size=3) rospy.Subscriber("/imu_angle", Vector3, self.angleCallback) def odmCallback(self, odm): self.__odm = odm def controlSignalCallback(self, controlSignal): self.__controlSignal = controlSignal.data def humanListCallback(self, humanDepthList): if self.__isImuValid is False or self.__isAngleValid is False: return if self.__controlState == 0: # 控制状态为0表明还未开始记录 if self.__controlSignal is False: return else: print "start recording!" self.__controlState = 1 if self.__controlState == 1: if self.__controlSignal is True: # 记录数据 curData = self.__extract.extractHumanPose( humanDepthList, self.__odm) curData.extend([ self.__angleData.x, self.__angleData.y, self.__angleData.z, self.__imuData.linear_acceleration.x, self.__imuData.linear_acceleration.y, self.__imuData.linear_acceleration.z, self.__imuData.angular_velocity.x, self.__imuData.angular_velocity.y, self.__imuData.angular_velocity.z ]) self.__data.append(curData) nowLocation = Point() nowLocation.x = curData[0] nowLocation.y = curData[1] nowLocation.z = 0 self.__pubLocation.publish(nowLocation) if self.__isAngleInit is False: self.__initAngleData.append(self.__angleData.z) if len(self.__initAngleData) >= 8: self.__isAngleInit = True self.__angleOffset = sum(self.__initAngleData) / len( self.__initAngleData) self.__initAngleData = [] if len(self.__data) >= 11: self.__isPredictionValid = True if len(self.__data) > 11: del self.__data[0] if self.__isPredictionValid is True: # print self.__odm npTrajectory = np.array(self.__data) cur = time.time() npFuture = self.__model.predictFuture( npTrajectory[:, srcIndex]) Weights = np.matmul(T_inv, npFuture) # plotLocationData(npTrajectory.transpose(), npFuture.transpose()) self.__collectData.append([ npTrajectory[:, srcIndex].transpose(), npFuture.transpose() ]) futureTrajectory = FutureTrajectory() for i in range(0, 8): point = Point() point.x = npFuture[i][0] point.y = npFuture[i][1] point.z = 0 futureTrajectory.locations.append(point) for i in range(0, 3): for j in range(0, 2): futureTrajectory.weights.append( Float64(Weights[i][j] / 1000)) futureTrajectory.weights.append( Float64(self.__angleData.z - self.__angleOffset)) print((self.__angleData.z - self.__angleOffset) / 180 * 3.14159) now = time.time() # print( now - cur ) self.__pubFuture.publish(futureTrajectory) else: self.__controlState = 0 # 将数据存成文件并且清空数据 nowTime = time.strftime("%Y-%m-%d-%H_%M_%S", time.localtime(time.time())) fileName = nowTime + ' Line3' + '.npy' # plotData(self.__data) np.save(fileName, self.__data) self.__data = [] self.__extract = Extractor() print "save as " + fileName self.__isAngleInit = False ''' for i in range(len(self.__collectData)): if i % 5 == 0: plotLocationData(self.__collectData[i][0], self.__collectData[i][1]) ''' def imuCallback(self, imuData): self.__isImuValid = True self.__imuData = imuData def angleCallback(self, angleData): self.__isAngleValid = True self.__angleData = angleData def run(self): rospy.spin()
class CollectData: def __init__(self): rospy.init_node("CollectData", anonymous=True) rospy.Subscriber("robot_odm", Pose2D, self.odmCallback) rospy.Subscriber("openpose_ros/human_depth_list", HumanDepthList, self.humanListCallback) rospy.Subscriber("/robot/controlSignal", Bool, self.controlSignalCallback) rospy.Subscriber("/imu", Imu, self.imuCallback) rospy.Subscriber("/imu_angle", Vector3, self.angleCallback) self.__controlSignal = False self.__controlState = 0 self.__isAngleValid = False self.__isImuValid = False self.__extract = Extractor() self.__data = [] def odmCallback(self, odm): self.__odm = odm def controlSignalCallback(self, controlSignal): if controlSignal.data is True: time.sleep(12) self.__controlSignal = controlSignal.data def humanListCallback(self, humanDepthList): if self.__isImuValid is False or self.__isAngleValid is False: return if self.__controlState == 0: # 控制状态为0表明还未开始记录 if self.__controlSignal is False: return else: print "start recording!" self.__controlState = 1 if self.__controlState == 1: if self.__controlSignal is True: # 记录数据 curData = self.__extract.extractHumanPose( humanDepthList, self.__odm) curData.extend([ self.__angleData.x, self.__angleData.y, self.__angleData.z, self.__imuData.linear_acceleration.x, self.__imuData.linear_acceleration.y, self.__imuData.linear_acceleration.z, self.__imuData.angular_velocity.x, self.__imuData.angular_velocity.y, self.__imuData.angular_velocity.z, time.time(), self.__odm.x, self.__odm.y, self.__odm.theta ]) self.__data.append(curData) else: self.__controlState = 0 # 将数据存成文件并且清空数据 nowTime = time.strftime("%Y-%m-%d-%H_%M_%S", time.localtime(time.time())) fileName = nowTime + ' Turn1' + '.npy' result = plotData(self.__data) if result is True: np.save(fileName, self.__data) print "save as " + fileName else: print "wrong, reject" self.__data = [] self.__extract = Extractor() def imuCallback(self, imuData): self.__isImuValid = True self.__imuData = imuData def angleCallback(self, angleData): self.__isAngleValid = True self.__angleData = angleData def run(self): rospy.spin()