def moveRover(uniboard ,gyro, distance):
initialPos = pos
rospy.loginfo("MOVEMENT: attempting to move %s meters from x=%s", distance, pos)
# now we move in a straight line for h meters (while our moved distance minus starting point is less than desired)
if (distance > .05):
u('motor_left', 2, '0.25', rospy.Time.now())
u('motor_right', 2, '0.25', rospy.Time.now())
while (abs(pos) - abs(initialPos) < abs(distance)):
pass
else:
rospy.loginfo("MOVEMENT: Too small of distance to move")
# met our distance, time to halt
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
# sit, zero, sit
time.sleep(1.0)
gyro()
time.sleep(1.0)
rospy.loginfo("MOVEMENT: done, wheels off, gryo zeroed")
def moveRover(uniboard, gyro, distance):
initialPos = pos
rospy.loginfo("MOVEMENT: attempting to move %s meters from x=%s", distance,
pos)
# now we move in a straight line for h meters (while our moved distance minus starting point is less than desired)
if (distance > .05):
u('motor_left', 2, '0.25', rospy.Time.now())
u('motor_right', 2, '0.25', rospy.Time.now())
while (abs(pos) - abs(initialPos) < abs(distance)):
pass
else:
rospy.loginfo("MOVEMENT: Too small of distance to move")
# met our distance, time to halt
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
# sit, zero, sit
time.sleep(1.0)
gyro()
time.sleep(1.0)
rospy.loginfo("MOVEMENT: done, wheels off, gryo zeroed")
def publish_data(self):
rate = rospy.Rate(SAMPLE_RATE)
while not rospy.is_shutdown():
data = self.get_data()
if len(data) > 0:
current = data.pop()
omega_degrees = current["gyro"][1]
theta_degrees = current["angle"][1]
msg = gyro()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "/imu"
msg.theta = float(theta_degrees) * (-np.pi / 180.0)
msg.omega = float(omega_degrees) * (-np.pi / 180.0)
self.pub.publish(msg)
rate.sleep()
def publish_data(self):
rate = rospy.Rate(SAMPLE_RATE)
while not rospy.is_shutdown():
data = self.get_data()
if len(data) > 0:
current = data.pop()
omega_degrees = current['gyro'][1]
theta_degrees = current['angle'][1]
msg = gyro()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '/imu'
msg.theta = float(theta_degrees) * (-np.pi / 180.0)
msg.omega = float(omega_degrees) * (-np.pi / 180.0)
self.pub.publish(msg)
rate.sleep()
