diff --git a/src/publish_vel_dist.py b/src/publish_vel_dist.py index bbe976a..d6f58bf 100644 --- a/src/publish_vel_dist.py +++ b/src/publish_vel_dist.py @@ -8,52 +8,26 @@ class VariableMaintainer(): def __init__(self): - self._time = False - self._velocity = [0,0,0] - self._position = [0,0,0] - - @property - def time(self): - return self._time - - @time.setter - def time(self, value): - self._time = value - - @property - def velocity(self): - return self._velocity - - @velocity.setter - def velocity(self, value): - self._velocity = value - - @property - def position(self): - return self._position - - @position.setter - def position(self, value): - self._position = value - - + self.time = False + self.velocity = [0,0,0] + self.position = [0,0,0] def publishVelDist(msg, vmaint): velPub = rospy.Publisher('calc_velocity', Vector3) posPub = rospy.Publisher('calc_position', Vector3) if not vmaint.time: # This means this is the first run - vmaint.time = msg.header.secs + msg.header.nsecs*math.pow(10,-9) + vmaint.time = msg.header.stamp.secs + msg.header.stamp.nsecs*math.pow(10,-9) vmaint.velocity = [0,0,0] vmaint.position = [0,0,0] else: - ctime = msg.header.secs + msg.header.nsecs*math.pow(10,-9) + ctime = msg.header.stamp.secs + msg.header.stamp.nsecs*math.pow(10,-9) tdelt = ctime - vmaint.time accel = [msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z] - map(lambda x: x*timedelt, accel) + map(lambda x: x*tdelt, accel) vmaint.velocity = [vmaint.velocity[i]+accel[i] for i in range(3)] - map(lambda x: x*timedelt, accel) + map(lambda x: x*tdelt, accel) vmaint.velocity = [vmaint.velocity[i]+accel[i] for i in range(3)] vmaint.time = ctime v = Vector3(vmaint.velocity[0],vmaint.velocity[1],vmaint.velocity[2]) @@ -64,9 +38,9 @@ def publishVelDist(msg, vmaint): def main(): # Collect first 50 entries and average them. rospy.init_node("publish_vel_dist") - timemaint = VariableMaintainer() + vmaint = VariableMaintainer() rospy.Subscriber('imu', Imu, publishVelDist, vmaint) rospy.spin() if __name__=='__main__': - main() \ No newline at end of file + main()