#!/usr/bin/env python
import vmu931_driver as vmu
import dato
def printValue(val):
print "setTimestam = {} w = {} x = {} y = {} z = {} heading ={} msg ={}".format(
val.timestamp, val.w, val.x, val.y, val.z, val.heading, val.msg)
d = vmu.vmu931("/dev/ttyUSB_VMU931")
d.connectToVMU931()
d.isConnected()
d.enableStreamingAccelerometers()
d.enableStreamingGyroscopes()
d.enableStreamingMagnetometers()
d.enableStreamingQuaternions()
d.enableStreamingEulerAngles()
d.enableStreamingHeading()
for x in range(0, 1):
print "-------------------------------"
d.readOneTime()
d.printAllValue()
def __init__(self, args):
self.node_name = rospy.get_name() #.replace('/','')
self.desired_freq = args['desired_freq']
# Checks value of freq
if self.desired_freq <= 0.0 or self.desired_freq > MAX_FREQ:
rospy.loginfo(
'%s::init: Desired freq (%f) is not possible. Setting desired_freq to %f'
% (self.node_name, self.desired_freq, DEFAULT_FREQ))
self.desired_freq = DEFAULT_FREQ
self.real_freq = 0.0
# configuration driver to imu
self._port = args['port']
self._frame_id = args['frame_id']
self._mode = args['mode']
if self._mode == VMU931_MODE_QUATERNION:
self._stream_gyro = False
self._stream_magnetometer = False
self._stream_accelerometer = False
self._stream_quaternion = True
self._stream_euler = True
self._stream_heading = True
elif self._mode == VMU931_MODE_GYROSCOPE:
self._stream_gyro = True
self._stream_magnetometer = True
self._stream_accelerometer = True
self._stream_quaternion = False
self._stream_euler = False
self._stream_heading = False
else:
if self._mode != VMU931_MODE_CUSTOM:
rospy.logwarn('%s::init: unknown mode %s. Setting %s mode',
self.node_name, self._mode, VMU931_MODE_CUSTOM)
self._mode = VMU931_MODE_CUSTOM
self._stream_gyro = args['gyroscope']
self._stream_magnetometer = args['magnetometer']
self._stream_accelerometer = args['accelerometer']
self._stream_quaternion = args['quaternion']
self._stream_euler = args['euler']
self._stream_heading = args['heading']
# Saves the state of the component
self.state = State.INIT_STATE
# Saves the previous state
self.previous_state = State.INIT_STATE
# flag to control the initialization of the component
self.initialized = False
# flag to control the initialization of ROS stuff
self.ros_initialized = False
# flag to control that the control loop is running
self.running = False
# Variable used to control the loop frequency
self.time_sleep = 1.0
# State msg to publish
self.msg_state = VmuState()
self.msg_state.mode = self._mode
# Timer to publish state
self.publish_state_timer = 1
# Flag active to calibrate the sensor
self._calibration = False
self._calibration_step = 0
self._emergency_recovery_timer = rospy.Time.now()
self._imu_dev = vmu.vmu931(self._port, baudrate=9600)
self.t_publish_state = threading.Timer(self.publish_state_timer,
self.publishROSstate)
self._imu_msg = Imu()
self._imu_msg.header.frame_id = self._frame_id
self._imu_msg.orientation_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
self._imu_msg.angular_velocity_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
self._imu_msg.linear_acceleration_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
self._gyro_msg = Vector3Stamped()
self._gyro_msg.header.frame_id = self._frame_id
self._euler_msg = Vector3Stamped()
self._euler_msg.header.frame_id = self._frame_id
self._accel_msg = Vector3Stamped()
self._accel_msg.header.frame_id = self._frame_id
self._magnet_msg = Vector3Stamped()
self._magnet_msg.header.frame_id = self._frame_id
self._quaternion_msg = QuaternionStamped()
self._quaternion_msg.header.frame_id = self._frame_id
self._heading_msg = Vector3Stamped()
self._heading_msg.vector.x = self._heading_msg.vector.y = 0
self._heading_msg.header.frame_id = self._frame_id
#!/usr/bin/env python3
import vmu931_driver as vmu
import dato
d = vmu.vmu931("/dev/ttyACM0")
d.setup()
d.isConnected()
d.streamingAccelerometers("Accelerometers")
d.streamingGyroscopes("Gyroscopes")
d.streamingHeading("Heading")
d.streamingMagnetometers("Magnetometers")
# Now calibrate
d.calibrate()
d.close()
print("\nCalibration finished\n")