尝试对运行以下代码所需的循环速率计时,其中我在循环中附加了经过的时间以及结束时间和开始时间
rate = rospy.Rate(300) # 10
# Define scaling variables:
# Scaling as per the environment.hpp obscaled operation
# to make input consistent with simulation
obsMean = np.zeros((18))
obsStd = np.hstack((np.full((1,9),0.7),np.full((1,3),0.5),np.full((1,3),3),np.full((1,3),8))).reshape(-1)
# Create some arrays here for storing variables
# First thing is to create a couple of array to store the variables
# I wanted to plot results
obs_array = []
action_array = []
time_array = []
looprate_array = []
# Actual start time
time_start = time.time()
while not rospy.is_shutdown():
# For calculating how long it takes for predict to work.
start = time.time()
# Construct obs array for feeding into model predict
obs = np.hstack(
[receiveData.rotMatrix.reshape(-1),
receiveData.pos_offset,
receiveData.linear_velocity,
receiveData.angular_velocity])
# Needed to implement observation scaling as detailed inside the
# Environment.hpp to actually input the right values to get the
# right action values
# Scaling as per the environment.hpp obscaled operation
# to make input consistent with simulation
obscale = np.divide( np.subtract(obs, obsMean), obsStd)
obs_array.append(obscale)
print("Obs original Array: \r\n" + str(obs))
print("Obs scaled Array: \r\n" + str(obscale))
print("Current Position: \r\n" + str(receiveData.pos_converted.pose.position))
print("Current Euler: \r\n" + str(receiveData.eulerAngle))
print("Target Position \r\n" + str(receiveData.target_pos))
print("Position Offset: \r\n" + str(receiveData.pos_offset))
#Predict action from received data.
action, _ = model.predict(obscale)
# Need to append the action array
action_array.append(action)
# Need to append the time array
time_end = time.time()
elapsedtime = (time_end-time_start)
time_array.append(elapsedtime)
# Store variables in a list and put it in controls
acm_list = [0]*8
acm_list[0] = action[0]
acm_list[1] = action[1]
acm_list[2] = action[2]
acm_list[4] = action[3]
# Populate the actuator_control_message with timestamp
# etc.
actuator_control_message.header.stamp = rospy.Time.now()
actuator_control_message.group_mix = 0
actuator_control_message.controls = acm_list
rospy.loginfo(acm_list)
# Publish data to topic so that :
receiveData.pub1.publish(receiveData.broadcastVel)
receiveData.pub2.publish(receiveData.broadcastEuler)
receiveData.pub3.publish(receiveData.pos_converted)
pub4.publish(actuator_control_message)
rate.sleep()
end = time.time()
looprate_array.append((end - start) * 1000.0)
timestring = "Took %f ms" % ((end - start) * 1000.0)
rospy.loginfo(timestring)
with open('/home/ted/catkin_ws/src/transmit_thrust/scripts/RealLifeBenchmarkingChangeRate300Policy16650AlignedYPos-08.pickle', 'wb') as f:
pickle.dump([obs_array, action_array, time_array, looprate_array], f)
f.close()
当我试图绘制循环速率与时间的关系图时,我得到了一些与循环速率有趣的不一致,如下所示:
The first loop time is abnormally large
The loop latter on seemed to have its own frequency associated to it.
目前没有回答
相关问题 更多 >
编程相关推荐