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()