在python代码中使用rospy.rate进行一致计时?

2024-06-26 13:41:33 发布

您现在位置:Python中文网/ 问答频道 /正文

尝试对运行以下代码所需的循环速率计时,其中我在循环中附加了经过的时间以及结束时间和开始时间

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.


Tags: thetotimenpactionarraystartrospy