def Publish(self):
imu_data = self._hal_proxy.GetImu()
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self._frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.orientation.w = imu_data['orientation']['w']
imu_msg.orientation.x = imu_data['orientation']['x']
imu_msg.orientation.y = imu_data['orientation']['y']
imu_msg.orientation.z = imu_data['orientation']['z']
imu_msg.linear_acceleration.x = imu_data['linear_accel']['x']
imu_msg.linear_acceleration.y = imu_data['linear_accel']['y']
imu_msg.linear_acceleration.z = imu_data['linear_accel']['z']
imu_msg.angular_velocity.x = imu_data['angular_velocity']['x']
imu_msg.angular_velocity.y = imu_data['angular_velocity']['y']
imu_msg.angular_velocity.z = imu_data['angular_velocity']['z']
# Read the x, y, z and heading
self._publisher.publish(imu_msg)
def trigger(self):
cs = Compass()
# create frame id
cs.header.frame_id = self.frame_id
# create header (provides time stamp)
cs.header.stamp = rospy.Time.now()
# convert orientation from the CompassSensor.get_heading_lsb byte (heading in the 0-255 range)
# to quaternion
self.orientation = self.compass.get_sample()*-2.0 * math.pi/180.0
sample = self.compass.get_sample()
# create new imu msgs for the compass with time stamp as before
imu = Imu()
imu.header.frame_id = self.frame_id
imu.header.stamp = rospy.Time.now()
imu.angular_velocity.x = 0.0
imu.angular_velocity.y = 0.0
imu.angular_velocity.z = 1*math.pi/180.0 # the rotation speed
# covariance required by robot_pose_ekf
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
self.orienation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
imu.orientation = cs.orientation
self.prev_time = imu.header.stamp
(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
self.pub.publish(imu) # publish the message
def sensor_interface():
rospy.init_node('imu_razor9dof', anonymous = False)
interface.flushInput()
# Perform a loop checking on the enabled status of the drivers
while not rospy.is_shutdown():
# Fetch a JSON message from the controller and process (or atleast attempt to)
try:
# Get the object and create some messages
_object = json.loads(interface.readline())
_imu = Imu();
_heading = Float32()
# Get the imu data (f**k quaternions!!)
_imu.header = rospy.Header()
roll = _object['angles'][0]
pitch = _object['angles'][1]
yaw = _object['angles'][2]
#_imu.orientation = tf::createQuaternionFromRPY(roll, pitch, yaw)
_imu.linear_acceleration.x = _object['accel'][0]
_imu.linear_acceleration.y = _object['accel'][1]
_imu.linear_acceleration.z = _object['accel'][2]
# Get the heading data
#_heading.data = _object['heading']
_heading.data = _object['angles'][2]
# Publish the data
imuPublisher.publish(_imu);
cmpPublisher.publish(_heading);
except json.decoder.JSONDecodeError:
rospy.logwarn("Invalid message received")
except: pass
def run(self):
while not rospy.is_shutdown():
time_now = time.time()
if time_now - self.timestamp > 0.5:
self.cmd = (0, 0)
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count, rospy.Time.now(), self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
self.heartBeat.publish("I am alive from Glew")
if self.msg != None:
self.publisher.publish(self.msg.linear.y)
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0, 0)
def unpackIMU(self, data): imu_msg = Imu() #Unpack Header imu_msg.header = self.unpackIMUHeader(data[0:19]) #Unpack Orientation Message quat = self.bytestr_to_array(data[19:19 + (4*8)]) imu_msg.orientation.x = quat[0] imu_msg.orientation.y = quat[1] imu_msg.orientation.z = quat[2] imu_msg.orientation.w = quat[3] #Unpack Orientation Covariance imu_msg.orientation_covariance = list(self.bytestr_to_array(data[55:(55 + (9*8))])) #Unpack Angular Velocity ang = self.bytestr_to_array(data[127: 127 + (3*8)]) imu_msg.angular_velocity.x = ang[0] imu_msg.angular_velocity.y = ang[1] imu_msg.angular_velocity.z = ang[2] #Unpack Angular Velocity Covariance imu_msg.angular_velocity_covariance = list(self.bytestr_to_array(data[155:(155 + (9*8))])) #Unpack Linear Acceleration lin = self.bytestr_to_array(data[227: 227 + (3*8)]) imu_msg.linear_acceleration.x = lin[0] imu_msg.linear_acceleration.y = lin[1] imu_msg.linear_acceleration.z = lin[2] #Unpack Linear Acceleration Covariance imu_msg.linear_acceleration_covariance = list(self.bytestr_to_array(data[255:(255 + (9*8))])) return imu_msg
def publish(self, data):
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def post_imu(self, component_instance):
""" Publish the data of the IMU sensor as a custom ROS Imu message
"""
parent = component_instance.robot_parent
parent_name = parent.blender_obj.name
current_time = rospy.Time.now()
imu = Imu()
imu.header.seq = self._seq
imu.header.stamp = current_time
imu.header.frame_id = "/imu"
imu.orientation = self.orientation.to_quaternion()
imu.angular_velocity.x = component_instance.local_data['angular_velocity'][0]
imu.angular_velocity.y = component_instance.local_data['angular_velocity'][1]
imu.angular_velocity.z = component_instance.local_data['angular_velocity'][2]
imu.linear_acceleration.x = component_instance.local_data['linear_acceleration'][0]
imu.linear_acceleration.y = component_instance.local_data['linear_acceleration'][1]
imu.linear_acceleration.z = component_instance.local_data['linear_acceleration'][2]
for topic in self._topics:
# publish the message on the correct w
if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name):
topic.publish(imu)
self._seq += 1
def loop(self):
while not rospy.is_shutdown():
line = self.port.readline()
chunks = line.split(":")
if chunks[0] == "!QUAT":
readings = chunks[1].split(",")
if len(readings) == 10:
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = 'imu'
imu_msg.orientation.x = float(readings[0])
imu_msg.orientation.y = float(readings[1])
imu_msg.orientation.z = float(readings[2])
imu_msg.orientation.w = float(readings[3])
imu_msg.orientation_covariance = list(zeros((3,3)).flatten())
imu_msg.angular_velocity.x = float(readings[4])
imu_msg.angular_velocity.y = float(readings[5])
imu_msg.angular_velocity.z = float(readings[6])
imu_msg.angular_velocity_covariance = list(0.1*diagflat(ones((3,1))).flatten())
imu_msg.linear_acceleration.x = float(readings[7])
imu_msg.linear_acceleration.y = float(readings[8])
imu_msg.linear_acceleration.z = float(readings[9])
imu_msg.linear_acceleration_covariance = list(0.1*diagflat(ones((3,1))).flatten())
self.pub.publish(imu_msg)
quaternion = (imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w)
tf_br.sendTransform(translation = (0,0, 0), rotation = quaternion,time = rospy.Time.now(),child = 'imu',parent = 'world')
else:
rospy.logerr("Did not get a valid IMU packet, got %s", line)
else:
rospy.loginfo("Did not receive IMU data, instead got %s", line)
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def publish(self, event):
compass_accel = self.compass_accel.read()
compass = compass_accel[0:3]
accel = compass_accel[3:6]
gyro = self.gyro.read()
# Put together an IMU message
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.orientation_covariance[0] = -1
imu_msg.angular_velocity = gyro[0] * DEG_TO_RAD
imu_msg.angular_velocity = gyro[1] * DEG_TO_RAD
imu_msg.angular_velocity = gyro[2] * DEG_TO_RAD
imu_msg.linear_acceleration.x = accel[0] * G
imu_msg.linear_acceleration.y = accel[1] * G
imu_msg.linear_acceleration.z = accel[2] * G
self.pub_imu.publish(imu_msg)
# Put together a magnetometer message
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.magnetic_field.x = compass[0]
mag_msg.magnetic_field.y = compass[1]
mag_msg.magnetic_field.z = compass[2]
self.pub_mag.publish(mag_msg)
def publish_imu(self):
imu_msg = Imu()
imu_msg.header.stamp = self.time
imu_msg.header.frame_id = 'imu_odom'
#quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz')
a = self.kalman_state[3,0]
b = self.kalman_state[4,0]
c = self.kalman_state[5,0]
d = self.kalman_state[6,0]
q = math.sqrt(math.pow(a,2)+math.pow(b,2)+math.pow(c,2)+math.pow(d,2))
#angles = tf_math.euler_from_quaternion(self.kalman_state[3:7,0])
imu_msg.orientation.x = a/q#angles[0]
imu_msg.orientation.y = b/q
imu_msg.orientation.z = c/q
imu_msg.orientation.w = d/q
imu_msg.orientation_covariance = list(self.kalman_covariance[3:6,3:6].flatten())
imu_msg.angular_velocity.x = self.kalman_state[0,0]
imu_msg.angular_velocity.y = self.kalman_state[1,0]
imu_msg.angular_velocity.z = self.kalman_state[2,0]
imu_msg.angular_velocity_covariance = list(self.kalman_covariance[0:3,0:3].flatten())
imu_msg.linear_acceleration.x = self.kalman_state[10,0]
imu_msg.linear_acceleration.y = self.kalman_state[11,0]
imu_msg.linear_acceleration.z = self.kalman_state[12,0]
imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[10:13,10:13].flatten())
self.pub.publish(imu_msg)
def fake_imu():
pub = rospy.Publisher('IMU_ned', Imu, queue_size=10)
rospy.init_node('test_convertimu', anonymous=True)
rate = rospy.Rate(0.5) # 10hz
while not rospy.is_shutdown():
imu_message=Imu()
imu_message.header.stamp=rospy.Time.now()
imu_message.header.frame_id="IMU"
imu_message.orientation.x=1
imu_message.orientation.y=2
imu_message.orientation.z=3
imu_message.orientation.w=4
imu_message.orientation_covariance=[-1,0,0,0,-1,0,0,0,-1]
imu_message.linear_acceleration.x=6
imu_message.linear_acceleration.y=7
imu_message.linear_acceleration.z=8
imu_message.linear_acceleration_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6]
imu_message.angular_velocity.x=10
imu_message.angular_velocity.y=11
imu_message.angular_velocity.z=12
imu_message.angular_velocity_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6]
pub.publish(imu_message)
rate.sleep()
def trigger(self):
sample = self.gyro.get_sample()
gs = Gyro()
gs.header.frame_id = self.frame_id
gs.header.stamp = rospy.Time.now()
gs.calibration_offset.x = 0.0
gs.calibration_offset.y = 0.0
gs.calibration_offset.z = self.offset
gs.angular_velocity.x = 0.0
gs.angular_velocity.y = 0.0
gs.angular_velocity.z = (sample-self.offset)*math.pi/180.0
gs.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
self.pub.publish(gs)
imu = Imu()
imu.header.frame_id = self.frame_id
imu.header.stamp = rospy.Time.now()
imu.angular_velocity.x = 0.0
imu.angular_velocity.y = 0.0
imu.angular_velocity.z = 1*math.pi/180.0
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
self.prev_time = imu.header.stamp
(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
self.pub2.publish(imu)
def rospyrtimulib():
imuData = Imu()
print("Using settings file " + SETTINGS_FILE + ".ini")
if not os.path.exists(SETTINGS_FILE + ".ini"):
print("Settings file does not exist, will be created")
s = RTIMU.Settings(SETTINGS_FILE)
imu = RTIMU.RTIMU(s)
print("IMU Name: " + imu.IMUName())
if (not imu.IMUInit()):
print("IMU Init Failed")
sys.exit(1)
else:
print("IMU Init Succeeded")
# this is a good time to set any fusion parameters
imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)
pub = rospy.Publisher('imu', Imu, queue_size=10)
rospy.init_node('rospyrtimulib', anonymous=True)
rate = rospy.Rate(1000 / imu.IMUGetPollInterval())
while not rospy.is_shutdown():
if imu.IMURead():
# collect the data
data = imu.getIMUData()
fusionQPose = data["fusionQPose"]
imuData.orientation.x = fusionQPose[1]
imuData.orientation.y = fusionQPose[2]
imuData.orientation.z = fusionQPose[3]
imuData.orientation.w = fusionQPose[0]
gyro = data["gyro"]
imuData.angular_velocity.x = gyro[0]
imuData.angular_velocity.y = gyro[1]
imuData.angular_velocity.z = gyro[2]
accel = data["accel"]
imuData.linear_acceleration.x = accel[0] * 9.81
imuData.linear_acceleration.y = accel[1] * 9.81
imuData.linear_acceleration.z = accel[2] * 9.81
imuData.header.stamp = rospy.Time.now()
# no covariance
imuData.orientation_covariance[0] = -1
imuData.angular_velocity_covariance[0] = -1
imuData.linear_acceleration_covariance[0] = -1
imuData.header.frame_id = 'map'
# publish it
pub.publish(imuData)
rate.sleep()
def talker():
pub = rospy.Publisher('imu', Imu, queue_size=10)
rospy.init_node('imusensor', anonymous=True)
r = rospy.Rate(500)
while not rospy.is_shutdown():
#str = "hello world %s"%print_gyro().__str__()
arr = print_gyro()
i = Imu()
i.header = std_msgs.msg.Header()
i.header.frame_id="my_frame"
i.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
i.linear_acceleration.x=arr[0][0]
i.linear_acceleration.y=arr[0][1]
i.linear_acceleration.z=arr[0][2]
i.angular_velocity.x=arr[1][0]
i.angular_velocity.y=arr[1][1]
i.angular_velocity.z=arr[1][2]
i.orientation.x=0
i.orientation.y=0
i.orientation.z=0
i.orientation.w=0
#rospy.loginfo(str)
pub.publish(i)
r.sleep()
def publish_imu_data():
global imu, imu_publisher
imu_data = Imu()
imu_data.header.frame_id = "imu"
imu_data.header.stamp = rospy.Time.from_sec(imu.last_update_time)
# imu 3dmgx1 reports using the North-East-Down (NED) convention
# so we need to convert to East-North-Up (ENU) coordinates according to ROS REP103
# see http://answers.ros.org/question/626/quaternion-from-imu-interpreted-incorrectly-by-ros
q = imu.orientation
imu_data.orientation.w = q[0]
imu_data.orientation.x = q[2]
imu_data.orientation.y = q[1]
imu_data.orientation.z = -q[3]
imu_data.orientation_covariance = Config.get_orientation_covariance()
av = imu.angular_velocity
imu_data.angular_velocity.x = av[1]
imu_data.angular_velocity.y = av[0]
imu_data.angular_velocity.z = -av[2]
imu_data.angular_velocity_covariance = Config.get_angular_velocity_covariance()
la = imu.linear_acceleration
imu_data.linear_acceleration.x = la[1]
imu_data.linear_acceleration.y = la[0]
imu_data.linear_acceleration.z = - la[2]
imu_data.linear_acceleration_covariance = Config.get_linear_acceleration_covariance()
imu_publisher.publish(imu_data)
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
enc_msg = Int32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
self.heartBeat.publish("I am alive")
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0,0)
def run(self):
"""Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps.
The IMU message, if fully filled in, contains information on orientation,
acceleration (in m/s^2), and angular rate (in radians/sec). For each of
these quantities, the IMU message format also wants the corresponding
covariance matrix.
Wiimote only gives us acceleration and angular rate. So we ensure that the orientation
data entry is marked invalid. We do this by setting the first
entry of its associated covariance matrix to -1. The covariance
matrices are the 3x3 matrix with the axes' variance in the
diagonal. We obtain the variance from the Wiimote instance.
"""
rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).")
self.threadName = "IMU topic Publisher"
try:
while not rospy.is_shutdown():
(canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
msg = Imu(header=None,
orientation=None, # will default to [0.,0.,0.,0],
orientation_covariance=[-1.,0.,0.,0.,0.,0.,0.,0.,0.], # -1 indicates that orientation is unknown
angular_velocity=None,
angular_velocity_covariance=self.angular_velocity_covariance,
linear_acceleration=None,
linear_acceleration_covariance=self.linear_acceleration_covariance)
# If a gyro is plugged into the Wiimote, then note the
# angular velocity in the message, else indicate with
# the special gyroAbsence_covariance matrix that angular
# velocity is unavailable:
if self.wiistate.motionPlusPresent:
msg.angular_velocity.x = canonicalAngleRate[PHI]
msg.angular_velocity.y = canonicalAngleRate[THETA]
msg.angular_velocity.z = canonicalAngleRate[PSI]
else:
msg.angular_velocity_covariance = self.gyroAbsence_covariance
msg.linear_acceleration.x = canonicalAccel[X]
msg.linear_acceleration.y = canonicalAccel[Y]
msg.linear_acceleration.z = canonicalAccel[Z]
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
msg.header.stamp.secs = timeSecs
msg.header.stamp.nsecs = timeNSecs
self.pub.publish(msg)
rospy.logdebug("IMU state:")
rospy.logdebug(" IMU accel: " + str(canonicalAccel) + "\n IMU angular rate: " + str(canonicalAngleRate))
rospy.sleep(self.sleepDuration)
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Imu sender.")
exit(0)
def parse_imu(self, data):
'''
Given data from jaguar imu, parse and return a standard IMU message.
Return None when given bad data, or no complete message was found in data.
'''
# Use regular expressions to extract complete message
# message format: $val,val,val,val,val,val,val,val,val,val#\n
hit = re.search(r"\$-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*#", data)
if not hit:
# if there are no hits, return None
return None
else:
imu_data = hit.group(0)
try:
# Try to format the data
imu_data = imu_data[1:-1].split(",")
#Format (from drrobot docs): seq, accelx, accely, accelz, gyroY, gyroZ, gyroX, magnetonX, magnetonY, magnetonZ
seq = int(imu_data[0])
accelx = float(imu_data[1])
accely = float(imu_data[2])
accelz = float(imu_data[3])
gyroy = float(imu_data[4])
gyroz = float(imu_data[5])
gyrox = float(imu_data[6])
magnetonx = float(imu_data[7])
magnetony = float(imu_data[8])
magnetonz = float(imu_data[9])
rot_mat = [ [float(imu_data[10]), float(imu_data[11]), float(imu_data[12])],
[float(imu_data[13]), float(imu_data[14]), float(imu_data[15])],
[float(imu_data[16]), float(imu_data[17]), float(imu_data[18])] ]
except:
# bad data in match, pass
return None
else:
# data formatted fine, build message and publish
# if we didn't get a magnetometer update, set to current reading
if magnetonz == 0:
magnetonx = self.current_mag[0]
magnetony = self.current_mag[1]
magnetonz = self.current_mag[2]
# otherwise, update current magnetometer
else:
self.current_mag = [magnetonx, magnetony, magnetonz]
# Calculate quaternion from given rotation matrix
quat = rot_mat_to_quat(rot_mat);
# Build message
msg = Imu()
msg.header = Header(stamp = rospy.Time.now(), frame_id = "imu")
msg.linear_acceleration = Vector3(accelx, accely, accelz)
msg.angular_velocity = Vector3(gyrox, gyroy, gyroz)
if quat != None: msg.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
return msg
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
if(len(line) > 0):
lineParts = line.split('\t')
try:
if(lineParts[0] == 'quat'):
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
if(lineParts[0] == 'ypr'):
self._ax = float(lineParts[1])
self._ay = float(lineParts[2])
self._az = float(lineParts[3])
if(lineParts[0] == 'areal'):
self._lx = float(lineParts[1])
self._ly = float(lineParts[2])
self._lz = float(lineParts[3])
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
imu_msg.orientation.x = self._qx
imu_msg.orientation.y = self._qy
imu_msg.orientation.z = self._qz
imu_msg.orientation.w = self._qw
imu_msg.angular_velocity.x = self._ax
imu_msg.angular_velocity.y = self._ay
imu_msg.angular_velocity.z = self._az
imu_msg.linear_acceleration.x = self._lx
imu_msg.linear_acceleration.y = self._ly
imu_msg.linear_acceleration.z = self._lz
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
def spin(self):
self.prev_time = rospy.Time.now()
while not rospy.is_shutdown():
if self.calibrating:
self.calibrate()
self.calibrating = False
self.prev_time = rospy.Time.now()
acceldata = self.accelerometer.read6Reg(accel_x_low)
compassdata = self.compass.read6Reg(compass_x_high)
gyrodata = self.gyro.read6Reg(gyro_x_low)
# prepare Imu frame
imu = Imu()
imu.header.frame_id = self.frame_id
self.linear_acceleration = Vector3();
# get line from device
#str = self.ser.readline()
# timestamp
imu.header.stamp = rospy.Time.now()
#nums = str.split()
# check, if it was correct line
#if (len(nums) != 5):
# continue
self.linear_acceleration.x = self.twosComplement(acceldata[0], acceldata[1]) #/16384.0
self.linear_acceleration.y = self.twosComplement(acceldata[2], acceldata[3]) #/16384.0
self.linear_acceleration.z = self.twosComplement(acceldata[4], acceldata[5]) #/16384.0
imu.orientation.x = self.twosComplement(compassdata[1], compassdata[0]) #/1055.0,
imu.orientation.y = self.twosComplement(compassdata[3], compassdata[2]) #/1055.0,
imu.orientation.z = self.twosComplement(compassdata[5], compassdata[4]) #/950.0
imu.angular_velocity.x = self.twosComplement(gyrodata[0], gyrodata[1])
imu.angular_velocity.y = self.twosComplement(gyrodata[2], gyrodata[3])
imu.angular_velocity.z = self.twosComplement(gyrodata[4], gyrodata[5])
#gyro = int(nums[2])
#ref = int(nums[3])
#temp = int(nums[4])
#val = (ref-gyro - self.bias) * 1000 / 3 / 1024 * self.scale
#imu.angular_velocity.x = 0
#imu.angular_velocity.y = 0
#imu.angular_velocity.z = val * math.pi / 180
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
self.prev_time = imu.header.stamp
(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
self.pub.publish(imu)
def GetImuFromLine(line): (ts, ax, ay, az, gx, gy, gz) = [t(s) for t,s in zip((int,int, int, int, int, int, int),line.split())] imu = Imu() ts = float(ts)/1000000.0 ImuStamp = rospy.rostime.Time.from_sec(ts) imu.angular_velocity = create_vector3(gyro_raw_to_rads(gx), gyro_raw_to_rads(gy), gyro_raw_to_rads(gz)) imu.angular_velocity_covariance = create_diag_mat(gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0)); imu.linear_acceleration = create_vector3(acc_raw_to_ms(ax), acc_raw_to_ms(ay), acc_raw_to_ms(az)) imu.linear_acceleration_covariance = create_diag_mat(acc_raw_to_ms(1.0), acc_raw_to_ms(1.0), acc_raw_to_ms(1.0)) return (ts, ImuStamp, imu)
def handleIMU(data):
rospy.logdebug("TorsoIMU received for conversion: %f %f", data.angleX, data.angleY)
imu_msg = Imu()
q = tf.quaternion_from_euler(data.angleX, data.angleY, 0.0)
imu_msg.header = data.header
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
pub.publish(imu_msg)
def publish_imu_data (imu_data) :
mag_msg = Vector3Stamped()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.vector = Vector3(float(imu_data[0]),float(imu_data[1]),float(imu_data[2]))
mag_pub.publish(mag_msg)
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.linear_acceleration = Vector3(float(imu_data[3]),float(imu_data[4]),float(imu_data[5]))
imu_msg.angular_velocity = Vector3(float(imu_data[6]),float(imu_data[7]),float(imu_data[8]))
imu_pub.publish(imu_msg)
def Vn100Pub():
pub = rospy.Publisher('IMUData', Imu, queue_size=1)
pub2 = rospy.Publisher('IMUMag', MagneticField, queue_size=1)
# Initialize the node and name it.
rospy.init_node('IMUpub')
vn = imuthread3.Imuthread(port=rospy.get_param("imu_port"), pause=0.0)
vn.start()
vn.set_data_freq50()
vn.set_qmr_mode()
#vn.set_data_freq10() #to see if this fixes my gps drop out problem
rospy.sleep(3)
msg = Imu()
msg2 = MagneticField()
count = 0
while not rospy.is_shutdown():
if len(vn.lastreadings)>0:
if vn.lastreadings[0] =='VNQMR':
msg.header.seq = count
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'imu'
msg.orientation.x = float(vn.lastreadings[1])
msg.orientation.y = float(vn.lastreadings[2])
msg.orientation.z = float(vn.lastreadings[3])
msg.orientation.w = float(vn.lastreadings[4])
msg.orientation_covariance = [0,0,0,0,0,0,0,0,0]
msg.angular_velocity.x = float(vn.lastreadings[8])
msg.angular_velocity.y = float(vn.lastreadings[9])
msg.angular_velocity.z = float(vn.lastreadings[10])
msg.angular_velocity_covariance = [0,0,0,0,0,0,0,0,0]
msg.linear_acceleration.x = float(vn.lastreadings[11])
msg.linear_acceleration.y = float(vn.lastreadings[12])
msg.linear_acceleration.z = float(vn.lastreadings[13])
msg.linear_acceleration_covariance = [0,0,0,0,0,0,0,0,0]
msg2.header.seq = count
msg2.header.stamp = rospy.Time.now()
msg2.header.frame_id = 'imu'
msg2.magnetic_field.x = float(vn.lastreadings[5])
msg2.magnetic_field.x = float(vn.lastreadings[6])
msg2.magnetic_field.x = float(vn.lastreadings[7])
msg2.magnetic_field_covariance = [0,0,0,0,0,0,0,0,0]
#rospy.loginfo("vn100_pub " + str(msg.header.stamp) + " " + str(msg.orientation.x) + " "+ str(msg.orientation.y) + " "+ str(msg.orientation.z) + " "+ str(msg.orientation.w) + " ")
current_pose_euler = tf.transformations.euler_from_quaternion([
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
])
pub.publish(msg)
pub2.publish(msg2)
count += 1
#rospy.sleep(1.0/100.0)
vn.kill = True
def main():
rospy.init_node(NODE_NAME)
params = rospy.get_param("/imu".format(NODE_NAME),
{
'gyro': {
'zero':[-129.91, 63.81, -102.36],
'covariance':[0,0,0,
0,3.6935414044e-06,0,
0,0,0]
},
'angle':{
'zero':[0,-0.118467263978,0],
'covariance':[0,0,0,
0,3.57350008404e-05,0,
0,0,0]
}
})
rospy.set_param("/imu", params)
gyro_zeros = np.array(params['gyro']['zero'])
gyro_y_variance = params['gyro']['covariance'][4]
angle_y_zero = params['angle']['zero'][Y]
angle_y_variance = params['angle']['covariance'][4]
rate = rospy.Rate(SAMPLE_FREQ_HZ)
publisher = rospy.Publisher(IMU_TOPIC, Imu, queue_size=1, tcp_nodelay=True)
raw_publisher = rospy.Publisher(IMU_RAW_TOPIC, Imu, queue_size=1, tcp_nodelay=True)
rospy.loginfo("Starting {}, publishing at {} hz on {}".format(NODE_NAME, SAMPLE_FREQ_HZ, IMU_TOPIC))
state = Sample()
work = Sample()
sensor_reading = Sample()
raw_msg = Imu()
imu_msg = Imu()
imu_msg.angular_velocity_covariance = params['gyro']['covariance']
# imu_msg.linear_acceleration_covariance = params['accel']['covariance']
while not rospy.is_shutdown():
read_sensor(sensor_reading)
if PUBLISH_RAW:
populate_message(raw_msg, sensor_reading)
raw_publisher.publish(raw_msg)
process_reading(sensor_reading, work, state, gyro_zeros, gyro_y_variance, angle_y_zero, angle_y_variance)
populate_message(imu_msg, state)
publisher.publish(imu_msg)
rate.sleep()
def send_message(self, event=None):
if not self.valid or not self.connected:
rospy.loginfo("%s: measurements not valid, not sending imu message!", self.name)
return
# NOTE:
# the axis convention is taken from the old driver implementation
# this should be checked again and explicitly stated in the documentation
rotation = tft.quaternion_from_euler(0, 0, np.deg2rad(-self.yaw), axes="sxyz")
# NOTE:
# This is a message to hold data from an IMU (Inertial Measurement Unit)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each
# covariance matrix, and disregard the associated estimate.
msg = Imu()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame_id
msg.orientation.x = rotation[0]
msg.orientation.y = rotation[1]
msg.orientation.z = rotation[2]
msg.orientation.w = rotation[3]
# this is derived by looking at the datasheet under the bias offset factor
# and it should include the drift of the gyro if the yaw is computed by integration
msg.orientation_covariance[8] = np.deg2rad(self.yaw_std_z ** 2)
# this is to notify that this imu is not measuring this data
msg.linear_acceleration_covariance[0] = -1
msg.angular_velocity.z = np.deg2rad(-self.yaw_dot)
# this is derived by looking at the datasheet
msg.angular_velocity_covariance[8] = np.deg2rad(KVH_SIGMA ** 2)
self.pub_gyro.publish(msg)
if self.mode == KVH_MODE_R:
rospy.loginfo("%s: raw data: %+.5f -- yaw rate: %+.5f", self.name, self.data, self.yaw_dot)
else:
rospy.loginfo(
"%s: raw data: %+.5f -- yaw rate: %+.5f -- yaw: %+.5f", self.name, self.data, self.yaw_dot, self.yaw
)
def talker():
global oldTime
pubString = rospy.Publisher('sensor/string', String, queue_size=1000)
pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000)
rospy.init_node('sensornimureader', anonymous=True)
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
# sample data
currentTime = rospy.Time.now()
timeCheck = currentTime.to_sec() - oldTime.to_sec()
print timeCheck
if (timeCheck) > 2.0:
data = 'Z1.341725,103.965008,1.5100,0.0000'
oldTime = currentTime;
else: data = 'Y0.0000,0.0730,255.4516,1.5100,0.0000,0.0000,0.000,0.000,0.000'
# data = ser.readline()
pubString.publish(data)
if data[0] == 'Y':
data = data.replace("Y","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 9:
try:
##data_list structure: accel, magni, gyro
float_list = [float(i) for i in data_list]
imuData = Imu()
imuData.header.frame_id = "base_link"
imuData.header.stamp = rospy.Time.now()
##data form in yaw, pitch, roll
quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx')
imuData.orientation.x = quat[0]
imuData.orientation.y = quat[1]
imuData.orientation.z = quat[2]
imuData.orientation.w = quat[3]
imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale)
imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale)
imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale)
imuData.linear_acceleration.x = float_list[0]*accel_scale
imuData.linear_acceleration.y = -float_list[1]*accel_scale
imuData.linear_acceleration.z = -float_list[2]*accel_scale
imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6]
imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7]
imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3]
pubIMU.publish(imuData)
log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \
% (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z,
float_list[3], float_list[4], float_list[5],
imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time())
except: log = "IMU Data Error! Data : %s" % data
else: log = "Data Error! Data : %s" % data
rospy.loginfo(log)
rate.sleep()
def publish(self,event):
compass_accel = self.compass_accel.read()
compass = compass_accel[1]
accel = compass_accel[0]
gyro = self.gyro.read()
# Put together an IMU message
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = 'map'
# covariance matrix
imu_msg.orientation_covariance[0] = -1
imu_msg.angular_velocity_covariance[0] = -1
imu_msg.linear_acceleration_covariance[0] = -1
# angular velocity
imu_msg.angular_velocity.x = gyro[0][0]*self.DEG2RAD
imu_msg.angular_velocity.y = gyro[0][1]*self.DEG2RAD
imu_msg.angular_velocity.z = gyro[0][2]*self.DEG2RAD
# linear acceleration
imu_msg.linear_acceleration.x = accel[0]*self.G
imu_msg.linear_acceleration.y = accel[1]*self.G
imu_msg.linear_acceleration.z = (accel[2])*self.G
# calculate RPY
# refers to Adafruit_9DOF.cpp in GitHub
roll = math.atan2(accel[1], (accel[2]))
pitch = math.atan2(-accel[0], (accel[1] * math.sin(roll) + accel[2] * math.cos(roll)))
#yaw = math.atan2(compass[1],compass[0])
yaw = 0
#print "R=", math.degrees(roll)," P=", math.degrees(pitch), " Y=",math.degrees(yaw) # uncomment to print RPY
# convert RPY to quaternion
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
# orientation
imu_msg.orientation.x = quaternion[0]
imu_msg.orientation.y = quaternion[1]
imu_msg.orientation.z = quaternion[2]
imu_msg.orientation.w = quaternion[3]
# publish
self.pub_imu.publish(imu_msg)
# Put together a magnetometer message
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.magnetic_field.x = compass[0]
mag_msg.magnetic_field.y = compass[1]
mag_msg.magnetic_field.z = compass[2]
# publish
self.pub_mag.publish(mag_msg)
RY_msg = Float64MultiArray()
RY_msg.data = roll, pitch
self.pub_RP.publish(RY_msg)
'''self.br.sendTransform((0,0,0,),
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
if(len(line) > 0):
lineParts = line.split('\t')
try:
if(lineParts[0] == 'e'):
self._left_encoder_value = long(lineParts[1])
self._right_encoder_value = long(lineParts[2])
self._Left_Encoder.publish(self._left_encoder_value)
self._Right_Encoder.publish(self._right_encoder_value)
#if(lineParts[0] == 'b'):
#self._battery_value = float(lineParts[1])
#self._Battery_Level.publish(self._battery_value)
if(lineParts[0] == 'u'):
self._ultrasonic_value = float(lineParts[1])
self._Ultrasonic_Value.publish(self._ultrasonic_value)
if(lineParts[0] == 'i'):
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
self._qx_.publish(self._qx)
self._qy_.publish(self._qy)
self._qz_.publish(self._qz)
self._qw_.publish(self._qw)
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., ) * 9
imu_msg.angular_velocity_covariance = (-1., ) * 9
imu_msg.linear_acceleration_covariance = (-1., ) * 9
imu_msg.orientation.x = self._qx
imu_msg.orientation.y = self._qy
imu_msg.orientation.z = self._qz
imu_msg.orientation.w = self._qw
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
continue
strline = f1.readline()
while strline:
sli = strline.split(",")
strline = f1.readline()
if float(sli[0]) < float(sli0[0]):
continue
if float(sli[0]) == float(sli0[0]):
print(sli[0])
w, x, y, z = EulerAndQuaternionTransform(
[float(sli0[4]),
float(sli0[5]),
float(sli0[6])])
m_imu = Imu()
m_imu.header.stamp = rospy.Time.from_sec(float(sli[0]))
m_imu.linear_acceleration.x = float(sli[4])
m_imu.linear_acceleration.y = float(sli[5])
m_imu.linear_acceleration.z = float(sli[6])
m_imu.angular_velocity.x = float(sli[1])
m_imu.angular_velocity.y = float(sli[2])
m_imu.angular_velocity.z = float(sli[3])
m_imu.orientation.w = w
m_imu.orientation.x = x
m_imu.orientation.y = y
m_imu.orientation.z = z
outbag.write('/imu', m_imu, m_imu.header.stamp)
break
for topic, msg, t in rosbag.Bag(dst_dir + bag_name).read_messages():
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion((q.y, q.z, q.w, q.x))
#array = quaternion_from_euler(yaw + (self._angle * pi / 180), roll, pitch)
#array = quaternion_from_euler(yaw + (self._angle * pi / 180), -1 (roll - 180), -1 * pitch)
array = quaternion_from_euler((yaw + (self._angle * pi / 180)),
roll, -1 * pitch)
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def __init__(self):
#Set up udp and tcp sockets
self.host = rospy.get_param('/imu_node/host', '192.168.17.210')
print("Host set to", self.host)
self.port = 7778
self.UDP_IP = "0.0.0.0" #any interface that tries to connect on the laptop
self.addr = (self.host, self.port)
self.imu_sock = self.setup_TCP_sock()
print("TCP socket setup")
self.udp_sock = self.setup_UDP_sock()
print("UDP socket setup")
#set up ROS nodes and publisher
rospy.init_node("razor_node")
#We only care about the most recent measurement, i.e. queue_size=1
self.pub = rospy.Publisher('imu', Imu, queue_size=1)
self.srv = Server(
imuConfig,
self.reconfig_callback) # define dynamic_reconfigure callback
self.diag_pub = rospy.Publisher('diagnostics',
DiagnosticArray,
queue_size=1)
self.diag_pub_time = rospy.get_time()
#Set up IMU message
self.imuMsg = Imu()
# Orientation covariance estimation:
# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
# Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss
# Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could
# cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians
# i.e. variance in yaw: 0.0025
# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
# static roll/pitch error of 0.8%, owing to gravity orientation sensing
# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
# so set all covariances the same.
self.imuMsg.orientation_covariance = [
0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025
]
# Angular velocity covariance estimation:
# Observed gyro noise: 4 counts => 0.28 degrees/sec
# nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
# Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
self.imuMsg.angular_velocity_covariance = [
0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02
]
# linear acceleration covariance estimation:
# observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
# nonliniarity spec: 0.5% of full scale => 0.2m/s^2
# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
self.imuMsg.linear_acceleration_covariance = [
0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04
]
#accelerometer
self.accel_x_min = rospy.get_param('~accel_x_min', -250.0)
self.accel_x_max = rospy.get_param('~accel_x_max', 250.0)
self.accel_y_min = rospy.get_param('~accel_y_min', -250.0)
self.accel_y_max = rospy.get_param('~accel_y_max', 250.0)
self.accel_z_min = rospy.get_param('~accel_z_min', -250.0)
self.accel_z_max = rospy.get_param('~accel_z_max', 250.0)
# magnetometer
self.magn_x_min = rospy.get_param('~magn_x_min', -600.0)
self.magn_x_max = rospy.get_param('~magn_x_max', 600.0)
self.magn_y_min = rospy.get_param('~magn_y_min', -600.0)
self.magn_y_max = rospy.get_param('~magn_y_max', 600.0)
self.magn_z_min = rospy.get_param('~magn_z_min', -600.0)
self.magn_z_max = rospy.get_param('~magn_z_max', 600.0)
self.calibration_magn_use_extended = rospy.get_param(
'~calibration_magn_use_extended', False)
self.magn_ellipsoid_center = rospy.get_param(
'~self.magn_ellipsoid_center', [0, 0, 0])
self.magn_ellipsoid_transform = rospy.get_param(
'~self.magn_ellipsoid_transform',
[[0, 0, 0], [0, 0, 0], [0, 0, 0]])
self.imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0)
# gyroscope
self.gyro_average_offset_x = rospy.get_param('~gyro_average_offset_x',
0.0)
self.gyro_average_offset_y = rospy.get_param('~gyro_average_offset_y',
0.0)
self.gyro_average_offset_z = rospy.get_param('~gyro_average_offset_z',
0.0)
#rospy.loginfo("%f %f %f %f %f %f", accel_x_min, accel_x_max, accel_y_min, accel_y_max, accel_z_min, accel_z_max)
#rospy.loginfo("%f %f %f %f %f %f", magn_x_min, magn_x_max, magn_y_min, magn_y_max, magn_z_min, magn_z_max)
#rospy.loginfo("%s %s %s", str(calibration_magn_use_extended), str(self.magn_ellipsoid_center), str(self.magn_ellipsoid_transform[0][0]))
#rospy.loginfo("%f %f %f", gyro_average_offset_x, gyro_average_offset_y, gyro_average_offset_z)
self.roll = 0
self.pitch = 0
self.yaw = 0
self.seq = 0
self.accel_factor = 9.806 / 256.0 # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
#stop datastream
self.imu_sock.send('#o0' + '\n')
self.imu_sock.send('#o1' + '\n')
# To start display angle and sensor reading in text
self.imu_sock.send('#ox' + '\n')
rospy.loginfo("Writing calibration values to razor IMU board...")
print "Calibrating accelerometer"
#set calibration values
self.imu_sock.sendall('#caxm' + str(self.accel_x_min) + chr(13))
self.imu_sock.sendall('#caxM' + str(self.accel_x_max) + chr(13))
self.imu_sock.sendall('#caym' + str(self.accel_y_min) + chr(13))
self.imu_sock.sendall('#cayM' + str(self.accel_y_max) + chr(13))
self.imu_sock.sendall('#cazm' + str(self.accel_z_min) + chr(13))
self.imu_sock.sendall('#cazM' + str(self.accel_z_max) + chr(13))
print "Calibrating magnetometer"
#calibration values for magnetometer
if (not self.calibration_magn_use_extended):
self.imu_sock.sendall('#cmxm' + str(self.magn_x_min) + chr(13))
self.imu_sock.sendall('#cmxM' + str(self.magn_x_max) + chr(13))
self.imu_sock.sendall('#cmym' + str(self.magn_y_min) + chr(13))
self.imu_sock.sendall('#cmyM' + str(self.magn_y_max) + chr(13))
self.imu_sock.sendall('#cmzm' + str(self.magn_z_min) + chr(13))
self.imu_sock.sendall('#cmzM' + str(self.magn_z_max) + chr(13))
else:
self.imu_sock.sendall('#ccx' + str(self.magn_ellipsoid_center[0]) +
chr(13))
self.imu_sock.sendall('#ccy' + str(self.magn_ellipsoid_center[1]) +
chr(13))
self.imu_sock.sendall('#ccz' + str(self.magn_ellipsoid_center[2]) +
chr(13))
self.imu_sock.sendall('#ctxX' +
str(self.magn_ellipsoid_transform[0][0]) +
chr(13))
self.imu_sock.sendall('#ctxY' +
str(self.magn_ellipsoid_transform[0][1]) +
chr(13))
self.imu_sock.sendall('#ctxZ' +
str(self.magn_ellipsoid_transform[0][2]) +
chr(13))
self.imu_sock.sendall('#ctyX' +
str(self.magn_ellipsoid_transform[1][0]) +
chr(13))
self.imu_sock.sendall('#ctyY' +
str(self.magn_ellipsoid_transform[1][1]) +
chr(13))
self.imu_sock.sendall('#ctyZ' +
str(self.magn_ellipsoid_transform[1][2]) +
chr(13))
self.imu_sock.sendall('#ctzX' +
str(self.magn_ellipsoid_transform[2][0]) +
chr(13))
self.imu_sock.sendall('#ctzY' +
str(self.magn_ellipsoid_transform[2][1]) +
chr(13))
self.imu_sock.sendall('#ctzZ' +
str(self.magn_ellipsoid_transform[2][2]) +
chr(13))
print "Offsetting for gyro drift"
#Compensating for offset of gyro-drift
self.imu_sock.sendall('#cgx' + str(self.gyro_average_offset_x) + '\n')
self.imu_sock.sendall('#cgy' + str(self.gyro_average_offset_y) + '\n')
self.imu_sock.sendall('#cgz' + str(self.gyro_average_offset_z) + '\n')
#print calibration values for verification by user
self.imu_sock.sendall('#p' + '\n')
calib_data = self.recv_all()
calib_data_print = "Printing set calibration values:\r\n"
for line in calib_data:
calib_data_print += line
rospy.loginfo(calib_data_print)
self.imu_sock.send('#o0' + '\n')
#start datastream
self.imu_sock.send('#o1' + '\n')
#output all angles:
self.imu_sock.send('#ox' + '\n')
#automatic flush - NOT WORKING
#ser.flushInput() #discard old input, still in invalid format
#flush manually, as above command is not working - it breaks the serial connection
rospy.loginfo("Flushing first 100 IMU entries...")
for x in range(0, 100):
line = self.recv_all()
print "entry", x, ":", line
print "done flushing"
# self.imu_sock.send('#ox' + '\n')
rospy.loginfo("Publishing IMU data...")
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion
if __name__ == "__main__":
try:
rospy.init_node('imu_zero')
# publish (0,0,0,1)
pub_imu_zero = rospy.Publisher("capra/imu/zero", Imu, queue_size=10)
rate = rospy.Rate(10)
zero = Imu()
zero.header.frame_id = "odom"
zero.orientation = Quaternion(0, 0, 0, 1)
while not rospy.is_shutdown():
pub_imu_zero.publish(zero)
rate.sleep()
except rospy.ROSInterruptException:
pass
def control():
# sp = np.load("xy_sin.npy")
state_sub = rospy.Subscriber("/mavros/state",
State,
state_cb,
queue_size=10)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
acutator_control_pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",
PoseStamped,
queue_size=10)
imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=10)
local_pos_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
lp_cb,
queue_size=10)
local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity",
TwistStamped,
lv_cb,
queue_size=10)
act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub",
ActuatorControl,
act_cb,
queue_size=10)
rospy.init_node('control', anonymous=True)
rate = rospy.Rate(50.0)
# print("*"*80)
while not rospy.is_shutdown() and not current_state.connected:
rate.sleep()
rospy.loginfo(current_state.connected)
# print("#"*80)
pose = PoseStamped()
# mocap_pose = PoseStamped()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.Time.now()
i = 0
act = ActuatorControl()
flag1 = False
flag2 = False
prev_imu_data = Imu()
prev_time = rospy.Time.now()
# prev_x = 0
# prev_y = 0
# prev_z = 0
# prev_vx = 0
# prev_vy = 0
# prev_vz = 0
prev_omega_x = 0
prev_omega_y = 0
prev_omega_z = 0
prev_vx = 0
prev_vy = 0
prev_vz = 0
delta_t = 0.02
count = 0
theta = 0.0
# rospy.loginfo("Outside")
while not rospy.is_shutdown():
if current_state.mode != "OFFBOARD" and (
rospy.Time.now() - last_request > rospy.Duration(5.0)):
offb_set_mode_response = set_mode_client(offb_set_mode)
if offb_set_mode_response.mode_sent:
rospy.loginfo("Offboard enabled")
flag1 = True
last_request = rospy.Time.now()
else:
if current_state.armed == False:
arm_cmd_response = arming_client(arm_cmd)
if arm_cmd_response.success:
rospy.loginfo("Vehicle armed")
flag2 = True
last_request = rospy.Time.now()
# rospy.loginfo("Inside")
curr_time = rospy.Time.now()
if flag1 and flag2:
x_f.append(float(local_position.pose.position.x))
y_f.append(float(local_position.pose.position.y))
z_f.append(float(local_position.pose.position.z))
vx_f.append(float(local_velocity.twist.linear.x))
vy_f.append(float(local_velocity.twist.linear.y))
vz_f.append(float(local_velocity.twist.linear.z))
# print(local_velocity.twist.linear.x)
orientation = [
imu_data.orientation.w, imu_data.orientation.x,
imu_data.orientation.y, imu_data.orientation.z
]
(roll, pitch, yaw) = quaternion_to_euler_angle(
imu_data.orientation.w, imu_data.orientation.x,
imu_data.orientation.y, imu_data.orientation.z)
r_f.append(float(mt.radians(roll)))
p_f.append(float(mt.radians(pitch)))
yaw_f.append(float(mt.radians(yaw)))
sin_r_f.append(mt.sin(float(mt.radians(roll))))
sin_p_f.append(mt.sin(float(mt.radians(pitch))))
sin_y_f.append(mt.sin(float(mt.radians(yaw))))
cos_r_f.append(mt.cos(float(mt.radians(roll))))
cos_p_f.append(mt.cos(float(mt.radians(pitch))))
cos_y_f.append(mt.cos(float(mt.radians(yaw))))
rs_f.append(float(imu_data.angular_velocity.x))
ps_f.append(float(imu_data.angular_velocity.y))
ys_f.append(float(imu_data.angular_velocity.z))
ix.append(float(ixx))
iy.append(float(iyy))
iz.append(float(izz))
m.append(float(mass))
u0.append(act_controls.controls[0])
u1.append(act_controls.controls[1])
u2.append(act_controls.controls[2])
u3.append(act_controls.controls[3])
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 3
pose.pose.orientation.z = mt.sin(theta * PI / 2)
pose.pose.orientation.w = mt.cos(theta * PI / 2)
(roll, pitch,
yaw) = quaternion_to_euler_angle(pose.pose.orientation.w, 0, 0,
pose.pose.orientation.z)
x_des.append(0)
y_des.append(0)
z_des.append(3)
sin_y_des.append(yaw)
cos_yaw_des.append(yaw)
w_mag.append(0)
w_x.append(0)
w_y.append(0)
w_z.append(0)
n_t = curr_time - prev_time
#delta_t = float(n_t.nsecs) * 1e-9
a_x.append(
(float(local_velocity.twist.linear.x) - prev_vx) / delta_t)
a_y.append(
(float(local_velocity.twist.linear.y) - prev_vy) / delta_t)
a_z.append(
(float(local_velocity.twist.linear.z) - prev_vz) / delta_t)
ax_f.append(float(imu_data.linear_acceleration.x))
ay_f.append(float(imu_data.linear_acceleration.y))
az_f.append(float(imu_data.linear_acceleration.z))
prev_vx = float(local_velocity.twist.linear.x)
prev_vy = float(local_velocity.twist.linear.y)
prev_vz = float(local_velocity.twist.linear.z)
aplha_x.append(
(float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t)
aplha_y.append(
(float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t)
aplha_z.append(
(float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t)
prev_omega_x = float(imu_data.angular_velocity.x)
prev_omega_y = float(imu_data.angular_velocity.y)
prev_omega_z = float(imu_data.angular_velocity.z)
theta += 1.0 / 100
count += 1
local_pos_pub.publish(pose)
if (count >= data_points):
break
prev_time = curr_time
rate.sleep()
nn1_yaw_input_state = np.array([
vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, u3
],
ndmin=2).transpose()
nn1_yaw_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose()
nn2_yaw_input_state = np.array([
vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, u0, u1, u2
],
ndmin=2).transpose()
nn2_yaw_grd_truth = np.array([aplha_x, aplha_y, aplha_z],
ndmin=2).transpose()
print('nn1_yaw_input_state :', nn1_yaw_input_state.shape)
print('nn1_yaw_grd_truth :', nn1_yaw_grd_truth.shape)
print('nn2_yaw_input_state :', nn2_yaw_input_state.shape)
print('nn2_yaw_grd_truth :', nn2_yaw_grd_truth.shape)
np.save('nn1_yaw_input_state.npy', nn1_yaw_input_state)
np.save('nn1_yaw_grd_truth.npy', nn1_yaw_grd_truth)
np.save('nn2_yaw_input_state.npy', nn2_yaw_input_state)
np.save('nn2_yaw_grd_truth.npy', nn2_yaw_grd_truth)
s_yaw = np.array([
x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, rs_f, ps_f, ys_f, r_f, p_f, yaw_f
],
ndmin=2).transpose()
u_yaw = np.array([u0, u1, u2, u3], ndmin=2).transpose()
a_yaw = np.array([ax_f, ay_f, az_f], ndmin=2).transpose()
print('s_yaw :', s_yaw.shape)
print('u_yaw :', u_yaw.shape)
print('a_yaw :', a_yaw.shape)
np.save('s_yaw.npy', s_yaw)
np.save('u_yaw.npy', u_yaw)
np.save('a_yaw.npy', a_yaw)
(config['yaw_calibration']))
#if imu_yaw_calibration != config('yaw_calibration'):
imu_yaw_calibration = config['yaw_calibration']
rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
return config
rospy.init_node("razor_node")
#We only care about the most recent measurement, i.e. queue_size=1
pub = rospy.Publisher('imu', Imu, queue_size=1)
srv = Server(imuConfig,
reconfig_callback) # define dynamic_reconfigure callback
diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
diag_pub_time = rospy.get_time()
imuMsg = Imu()
# Orientation covariance estimation:
# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
# Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss
# Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could
# cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians
# i.e. variance in yaw: 0.0025
# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
# static roll/pitch error of 0.8%, owing to gravity orientation sensing
# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
# so set all covariances the same.
imuMsg.orientation_covariance = [0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025]
# Angular velocity covariance estimation:
# Observed gyro noise: 4 counts => 0.28 degrees/sec
publish_imu_topic = "/imu/data_raw" # For Magdwick filter convension imu_magdwick_topic = "/imu/data" velocity_command_topic = "/cmd_vel" # Po mereni smazat raw_imu_publish = "raw_data_imu" # Define static odometry information odom = Odometry() odom.header.frame_id = frame_id odom.child_frame_id = child_frame_id odom.pose.covariance = cov_pose odom.twist.covariance = cov_twist # Define static IMU information imu = Imu() imu.header.frame_id = frame_id imu.orientation_covariance = orientation_cov imu.angular_velocity_covariance = ang_vel_cov # Define static magnetic information mag = MagneticField() mag.header.frame_id = frame_id mag.magnetic_field_covariance = mag_field_cov # Define flags is_initialized = False is_calibrated = False theta_start = 0 # Calibration
#Magnetic = true + westerly declination , True= Magnetic - westerly declination
#Magnetic = true - easterly declination , True= Magnetic + easterly declination
#Digital compass heading offset in degree
# D_Compass_offset = rospy.get_param('~offset',0.)
D_Compass_declination = rospy.get_param(
'~declination', -7.462777777777778) * (math.pi / 180.0)
# By defaule IMU use Megnatic North as zero degree in Quaternion
# If we want to use it directly with GPS-UTM x,y as Global Heading, East is our zero
D_Compass_UseEastAsZero = rospy.get_param('~UseEastAsZero', True)
# use this try to control miss sync in USB-serial, when it happends, must restart
Checksum_error_limits = rospy.get_param('~Checksum_error_limits', 10.)
checksum_error_counter = 0
imu_data = Imu()
imu_data = Imu(header=rospy.Header(frame_id="SpartonCompassIMU"))
#TODO find a right way to convert imu acceleration/angularvel./orientation accuracy to covariance
imu_data.orientation_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6]
imu_data.angular_velocity_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6]
imu_data.linear_acceleration_covariance = [
1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6
]
myStr1 = '\r\n\r\nprinttrigger 0 set drop\r\n'
# this is with true north setting
# myStr2='printmask gyrop_trigger accelp_trigger or quat_trigger or yawt_trigger or time_trigger or set drop\r\n'
# this is output magnetic north
myStr2 = 'printmask gyrop_trigger accelp_trigger or quat_trigger or time_trigger or set drop\r\n'
def get_sensor_data(self):
"""Read IMU data from the sensor, parse and publish."""
# Initialize ROS msgs
imu_raw_msg = Imu()
imu_msg = Imu()
mag_msg = MagneticField()
temp_msg = Temperature()
# Initialize constants
acc_fact = 1000.0
mag_fact = 16.0
gyr_fact = 900.0
# read from sensor
buf = self.con.receive(registers.ACCEL_DATA, 45)
# Publish raw data
# TODO: convert rcl Clock time to ros time?
# imu_raw_msg.header.stamp = node.get_clock().now()
imu_raw_msg.header.frame_id = self.param.frame_id.value
# TODO: do headers need sequence counters now?
# imu_raw_msg.header.seq = seq
# TODO: make this an option to publish?
imu_raw_msg.orientation_covariance[0] = -1
imu_raw_msg.linear_acceleration.x = float(
struct.unpack('h', struct.pack('BB', buf[0],
buf[1]))[0]) / acc_fact
imu_raw_msg.linear_acceleration.y = float(
struct.unpack('h', struct.pack('BB', buf[2],
buf[3]))[0]) / acc_fact
imu_raw_msg.linear_acceleration.z = float(
struct.unpack('h', struct.pack('BB', buf[4],
buf[5]))[0]) / acc_fact
imu_raw_msg.linear_acceleration_covariance[0] = -1
imu_raw_msg.angular_velocity.x = float(
struct.unpack('h', struct.pack('BB', buf[12],
buf[13]))[0]) / gyr_fact
imu_raw_msg.angular_velocity.y = float(
struct.unpack('h', struct.pack('BB', buf[14],
buf[15]))[0]) / gyr_fact
imu_raw_msg.angular_velocity.z = float(
struct.unpack('h', struct.pack('BB', buf[16],
buf[17]))[0]) / gyr_fact
imu_raw_msg.angular_velocity_covariance[0] = -1
# node.get_logger().info('Publishing imu message')
self.pub_imu_raw.publish(imu_raw_msg)
# TODO: make this an option to publish?
# Publish filtered data
# imu_msg.header.stamp = node.get_clock().now()
imu_msg.header.frame_id = self.param.frame_id.value
q = Quaternion()
# imu_msg.header.seq = seq
q.w = float(struct.unpack('h', struct.pack('BB', buf[24], buf[25]))[0])
q.x = float(struct.unpack('h', struct.pack('BB', buf[26], buf[27]))[0])
q.y = float(struct.unpack('h', struct.pack('BB', buf[28], buf[29]))[0])
q.z = float(struct.unpack('h', struct.pack('BB', buf[30], buf[31]))[0])
# TODO(flynneva): replace with standard normalize() function
# normalize
norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w)
imu_msg.orientation.x = q.x / norm
imu_msg.orientation.y = q.y / norm
imu_msg.orientation.z = q.z / norm
imu_msg.orientation.w = q.w / norm
imu_msg.linear_acceleration.x = float(
struct.unpack('h', struct.pack('BB', buf[32],
buf[33]))[0]) / acc_fact
imu_msg.linear_acceleration.y = float(
struct.unpack('h', struct.pack('BB', buf[34],
buf[35]))[0]) / acc_fact
imu_msg.linear_acceleration.z = float(
struct.unpack('h', struct.pack('BB', buf[36],
buf[37]))[0]) / acc_fact
imu_msg.linear_acceleration_covariance[0] = -1
imu_msg.angular_velocity.x = float(
struct.unpack('h', struct.pack('BB', buf[12],
buf[13]))[0]) / gyr_fact
imu_msg.angular_velocity.y = float(
struct.unpack('h', struct.pack('BB', buf[14],
buf[15]))[0]) / gyr_fact
imu_msg.angular_velocity.z = float(
struct.unpack('h', struct.pack('BB', buf[16],
buf[17]))[0]) / gyr_fact
imu_msg.angular_velocity_covariance[0] = -1
self.pub_imu.publish(imu_msg)
# Publish magnetometer data
# mag_msg.header.stamp = node.get_clock().now()
mag_msg.header.frame_id = self.param.frame_id.value
# mag_msg.header.seq = seq
mag_msg.magnetic_field.x = \
float(struct.unpack('h', struct.pack('BB', buf[6], buf[7]))[0]) / mag_fact
mag_msg.magnetic_field.y = \
float(struct.unpack('h', struct.pack('BB', buf[8], buf[9]))[0]) / mag_fact
mag_msg.magnetic_field.z = \
float(struct.unpack('h', struct.pack('BB', buf[10], buf[11]))[0]) / mag_fact
self.pub_mag.publish(mag_msg)
# Publish temperature
# temp_msg.header.stamp = node.get_clock().now()
temp_msg.header.frame_id = self.param.frame_id.value
# temp_msg.header.seq = seq
temp_msg.temperature = float(buf[44])
self.pub_temp.publish(temp_msg)
# Print out an error if system status is in error mode.
if status == 0x01:
rospy.logerr('System error: {0}'.format(error))
rospy.logerr('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = bno.get_revision()
rospy.loginfo('Software version: {0}'.format(sw))
rospy.loginfo('Bootloader version: {0}'.format(bl))
rospy.loginfo('Accelerometer ID: 0x{0:02X}'.format(accel))
rospy.loginfo('Magnetometer ID: 0x{0:02X}'.format(mag))
rospy.loginfo('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
print('Reading BNO055 data, press Ctrl-C to quit...')
i = Imu()
ps = PoseStamped()
i.header.frame_id = 'BNO055'
try:
while True:
'''
if confMode == False and (sys != 3 or mag != 3):
print("Reloading calibration file...")
bno.set_calibration(data)
'''
# Read the Euler angles for heading, roll, pitch (all in degrees)
heading, roll, pitch = bno.read_euler()
# Read the calibration status, 0=uncalibrated and 3=fully calibrated
sys, gyro, accel, mag = bno.get_calibration_status()
print('Roll={0:0.8F} Pitch={1:0.8F} Yaw={2:0.8F}'.format(roll, pitch, yaw))
def status_callback(imu_status):
system_status, gyro_status, accel_status, mag_status = imu_status.data
print('Status: Sys={0} Mag={1} Accel={2} Gyro={3}'.format(system_status, mag_status, accel_status, gyro_status))
def accuracy_callback(accuracy):
print('Rotation Accuracy={0}'.format(accuracy.data))
# odomMsg = Odometry()
# rospy.Subscriber("/odometry/filtered", Odometry, odom_callback)
imuMsg = Imu()
rospy.Subscriber("/imu/data", Imu, imu_callback)
# statusMessage = Int8MultiArray()
# rospy.Subscriber("/imu/debug", Int8MultiArray, status_callback)
accuracyMessage = Int8MultiArray()
rospy.Subscriber("/imu/direction_accuracy", Float32, accuracy_callback)
def update_position():
rate.sleep()
while not rospy.is_shutdown():
def talker():
global global_g, deg_rad
ser.flush()
pub = rospy.Publisher('/imu/data_raw', Imu, queue_size=100)
rospy.init_node('imu_node', anonymous=True)
rate = rospy.Rate(100) # 10hz
imuMsg = Imu()
seq = 0
while not rospy.is_shutdown():
#$GTIMU,GPSWeek,GPSTime,GyroX,GyroY,GyroZ,AccX,AccY,AccZ,Tpr*cs<CR><LF>
if ser.read() != '$':
continue
line0 = ser.readline()
print line0
cs = line0[-4:-2]
print cs
cs1 = int(cs, 16)
cs2 = checksum(line0)
print("cs1,cs2", cs1, cs2)
if cs1 != cs2:
continue
if line0[0:5] == "GTIMU":
line = line0.replace("GTIMU,", "")
line = line.replace("\r\n", "")
if "\x00" in line:
continue
if not string.find('*', line):
continue
line = line.replace("*", ",")
words = string.split(line, ",")
GyroX = string.atof(words[2])
GyroY = string.atof(words[3])
GyroZ = string.atof(words[4])
AccX = string.atof(words[5])
AccY = string.atof(words[6])
AccZ = string.atof(words[7])
Tpr = string.atof(words[8])
imuMsg.linear_acceleration.x = AccX * global_g
imuMsg.linear_acceleration.y = AccY * global_g
imuMsg.linear_acceleration.z = AccZ * global_g
imuMsg.linear_acceleration_covariance[0] = 0.0001
imuMsg.linear_acceleration_covariance[4] = 0.0001
imuMsg.linear_acceleration_covariance[8] = 0.0001
imuMsg.angular_velocity.x = GyroX * deg_rad
imuMsg.angular_velocity.y = GyroY * deg_rad
imuMsg.angular_velocity.z = GyroZ * deg_rad
imuMsg.angular_velocity_covariance[0] = 0.0001
imuMsg.angular_velocity_covariance[4] = 0.0001
imuMsg.angular_velocity_covariance[8] = 0.0001
q = quaternion_from_euler(0, 0, 0)
imuMsg.orientation.x = q[0]
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'imu'
imuMsg.header.seq = seq
seq = seq + 1
pub.publish(imuMsg)
rate.sleep()
else:
continue
def update_sensors(self):
# print "accelerometer:", self._bridge.get_accelerometer()
# print "proximity:", self._bridge.get_proximity()
# print "light:", self._bridge.get_light_sensor()
# print "motor_position:", self._bridge.get_motor_position()
# print "floor:", self._bridge.get_floor_sensors()
# print "image:", self._bridge.get_image()
## If image from camera
if self.enabled_sensors['camera']:
# Get Image
image = self._bridge.get_image()
#print image
if image is not None:
nimage = np.asarray(image)
image_msg = CvBridge().cv2_to_imgmsg(nimage, "rgb8")
self.image_publisher.publish(image_msg)
if self.enabled_sensors['proximity']:
prox_sensors = self._bridge.get_proximity()
for i in range(0, 8):
if prox_sensors[i] > 0:
self.prox_msg[i].range = 0.5 / math.sqrt(
prox_sensors[i]
) # Transform the analog value to a distance value in meters (given from field tests).
else:
self.prox_msg[i].range = self.prox_msg[i].max_range
if self.prox_msg[i].range > self.prox_msg[i].max_range:
self.prox_msg[i].range = self.prox_msg[i].max_range
if self.prox_msg[i].range < self.prox_msg[i].min_range:
self.prox_msg[i].range = self.prox_msg[i].min_range
self.prox_msg[i].header.stamp = rospy.Time.now()
self.prox_publisher[i].publish(self.prox_msg[i])
# e-puck proximity positions (cm), x pointing forward, y pointing left
# P7(3.5, 1.0) P0(3.5, -1.0)
# P6(2.5, 2.5) P1(2.5, -2.5)
# P5(0.0, 3.0) P2(0.0, -3.0)
# P4(-3.5, 2.0) P3(-3.5, -2.0)
#
# e-puck proximity orentations (degrees)
# P7(10) P0(350)
# P6(40) P1(320)
# P5(90) P2(270)
# P4(160) P3(200)
self.br.sendTransform(
(0.035, -0.010, 0.034),
tf.transformations.quaternion_from_euler(0, 0, 6.11),
rospy.Time.now(), self._name + "/base_prox0",
self._name + "/base_link")
self.br.sendTransform(
(0.025, -0.025, 0.034),
tf.transformations.quaternion_from_euler(0, 0, 5.59),
rospy.Time.now(), self._name + "/base_prox1",
self._name + "/base_link")
self.br.sendTransform(
(0.000, -0.030, 0.034),
tf.transformations.quaternion_from_euler(0, 0, 4.71),
rospy.Time.now(), self._name + "/base_prox2",
self._name + "/base_link")
self.br.sendTransform(
(-0.035, -0.020, 0.034),
tf.transformations.quaternion_from_euler(0, 0, 3.49),
rospy.Time.now(), self._name + "/base_prox3",
self._name + "/base_link")
self.br.sendTransform(
(-0.035, 0.020, 0.034),
tf.transformations.quaternion_from_euler(0, 0, 2.8),
rospy.Time.now(), self._name + "/base_prox4",
self._name + "/base_link")
self.br.sendTransform(
(0.000, 0.030, 0.034),
tf.transformations.quaternion_from_euler(0, 0, 1.57),
rospy.Time.now(), self._name + "/base_prox5",
self._name + "/base_link")
self.br.sendTransform(
(0.025, 0.025, 0.034),
tf.transformations.quaternion_from_euler(0, 0, 0.70),
rospy.Time.now(), self._name + "/base_prox6",
self._name + "/base_link")
self.br.sendTransform(
(0.035, 0.010, 0.034),
tf.transformations.quaternion_from_euler(0, 0, 0.17),
rospy.Time.now(), self._name + "/base_prox7",
self._name + "/base_link")
if self.enabled_sensors['accelerometer']:
accel = self._bridge.get_accelerometer()
accel_msg = Imu()
accel_msg.header.stamp = rospy.Time.now()
accel_msg.header.frame_id = self._name + "/base_link"
accel_msg.linear_acceleration.x = (
accel[1] - 2048.0
) / 800.0 * 9.81 # 1 g = about 800, then transforms in m/s^2.
accel_msg.linear_acceleration.y = (accel[0] -
2048.0) / 800.0 * 9.81
accel_msg.linear_acceleration.z = (accel[2] -
2048.0) / 800.0 * 9.81
accel_msg.linear_acceleration_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
#print "accel raw: " + str(accel[0]) + ", " + str(accel[1]) + ", " + str(accel[2])
#print "accel (m/s2): " + str((accel[0]-2048.0)/800.0*9.81) + ", " + str((accel[1]-2048.0)/800.0*9.81) + ", " + str((accel[2]-2048.0)/800.0*9.81)
accel_msg.angular_velocity.x = 0
accel_msg.angular_velocity.y = 0
accel_msg.angular_velocity.z = 0
accel_msg.angular_velocity_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
q = tf.transformations.quaternion_from_euler(0, 0, 0)
accel_msg.orientation = Quaternion(*q)
accel_msg.orientation_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
self.accel_publisher.publish(accel_msg)
if self.enabled_sensors['motor_position']:
motor_pos = list(
self._bridge.get_motor_position()
) # Get a list since tuple elements returned by the function are immutable.
# Convert to 16 bits signed integer.
if (motor_pos[0] & 0x8000):
motor_pos[0] = -0x10000 + motor_pos[0]
if (motor_pos[1] & 0x8000):
motor_pos[1] = -0x10000 + motor_pos[1]
#print "motor_pos: " + str(motor_pos[0]) + ", " + str(motor_pos[1])
self.leftStepsDiff = motor_pos[
0] * MOT_STEP_DIST - self.leftStepsPrev # Expressed in meters.
self.rightStepsDiff = motor_pos[
1] * MOT_STEP_DIST - self.rightStepsPrev # Expressed in meters.
#print "left, right steps diff: " + str(self.leftStepsDiff) + ", " + str(self.rightStepsDiff)
self.deltaTheta = (self.rightStepsDiff - self.leftStepsDiff
) / WHEEL_DISTANCE # Expressed in radiant.
self.deltaSteps = (self.rightStepsDiff +
self.leftStepsDiff) / 2 # Expressed in meters.
#print "delta theta, steps: " + str(self.deltaTheta) + ", " + str(self.deltaSteps)
self.x_pos += self.deltaSteps * math.cos(
self.theta + self.deltaTheta / 2) # Expressed in meters.
self.y_pos += self.deltaSteps * math.sin(
self.theta + self.deltaTheta / 2) # Expressed in meters.
self.theta += self.deltaTheta # Expressed in radiant.
#print "x, y, theta: " + str(self.x_pos) + ", " + str(self.y_pos) + ", " + str(self.theta)
self.leftStepsPrev = motor_pos[
0] * MOT_STEP_DIST # Expressed in meters.
self.rightStepsPrev = motor_pos[
1] * MOT_STEP_DIST # Expressed in meters.
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = self._name + "/base_link"
odom_msg.pose.pose.position = Point(self.x_pos, self.y_pos, 0)
q = tf.transformations.quaternion_from_euler(0, 0, self.theta)
odom_msg.pose.pose.orientation = Quaternion(*q)
self.endTime = time.time()
odom_msg.twist.twist.linear.x = self.deltaSteps / (
self.endTime - self.startTime
) # self.deltaSteps is the linear distance covered in meters from the last update (delta distance);
# the time from the last update is measured in seconds thus to get m/s we multiply them.
odom_msg.twist.twist.angular.z = self.deltaTheta / (
self.endTime - self.startTime
) # self.deltaTheta is the angular distance covered in radiant from the last update (delta angle);
# the time from the last update is measured in seconds thus to get rad/s we multiply them.
#print "time elapsed = " + str(self.endTime-self.startTime) + " seconds"
self.startTime = self.endTime
self.odom_publisher.publish(odom_msg)
pos = (odom_msg.pose.pose.position.x,
odom_msg.pose.pose.position.y,
odom_msg.pose.pose.position.z)
ori = (odom_msg.pose.pose.orientation.x,
odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z,
odom_msg.pose.pose.orientation.w)
self.br.sendTransform(pos, ori, odom_msg.header.stamp,
odom_msg.child_frame_id,
odom_msg.header.frame_id)
if self.enabled_sensors['light']:
light_sensors = self._bridge.get_light_sensor()
light_sensors_marker_msg = Marker()
light_sensors_marker_msg.header.frame_id = self._name + "/base_link"
light_sensors_marker_msg.header.stamp = rospy.Time.now()
light_sensors_marker_msg.type = Marker.TEXT_VIEW_FACING
light_sensors_marker_msg.pose.position.x = 0.15
light_sensors_marker_msg.pose.position.y = 0
light_sensors_marker_msg.pose.position.z = 0.15
q = tf.transformations.quaternion_from_euler(0, 0, 0)
light_sensors_marker_msg.pose.orientation = Quaternion(*q)
light_sensors_marker_msg.scale.z = 0.01
light_sensors_marker_msg.color.a = 1.0
light_sensors_marker_msg.color.r = 1.0
light_sensors_marker_msg.color.g = 1.0
light_sensors_marker_msg.color.b = 1.0
light_sensors_marker_msg.text = "light: " + str(light_sensors)
self.light_publisher.publish(light_sensors_marker_msg)
if self.enabled_sensors['floor']:
floor_sensors = self._bridge.get_floor_sensors()
floor_marker_msg = Marker()
floor_marker_msg.header.frame_id = self._name + "/base_link"
floor_marker_msg.header.stamp = rospy.Time.now()
floor_marker_msg.type = Marker.TEXT_VIEW_FACING
floor_marker_msg.pose.position.x = 0.15
floor_marker_msg.pose.position.y = 0
floor_marker_msg.pose.position.z = 0.13
q = tf.transformations.quaternion_from_euler(0, 0, 0)
floor_marker_msg.pose.orientation = Quaternion(*q)
floor_marker_msg.scale.z = 0.01
floor_marker_msg.color.a = 1.0
floor_marker_msg.color.r = 1.0
floor_marker_msg.color.g = 1.0
floor_marker_msg.color.b = 1.0
floor_marker_msg.text = "floor: " + str(floor_sensors)
self.floor_publisher.publish(floor_marker_msg)
if self.enabled_sensors['selector']:
curr_sel = self._bridge.get_selector()
selector_marker_msg = Marker()
selector_marker_msg.header.frame_id = self._name + "/base_link"
selector_marker_msg.header.stamp = rospy.Time.now()
selector_marker_msg.type = Marker.TEXT_VIEW_FACING
selector_marker_msg.pose.position.x = 0.15
selector_marker_msg.pose.position.y = 0
selector_marker_msg.pose.position.z = 0.11
q = tf.transformations.quaternion_from_euler(0, 0, 0)
selector_marker_msg.pose.orientation = Quaternion(*q)
selector_marker_msg.scale.z = 0.01
selector_marker_msg.color.a = 1.0
selector_marker_msg.color.r = 1.0
selector_marker_msg.color.g = 1.0
selector_marker_msg.color.b = 1.0
selector_marker_msg.text = "selector: " + str(curr_sel)
self.selector_publisher.publish(selector_marker_msg)
if self.enabled_sensors['motor_speed']:
motor_speed = list(
self._bridge.get_motor_speed()
) # Get a list since tuple elements returned by the function are immutable.
# Convert to 16 bits signed integer.
if (motor_speed[0] & 0x8000):
motor_speed[0] = -0x10000 + motor_speed[0]
if (motor_speed[1] & 0x8000):
motor_speed[1] = -0x10000 + motor_speed[1]
motor_speed_marker_msg = Marker()
motor_speed_marker_msg.header.frame_id = self._name + "/base_link"
motor_speed_marker_msg.header.stamp = rospy.Time.now()
motor_speed_marker_msg.type = Marker.TEXT_VIEW_FACING
motor_speed_marker_msg.pose.position.x = 0.15
motor_speed_marker_msg.pose.position.y = 0
motor_speed_marker_msg.pose.position.z = 0.09
q = tf.transformations.quaternion_from_euler(0, 0, 0)
motor_speed_marker_msg.pose.orientation = Quaternion(*q)
motor_speed_marker_msg.scale.z = 0.01
motor_speed_marker_msg.color.a = 1.0
motor_speed_marker_msg.color.r = 1.0
motor_speed_marker_msg.color.g = 1.0
motor_speed_marker_msg.color.b = 1.0
motor_speed_marker_msg.text = "speed: " + str(motor_speed)
self.motor_speed_publisher.publish(motor_speed_marker_msg)
if self.enabled_sensors['microphone']:
mic = self._bridge.get_microphone()
microphone_marker_msg = Marker()
microphone_marker_msg.header.frame_id = self._name + "/base_link"
microphone_marker_msg.header.stamp = rospy.Time.now()
microphone_marker_msg.type = Marker.TEXT_VIEW_FACING
microphone_marker_msg.pose.position.x = 0.15
microphone_marker_msg.pose.position.y = 0
microphone_marker_msg.pose.position.z = 0.07
q = tf.transformations.quaternion_from_euler(0, 0, 0)
microphone_marker_msg.pose.orientation = Quaternion(*q)
microphone_marker_msg.scale.z = 0.01
microphone_marker_msg.color.a = 1.0
microphone_marker_msg.color.r = 1.0
microphone_marker_msg.color.g = 1.0
microphone_marker_msg.color.b = 1.0
microphone_marker_msg.text = "microphone: " + str(mic)
self.microphone_publisher.publish(microphone_marker_msg)
def main():
# init imu node
rospy.init_node('imu', anonymous=False)
# get imu config
imu_manager = imu.IMU(rospy.get_param('/ema/imu'))
# list published topics
pub = {}
for name in imu_manager.imus:
# pub[name] = rospy.Publisher('imu/' + name, String, queue_size=10)
pub[name] = rospy.Publisher('imu/' + name, Imu, queue_size=10)
# pub[name + '_buttons'] = rospy.Publisher('imu/' + name + '_buttons', Int8, queue_size=10)
# define loop rate (in hz)
rate = rospy.Rate(200)
# node loop
while not rospy.is_shutdown():
try:
timestamp = rospy.Time.now()
frame_id = 'base_link'
# POOLING MODE
if imu_manager.streaming == False:
## messages are shared by all imus
imuMsg = Imu()
imuMsg.header.stamp = timestamp
imuMsg.header.frame_id = frame_id
buttons = Int8()
for name in imu_manager.imus:
orientation = imu_manager.getQuaternion(name)
imuMsg.orientation.x = orientation[0]
imuMsg.orientation.y = orientation[1]
imuMsg.orientation.z = orientation[2]
imuMsg.orientation.w = orientation[3]
angular_velocity = imu_manager.getGyroData(name)
imuMsg.angular_velocity.x = angular_velocity[0]
imuMsg.angular_velocity.y = angular_velocity[1]
imuMsg.angular_velocity.z = angular_velocity[2]
linear_acceleration = imu_manager.getAccelData(name)
imuMsg.linear_acceleration.x = -linear_acceleration[0]
imuMsg.linear_acceleration.y = -linear_acceleration[1]
imuMsg.linear_acceleration.z = -linear_acceleration[2]
pub[name].publish(imuMsg)
buttons = imu_manager.getButtonState(name)
pub[name + '_buttons'].publish(buttons)
else:
# STREAMING MODE
if imu_manager.broadcast == False:
for name in imu_manager.imus:
# one message per imu
imuMsg = Imu()
imuMsg.header.stamp = timestamp
imuMsg.header.frame_id = frame_id
# buttons = Int8()
streaming_data = imu_manager.getStreamingData(name)
idx = 0
for slot in imu_manager.streaming_slots[name]:
#print name, slot
if slot == 'getTaredOrientationAsQuaternion':
imuMsg.orientation.x = streaming_data[idx]
imuMsg.orientation.y = streaming_data[idx + 1]
imuMsg.orientation.z = streaming_data[idx + 2]
imuMsg.orientation.w = streaming_data[idx + 3]
idx = idx + 4
elif slot == 'getNormalizedGyroRate':
imuMsg.angular_velocity.x = streaming_data[idx]
imuMsg.angular_velocity.y = streaming_data[idx
+ 1]
imuMsg.angular_velocity.z = streaming_data[idx
+ 2]
idx = idx + 3
elif slot == 'getCorrectedAccelerometerVector':
imuMsg.linear_acceleration.x = -streaming_data[
idx]
imuMsg.linear_acceleration.y = -streaming_data[
idx + 1]
imuMsg.linear_acceleration.z = -streaming_data[
idx + 2]
idx = idx + 3
print type(streaming_data)
elif slot == 'getButtonState':
if type(streaming_data) == 'tuple':
buttons = streaming_data[idx]
idx = idx + 1
else:
# imu is only streaming button state, result is not a tuple
buttons = streaming_data
# publish streamed data
pub[name].publish(imuMsg)
# pub[name + '_buttons'].publish(buttons)
# BROADCAST MODE
else:
for name in imu_manager.imus:
## one message per imu
imuMsg = Imu()
imuMsg.header.stamp = timestamp
imuMsg.header.frame_id = frame_id
buttons = Int8()
streaming_data = imu_manager.devices[
name].getStreamingBatch()
idx = 0
imuMsg.orientation.x = streaming_data[idx]
imuMsg.orientation.y = streaming_data[idx + 1]
imuMsg.orientation.z = streaming_data[idx + 2]
imuMsg.orientation.w = streaming_data[idx + 3]
idx = idx + 4
imuMsg.angular_velocity.x = streaming_data[idx]
imuMsg.angular_velocity.y = streaming_data[idx + 1]
imuMsg.angular_velocity.z = streaming_data[idx + 2]
idx = idx + 3
imuMsg.linear_acceleration.x = -streaming_data[idx]
imuMsg.linear_acceleration.y = -streaming_data[idx + 1]
imuMsg.linear_acceleration.z = -streaming_data[idx + 2]
idx = idx + 3
buttons = streaming_data[idx]
# publish streamed data
pub[name].publish(imuMsg)
pub[name + '_buttons'].publish(buttons)
except TypeError:
print 'TypeError occured!'
# sleep until it's time to work again
rate.sleep()
# cleanup
imu_manager.shutdown()
GPS[i] = False
mx = new_acc_imu[0, :] # acceleration x axis
my = new_acc_imu[1, :]
mz = new_acc_imu[2, :]
measurements = np.vstack((new_mpx, new_mpy, new_mpz, mx, my, mz))
imu_pub = rospy.Publisher('imu_hector', Imu, queue_size=20)
pose_pub = rospy.Publisher('pose_hector', Pose, queue_size=20)
rospy.init_node('nodeA', anonymous=True)
rate = rospy.Rate(100) #100hz
i = 0
while (not rospy.is_shutdown()) and i < counter:
imu_message = Imu()
acc_message = Vector3()
coordinates_gnss = Point()
position_gnss = Pose()
acc_message.x = mx[i]
acc_message.y = my[i]
acc_message.z = mz[i]
imu_message.linear_acceleration = acc_message
coordinates_gnss.x = new_mpx[i]
coordinates_gnss.y = new_mpy[i]
coordinates_gnss.z = new_mpz[i]
position_gnss.position = coordinates_gnss
imu_pub.publish(imu_message)
pose_pub.publish(position_gnss)
def __init__(self, msg):
self.id = msg.id
self.diagstatus = msg.diagstatus
self.gpoint = Point()
self.imu = Imu()
self.health = KeyValue()
def msg_process(self, msg):
msg_list = msg.split(";")
pkgSizeAcc = int(msg_list[0])
moduleSizeAcc = 4 * pkgSizeAcc + 2
acc_msg = msg_list[2:moduleSizeAcc]
pkgSizeGyro = int(msg_list[moduleSizeAcc])
moduleSizeGyro = 4 * pkgSizeGyro + 2
gyro_msg = msg_list[moduleSizeAcc + 2:moduleSizeAcc + moduleSizeGyro]
pkgSizeOrient = int(msg_list[moduleSizeAcc + moduleSizeGyro])
moduleSizeOrient = 4 * pkgSizeOrient + 2
orient_msg = msg_list[moduleSizeAcc + moduleSizeGyro +
2:moduleSizeAcc + moduleSizeGyro +
moduleSizeOrient]
biggerpkg = max([pkgSizeAcc, pkgSizeGyro, pkgSizeOrient])
imu_list = []
acceleration = Vector3()
velocity = Vector3()
orientation = Quaternion()
for i in range(0, biggerpkg):
time = ""
if i < pkgSizeGyro:
velocity = Vector3()
velocity.x = float(gyro_msg[3 * i].replace(',', '.'))
velocity.y = float(gyro_msg[3 * i + 1].replace(',', '.'))
velocity.z = float(gyro_msg[3 * i + 2].replace(',', '.'))
time = gyro_msg[3 * pkgSizeGyro + i]
if i < pkgSizeAcc:
acceleration = Vector3()
acceleration.x = float(acc_msg[3 * i].replace(',', '.'))
acceleration.y = float(acc_msg[3 * i + 1].replace(',', '.'))
acceleration.z = float(acc_msg[3 * i + 2].replace(',', '.'))
time = acc_msg[3 * pkgSizeAcc + i]
if i < pkgSizeOrient:
orientation = Quaternion()
orientation.x = float(orient_msg[3 * i].replace(',',
'.')) * 10**-6
orientation.y = float(orient_msg[3 * i + 1].replace(
',', '.')) * 10**-6
orientation.z = float(orient_msg[3 * i + 2].replace(
',', '.')) * 10**-6
orientation.w = 0
time = orient_msg[3 * pkgSizeOrient + i]
imu_message = Imu()
now = rospy.get_rostime()
imu_message.header.stamp.secs = now.secs
imu_message.header.stamp.nsecs = now.nsecs
imu_message.header.frame_id = time
imu_message.angular_velocity = velocity
imu_message.linear_acceleration = acceleration
imu_message.orientation = orientation
imu_list.append(imu_message)
return imu_list
def _create_msg(self, data):
"""
Messages published with ROS have axis from the robot frame : x forward, y ?, z up or down
"""
msg1 = Imu()
msg1_magfield = MagneticField()
msg1.header.stamp = rospy.Time.now()
msg1.header.frame_id = '/base_link'
msg1_magfield.header.stamp = rospy.Time.now()
msg1_magfield.header.frame_id = '/base_link'
msg1.linear_acceleration.x = -data["IMU1"]["accel_y"]
msg1.linear_acceleration.y = -data["IMU1"]["accel_z"]
msg1.linear_acceleration.z = data["IMU1"]["accel_x"]
msg1.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1.angular_velocity.x = -data["IMU1"]["gyro_y"]
msg1.angular_velocity.y = -data["IMU1"]["gyro_z"]
msg1.angular_velocity.z = data["IMU1"]["gyro_x"]
msg1.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1.orientation.w = 0
msg1.orientation.x = 0
msg1.orientation.y = 0
msg1.orientation.z = 0
msg1.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1_magfield.magnetic_field.x = -data["IMU1"]["mag_y"]
msg1_magfield.magnetic_field.y = -data["IMU1"]["mag_z"]
msg1_magfield.magnetic_field.z = data["IMU1"]["mag_x"]
msg2 = Imu()
msg2_magfield = MagneticField()
msg2.header.stamp = rospy.Time.now()
msg2.header.frame_id = '/base_link'
msg2_magfield.header.stamp = rospy.Time.now()
msg2_magfield.header.frame_id = '/base_link'
msg2.linear_acceleration.x = data["IMU2"]["accel_y"]
msg2.linear_acceleration.y = data["IMU2"]["accel_z"]
msg2.linear_acceleration.z = data["IMU2"]["accel_x"]
msg2.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2.angular_velocity.x = data["IMU2"]["gyro_y"]
msg2.angular_velocity.y = data["IMU2"]["gyro_z"]
msg2.angular_velocity.z = data["IMU2"]["gyro_x"]
msg2.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2.orientation.w = 0
msg2.orientation.x = 0
msg2.orientation.y = 0
msg2.orientation.z = 0
msg2.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2_magfield.magnetic_field.x = data["IMU2"]["mag_y"]
msg2_magfield.magnetic_field.y = data["IMU2"]["mag_z"]
msg2_magfield.magnetic_field.z = data["IMU2"]["mag_x"]
msg3 = Imu()
msg3_magfield = MagneticField()
msg3.header.stamp = rospy.Time.now()
msg3.header.frame_id = '/base_link'
msg3_magfield.header.stamp = rospy.Time.now()
msg3_magfield.header.frame_id = '/base_link'
msg3.linear_acceleration.x = (msg1.linear_acceleration.x +
msg2.linear_acceleration.x) / 2.
msg3.linear_acceleration.y = (msg1.linear_acceleration.y +
msg2.linear_acceleration.y) / 2.
msg3.linear_acceleration.z = (msg1.linear_acceleration.z +
msg2.linear_acceleration.z) / 2.
msg3.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg3.angular_velocity.x = (msg1.angular_velocity.x +
msg2.angular_velocity.x) / 2.
msg3.angular_velocity.y = (msg1.angular_velocity.y +
msg2.angular_velocity.y) / 2.
msg3.angular_velocity.z = (msg1.angular_velocity.z +
msg2.angular_velocity.z) / 2.
msg3.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg3.orientation.w = 0
msg3.orientation.x = 0
msg3.orientation.y = 0
msg3.orientation.z = 0
msg3.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg3_magfield.magnetic_field.x = (msg1_magfield.magnetic_field.x +
msg2_magfield.magnetic_field.x) / 2.
msg3_magfield.magnetic_field.y = (msg1_magfield.magnetic_field.y +
msg2_magfield.magnetic_field.y) / 2.
msg3_magfield.magnetic_field.z = (msg1_magfield.magnetic_field.z +
msg2_magfield.magnetic_field.z) / 2.
return msg1, msg1_magfield, msg2, msg2_magfield, msg3, msg3_magfield
IMU_FRAME = 'imu_link'
grad2rad = 3.141592654 / 180.0
if __name__ == '__main__':
rospy.init_node('adis_16488')
imu_pub = rospy.Publisher(rospy.get_name() + '/imu_data_raw', Imu)
adis_info_pub = rospy.Publisher(rospy.get_name() + '/adis_16488_info',
ADIS16488)
port = rospy.get_param("~port", "/dev/ttyACM0")
baud = int(rospy.get_param("~baud", "115200"))
ser = serial.Serial(port, baud, timeout=1)
rospy.loginfo('Publishing imu data on topics:\n' + rospy.get_name() +
'/imu_data_raw\n' + '/adis_16488_info')
imuMsg = Imu()
ADISInfo = ADIS16488()
imuMsg.orientation_covariance = [999999, 0, 0, 0, 9999999, 0, 0, 0, 999999]
imuMsg.angular_velocity_covariance = [9999, 0, 0, 0, 99999, 0, 0, 0, 0.02]
imuMsg.linear_acceleration_covariance = [0.2, 0, 0, 0, 0.2, 0, 0, 0, 0.2]
yaw = 0.0
pitch = 0.0
roll = 0.0
prod_id = 0.0
rospy.sleep(3)
while not rospy.is_shutdown():
line = ser.readline()
words = string.split(line, ",")
#print words
def __init__(self):
self.DEVICE_ID = rospy.get_param(
'~device_id', '/dev/serial/by-id/usb-SparkFun_SFE_9DOF-D21-if00')
self.imu_pub = rospy.Publisher('imu/camera_tilt', Imu, queue_size=1)
self.mag_pub = rospy.Publisher("/imu/mag_camera",
MagneticField,
queue_size=1)
self.roll_pub = rospy.Publisher("camera_roll", Float64, queue_size=1)
self.pitch_pub = rospy.Publisher("camera_pitch", Float64, queue_size=1)
self.yaw_pub = rospy.Publisher("camera_yaw", Float64, queue_size=1)
#self.mag_pub = rospy.Publisher('imu/camera_mag', MagneticField, queue_size = 1)
head1 = 0
head2 = 0
head3 = 0
head4 = 0
head5 = 0
head6 = 0
imu = Imu()
mag = MagneticField()
ser = serial.Serial(self.DEVICE_ID)
gyro = [0.0, 0.0, 0.0, 0.0]
magn = [0.0, 0.0, 0.0, 0.0]
quat = [0.0, 1.0, 0.0, 0.0, 0.0]
rate = rospy.Rate(130)
while not rospy.is_shutdown():
line = ser.readline()
accel = line.split(",", 4)
if accel[0] == 'A':
line = ser.readline()
gyro = line.split(",", 4)
line = ser.readline()
magn = line.split(",", 4)
line = ser.readline()
quat = line.split(",", 5)
line = ser.readline()
rpy = line.split(",", 4)
magneticsphere = ([
1.0974375136949293, 0.008593465160122918, 0.0003032211129407881
], [0.008593465160122857, 1.1120651652148803,
0.8271491584066613], [
0.00030322111294077105, 0.0926004380061327,
0.5424720769663267
])
bias = ([-0.021621641870448665], [-0.10661356710756408],
[0.4377993330407842])
raw_values = ([float(magn[1])], [float(magn[2])], [float(magn[3])])
shift = ([raw_values[0][0] - bias[0][0]
], [raw_values[1][0] - bias[1][0]],
[raw_values[2][0] - bias[2][0]])
magneticfield = np.dot(magneticsphere, shift)
#rospy.loginfo(magneticfield)
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = '/base_link' # i.e. '/odom'
#pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint'
imu.angular_velocity.x = float(gyro[1])
imu.angular_velocity.y = float(gyro[2])
imu.angular_velocity.z = float(gyro[3])
imu.angular_velocity_covariance = [
.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01
]
imu.linear_acceleration.x = float(accel[1])
imu.linear_acceleration.y = float(accel[2])
imu.linear_acceleration.z = float(accel[3])
imu.linear_acceleration_covariance = [
.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01
]
mag.header.stamp = rospy.Time.now()
mag.header.frame_id = '/base_link' # i.e. '/odom'
mag.magnetic_field.x = float(magn[1]) / 100.0
mag.magnetic_field.y = float(magn[2]) / 100.0
mag.magnetic_field.z = float(magn[3]) / 100.0
mag.magnetic_field_covariance = [
.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01
]
imu.orientation.w = float(quat[1])
imu.orientation.x = float(quat[2])
imu.orientation.y = float(quat[3])
imu.orientation.z = float(quat[4])
imu.orientation_covariance = [
.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01
]
#magn[2] = -float(magn[2])
#heading = math.atan2(float(magn[2]),float(magn[1]))*(180/math.pi)
#heading = math.atan2(float(magn[2]),float(magn[1]))
#if heading < 0:
# heading += 360
#rospy.loginfo(head[1])
self.roll_pub.publish(float(rpy[1]))
self.pitch_pub.publish(float(rpy[2]))
self.pitch_pub.publish(float(rpy[3]))
self.imu_pub.publish(imu)
self.mag_pub.publish(mag)
rate.sleep()
magnetic_pub = rospy.Publisher('mag_output', MagneticField, queue_size = 10)
rospy.init_node('imu_node', anonymous = True)
serial_port = rospy.get_param('~port', '/dev/pts/2')
serial_baud = rospy.get_param('~baudrate', 115200)
port = serial.Serial(serial_port, serial_baud)
rospy.logdebug("Initialization Complete")
rospy.loginfo("Publishing IMU Information")
#print'debug'
try:
while not rospy.is_shutdown():
line = port.readline()
if '$VNYMR' in line:
imu_msg = Imu()
mag_msg = MagneticField()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = "imu"
mag_msg.header.stamp = rospy.Time.now()
mag_msg.header.frame_id = "mag"
#print "debug1"
try:
#print "debug2"
#print 'debug3'
sensor_data = line.split(",")
def update_physics(delay, quadcopterId, quadcopter_controller, cmd_sub,
imu_pub, range_pub):
vol_meas = p.getBaseVelocity(quadcopterId)
one_over_sample_rate = 1 / delay
previous_publish = time.perf_counter()
try:
while not rospy.is_shutdown():
start = time.perf_counter()
# the controllers are evaluated at a slower rate, only once in the
# control_subsample times the controller is evaluated
pos_meas, quaternion_meas = p.getBasePositionAndOrientation(
quadcopterId)
previous_vol_meas = np.array(vol_meas)
vol_meas = np.array(p.getBaseVelocity(quadcopterId))
#print(vol_meas, type(vol_meas))
acc = (vol_meas - previous_vol_meas) * one_over_sample_rate
#print("VOL MEAS", previous_vol_meas, acc)
if quadcopter_controller.sample == 0:
quadcopter_controller.sample = control_subsample
#force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas)
actual_values = cmd_sub.get_control()
vectorizer = np.zeros([5, 3])
vectorizer[:, 2] = actual_values
#print("input forces ", actual_values)
force_act1, force_act2, force_act3, force_act4, moment_yaw = vectorizer
#print("Well well well\t\t", vectorizer, force_act1, moment_yaw)
else:
quadcopter_controller.sample -= 1
# apply forces/moments from controls etc:
# (do this each time, because forces and moments are reset to zero after a stepSimulation())
#assuming right is -y
p.applyExternalForce(quadcopterId, -1, force_act1,
[arm_length, -arm_length, 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act2,
[-arm_length, -arm_length, 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act3,
[-arm_length, arm_length, 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act4,
[arm_length, arm_length, 0.], p.LINK_FRAME)
# for the yaw-control:
p.applyExternalTorque(quadcopterId, -1, moment_yaw, p.LINK_FRAME)
# do simulation with pybullet:
p.stepSimulation()
header = Header()
header.frame_id = 'Body'
header.stamp = rospy.Time.now()
Tsample_physics
imu_message = Imu()
imu_message.header = header
imu_message.orientation.x = quaternion_meas[0]
imu_message.orientation.y = quaternion_meas[1]
imu_message.orientation.z = quaternion_meas[2]
imu_message.orientation.w = quaternion_meas[3]
imu_message.linear_acceleration.x = acc[0, 0]
imu_message.linear_acceleration.y = acc[0, 1]
imu_message.linear_acceleration.z = acc[0, 2]
msg = Range()
msg.header = header
msg.max_range = 200
msg.min_range = 0
msg.range = pos_meas[2]
if start - previous_publish > 1 / 100:
imu_pub.publish(imu_message)
range_pub.publish(msg)
previous_publish = start
#print("BUBLISHED RANON /simulation/infrared_sensor_node", msg.range)
# delay than repeat
calc_time = time.perf_counter() - start
if (calc_time > delay):
#print("Time to update physics is {} and is more than 20% of the desired update time ({}).".format(calc_time,delay))
pass
else:
# print("calc_time = ",calc_time)
while (time.perf_counter() - start < delay):
time.sleep(delay / 10)
except KeyboardInterrupt:
import sys
print("SYS exit in physcis update")
sys.exit()
cov_acc = rospy.get_param('~cov_acc')
cov_gnss = rospy.get_param('~cov_gnss')
cov_odom = rospy.get_param('~cov_odom')
# ROS publishers
pub_imu = rospy.Publisher('/imu', Imu, queue_size=10)
pub_odom = rospy.Publisher('/odom', Odometry, queue_size=10)
pub_gnss = rospy.Publisher('/gnss', Odometry, queue_size=10)
# Load IMU data
mat = sio.loadmat(file_path)
base_time = rospy.get_rostime()
imu_data = []
for k in range(len(mat['in_data']['IMU'][0][0]['t'][0][0])):
msg = Imu()
t = mat['in_data']['IMU'][0][0]['t'][0][0][k][0]
msg.header.stamp = base_time + rospy.Duration.from_sec(t)
msg.header.frame_id = 'base_link'
msg.linear_acceleration.x = mat['in_data']['IMU'][0][0]['acc'][0][0][0][k]
msg.linear_acceleration.y = mat['in_data']['IMU'][0][0]['acc'][0][0][1][k]
msg.linear_acceleration.z = mat['in_data']['IMU'][0][0]['acc'][0][0][2][k]
msg.linear_acceleration_covariance[0] = cov_acc
msg.linear_acceleration_covariance[4] = cov_acc
msg.linear_acceleration_covariance[8] = cov_acc
msg.angular_velocity.x = mat['in_data']['IMU'][0][0]['gyro'][0][0][0][k]
msg.angular_velocity.y = mat['in_data']['IMU'][0][0]['gyro'][0][0][1][k]
msg.angular_velocity.z = mat['in_data']['IMU'][0][0]['gyro'][0][0][2][k]
msg.angular_velocity_covariance[0] = cov_gyro
def controller(self):
rate = rospy.Rate(10) # 10hz
# self.x = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[0] # update self.x everytime
# self.y = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[1] # update self.x everytime
while not rospy.is_shutdown():
# for simulating
self.vx = self.twist.linear.x
self.vy = self.twist.linear.y
self.vth = self.twist.angular.z
self.current_time = rospy.Time.now()
# We do not need this part, we are doing our own localization
# compute odometry in a typical way given the velocities of the robot
self.dt = (self.current_time - self.last_time).to_sec()
self.delta_x = (self.vx * cos(self.th) -
self.vy * sin(self.th)) * self.dt
self.delta_y = (self.vx * sin(self.th) +
self.vy * cos(self.th)) * self.dt
self.delta_th = self.vth * self.dt
# from GPS
# self.target_location = [earth_radius, 41.103465, 29.027909] # publish to move_base_simple/goal
# from magnetometer
# self.yaw = 0
# X, Y = current location
# th = current yaw
"""
self.x += self.delta_x
self.y += self.delta_y
self.th += self.delta_th
"""
# since all odometry is 6DOF we'll need a quaternion created from yaw
self.odom_quat = tf.transformations.quaternion_from_euler(
0, 0, self.th)
# first, we'll publish the transform over tf
"""
self.odom_broadcaster.sendTransform(
(self.x, self.y, 0.),
self.odom_quat,
self.current_time,
"base_link",
"odom"
)
"""
# next, we'll publish the odometry message over ROS
self.odom = Odometry()
self.odom.header.stamp = self.current_time
self.odom.header.frame_id = "odom"
# set the position
self.odom.pose.pose = Pose(Point(self.x, self.y, 0.),
Quaternion(*self.odom_quat))
# Write a tranform formula for calculating linear.x linear.y and angular.z speed
# set the velocity
self.odom.child_frame_id = "base_link"
self.odom.twist.twist = Twist(Vector3(self.vx, self.vy, 0),
Vector3(0, 0, self.vth))
# publish the PoseWithCovarianceStamped
self.pwcs = PoseWithCovarianceStamped()
self.pwcs.header.stamp = self.odom.header.stamp
self.pwcs.header.frame_id = "odom"
self.pwcs.pose.pose = self.odom.pose.pose
self.last_time = self.current_time
# publish the NavSatFix
self.sensor_data = None
self.gps_fix = NavSatFix()
self.gps_fix.header.frame_id = "base_link"
self.gps_fix.header.stamp = self.odom.header.stamp
self.gps_fix.status.status = 0 # GPS FIXED
self.gps_fix.status.service = 1 # GPS SERVICE = GPS
# Buralar bizden gelecek
self.gps_fix.latitude = 41.24600
self.gps_fix.longitude = 29.123123
self.gps_fix.altitude = 0
self.gps_fix.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
self.gps_fix.position_covariance_type = 0
# publish the NavSatFix
self.waypoint = WayPoint()
# self.waypoint.id.uuid = [1]
self.waypoint.position.latitude = 41.24678
self.waypoint.position.longitude = 29.123100
self.waypoint.position.altitude = 0
# self.waypoint.props.key = "key"
# self.waypoint.props.value = "1"
# publish the NavSatFix
self.imuMsg = Imu()
self.imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
self.imuMsg.angular_velocity_covariance = [
0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.imuMsg.linear_acceleration_covariance = [
0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.roll = 0
self.pitch = 0
self.yaw = self.th # şimdilik
# Acceloremeter
self.imuMsg.linear_acceleration.x = 0
self.imuMsg.linear_acceleration.y = 0
self.imuMsg.linear_acceleration.z = 0
# Gyro
self.imuMsg.angular_velocity.x = 0
self.imuMsg.angular_velocity.y = 0
self.imuMsg.angular_velocity.z = 0
self.q = tf.transformations.quaternion_from_euler(
self.roll, self.pitch, self.yaw)
self.imuMsg.orientation.x = self.q[0] #magnetometer
self.imuMsg.orientation.y = self.q[1]
self.imuMsg.orientation.z = self.q[2]
self.imuMsg.orientation.w = self.q[3]
self.imuMsg.header.frame_id = "base_link"
self.imuMsg.header.stamp = self.odom.header.stamp
# Subscriber(s)
rospy.Subscriber('/husky_velocity_controller/cmd_vel', Twist,
self.callback)
rospy.Subscriber('/odometry/gps', Odometry, self.gps_callback)
# Publisher(s)
self.odom_pub.publish(self.odom)
self.odom_combined_pub.publish(self.pwcs)
self.gps_pub.publish(self.gps_fix)
self.waypoint_pub.publish(self.waypoint)
self.imu_pub.publish(self.imuMsg)
rate.sleep()
def _create_msg(self, data):
"""
Messages published with ROS have axis from the robot frame : x forward, y ?, z up or down
"""
#----IMU 1 calibrate---
msg1 = Imu()
msg1_magfield = MagneticField()
msg1.header.stamp = rospy.Time.now()
msg1.header.frame_id = '/base_link'
msg1_magfield.header.stamp = rospy.Time.now()
msg1_magfield.header.frame_id = '/base_link'
#For calibration and normalisation of linear_acceleration and magnetometer
#Calibrating file are multiplied by 1e3, need to multiply raw acceleration and magnetometer by 1e3 for concordance
self.IMU1_accX = self.w * (-data["IMU1"]["accel_y"] -
self.calibrationFileIMU1['acc_off_x'] *
1e-3) + (1 - self.w) * self.IMU1_accX
self.IMU1_accY = self.w * (-data["IMU1"]["accel_z"] -
self.calibrationFileIMU1['acc_off_y'] *
1e-3) + (1 - self.w) * self.IMU1_accY
self.IMU1_accZ = self.w * (data["IMU1"]["accel_x"] -
self.calibrationFileIMU1['acc_off_z'] *
1e-3) + (1 - self.w) * self.IMU1_accZ
msg1.linear_acceleration.x = self.IMU1_accX
msg1.linear_acceleration.y = self.IMU1_accY
msg1.linear_acceleration.z = self.IMU1_accZ
msg1.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1.angular_velocity.x = -data["IMU1"]["gyro_y"]
msg1.angular_velocity.y = -data["IMU1"]["gyro_z"]
msg1.angular_velocity.z = data["IMU1"]["gyro_x"]
msg1.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1.orientation.w = 0
msg1.orientation.x = 0
msg1.orientation.y = 0
msg1.orientation.z = 0
msg1.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1_magfield.magnetic_field.x = (
-data["IMU1"]["mag_y"] * 1e3 -
self.calibrationFileIMU1['magn_off_x']
) / self.calibrationFileIMU1['magn_scale_x']
msg1_magfield.magnetic_field.y = (
-data["IMU1"]["mag_z"] * 1e3 -
self.calibrationFileIMU1['magn_off_y']
) / self.calibrationFileIMU1['magn_scale_y']
msg1_magfield.magnetic_field.z = (
data["IMU1"]["mag_x"] * 1e3 -
self.calibrationFileIMU1['magn_off_z']
) / self.calibrationFileIMU1['magn_scale_z']
#----IMU 1 raw --------
msg1_raw = Imu()
msg1_magfield_raw = MagneticField()
msg1_raw.header.stamp = rospy.Time.now()
msg1_raw.header.frame_id = '/base_link'
msg1_magfield_raw.header.stamp = rospy.Time.now()
msg1_magfield_raw.header.frame_id = '/base_link'
msg1_raw.linear_acceleration.x = -data["IMU1"]["accel_y"]
msg1_raw.linear_acceleration.y = -data["IMU1"]["accel_z"]
msg1_raw.linear_acceleration.z = data["IMU1"]["accel_x"]
msg1_raw.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1_raw.angular_velocity.x = -data["IMU1"]["gyro_y"]
msg1_raw.angular_velocity.y = -data["IMU1"]["gyro_z"]
msg1_raw.angular_velocity.z = data["IMU1"]["gyro_x"]
msg1_raw.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1_raw.orientation.w = 0
msg1_raw.orientation.x = 0
msg1_raw.orientation.y = 0
msg1_raw.orientation.z = 0
msg1_raw.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1_magfield_raw.magnetic_field.x = -data["IMU1"]["mag_y"]
msg1_magfield_raw.magnetic_field.y = -data["IMU1"]["mag_z"]
msg1_magfield_raw.magnetic_field.z = data["IMU1"]["mag_x"]
#----IMU 2 calibrate---
msg2 = Imu()
msg2_magfield = MagneticField()
msg2.header.stamp = rospy.Time.now()
msg2.header.frame_id = '/base_link'
msg2_magfield.header.stamp = rospy.Time.now()
msg2_magfield.header.frame_id = '/base_link'
self.IMU2_accX = self.w * (data["IMU2"]["accel_y"] -
self.calibrationFileIMU2['acc_off_x'] *
1e-3) + (1 - self.w) * self.IMU2_accX
self.IMU2_accY = self.w * (data["IMU2"]["accel_z"] -
self.calibrationFileIMU2['acc_off_y'] *
1e-3) + (1 - self.w) * self.IMU2_accY
self.IMU2_accZ = self.w * (data["IMU2"]["accel_x"] -
self.calibrationFileIMU2['acc_off_z'] *
1e-3) + (1 - self.w) * self.IMU2_accZ
msg2.linear_acceleration.x = self.IMU2_accX
msg2.linear_acceleration.y = self.IMU2_accY
msg2.linear_acceleration.z = self.IMU2_accZ
msg2.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2.angular_velocity.x = data["IMU2"]["gyro_y"]
msg2.angular_velocity.y = data["IMU2"]["gyro_z"]
msg2.angular_velocity.z = data["IMU2"]["gyro_x"]
msg2.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2.orientation.w = 0
msg2.orientation.x = 0
msg2.orientation.y = 0
msg2.orientation.z = 0
msg2.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2_magfield.magnetic_field.x = (
data["IMU2"]["mag_y"] * 1e3 -
self.calibrationFileIMU2['magn_off_x']
) / self.calibrationFileIMU2['magn_scale_x']
msg2_magfield.magnetic_field.y = (
data["IMU2"]["mag_z"] * 1e3 -
self.calibrationFileIMU2['magn_off_y']
) / self.calibrationFileIMU2['magn_scale_y']
msg2_magfield.magnetic_field.z = (
data["IMU2"]["mag_x"] * 1e3 -
self.calibrationFileIMU2['magn_off_z']
) / self.calibrationFileIMU2['magn_scale_z']
#----IMU 2 raw --------
msg2_raw = Imu()
msg2_magfield_raw = MagneticField()
msg2_raw.header.stamp = rospy.Time.now()
msg2_raw.header.frame_id = '/base_link'
msg2_magfield_raw.header.stamp = rospy.Time.now()
msg2_magfield_raw.header.frame_id = '/base_link'
msg2_raw.linear_acceleration.x = data["IMU2"]["accel_y"]
msg2_raw.linear_acceleration.y = data["IMU2"]["accel_z"]
msg2_raw.linear_acceleration.z = data["IMU2"]["accel_x"]
msg2_raw.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2_raw.angular_velocity.x = data["IMU2"]["gyro_y"]
msg2_raw.angular_velocity.y = data["IMU2"]["gyro_z"]
msg2_raw.angular_velocity.z = data["IMU2"]["gyro_x"]
msg2_raw.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2_raw.orientation.w = 0
msg2_raw.orientation.x = 0
msg2_raw.orientation.y = 0
msg2_raw.orientation.z = 0
msg2_raw.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2_magfield_raw.magnetic_field.x = data["IMU2"]["mag_y"]
msg2_magfield_raw.magnetic_field.y = data["IMU2"]["mag_z"]
msg2_magfield_raw.magnetic_field.z = data["IMU2"]["mag_x"]
return msg1, msg1_magfield, msg1_raw, msg1_magfield_raw, msg2, msg2_magfield, msg2_raw, msg2_magfield_raw
def imu_callback(imu_raw):
global imu, mag, is_calibrated, samples
now = imu_raw.header.stamp
############# Po mereni smazat #####################
raw_vector6 = Imu()
raw_vector6.header.stamp = now
raw_vector6.linear_acceleration.x = imu_raw.raw_linear_acceleration.x
raw_vector6.linear_acceleration.y = imu_raw.raw_linear_acceleration.y
raw_vector6.linear_acceleration.z = imu_raw.raw_linear_acceleration.z
raw_vector6.angular_velocity.x = imu_raw.raw_angular_velocity.x
raw_vector6.angular_velocity.y = imu_raw.raw_angular_velocity.y
raw_vector6.angular_velocity.z = imu_raw.raw_angular_velocity.z
imu_raw_publisher.publish(raw_vector6)
#######################################################
if not (is_calibrated):
acc_bias[0] = acc_bias[0] + imu_raw.raw_linear_acceleration.x
acc_bias[1] = acc_bias[1] + imu_raw.raw_linear_acceleration.y
acc_bias[2] = acc_bias[2] + imu_raw.raw_linear_acceleration.z
gyro_bias[0] = gyro_bias[0] + imu_raw.raw_angular_velocity.x
gyro_bias[1] = gyro_bias[1] + imu_raw.raw_angular_velocity.y
gyro_bias[2] = gyro_bias[2] + imu_raw.raw_angular_velocity.z
samples += 1
if samples >= SamplesNumber:
is_calibrated = True
acc_bias[0] = acc_bias[0] / SamplesNumber
acc_bias[1] = acc_bias[1] / SamplesNumber
acc_bias[2] = acc_bias[2] / SamplesNumber
gyro_bias[0] = gyro_bias[0] / SamplesNumber
gyro_bias[1] = gyro_bias[1] / SamplesNumber
gyro_bias[2] = gyro_bias[2] / SamplesNumber
rospy.loginfo('IMU calibrated')
return
## Correct imu data with calibration value
imu.header.stamp = now
# Accelerometer
imu.linear_acceleration.x = imu_raw.raw_linear_acceleration.x - acc_bias[0]
imu.linear_acceleration.y = imu_raw.raw_linear_acceleration.y - acc_bias[1]
imu.linear_acceleration.z = imu_raw.raw_linear_acceleration.z - acc_bias[
2] + 9.81
# Gyroscope
imu.angular_velocity.x = imu_raw.raw_angular_velocity.x - gyro_bias[0]
imu.angular_velocity.y = imu_raw.raw_angular_velocity.y - gyro_bias[1]
imu.angular_velocity.z = imu_raw.raw_angular_velocity.z - gyro_bias[2]
# Magnetic
mag_raw = array([
imu_raw.raw_magnetic_field.x, imu_raw.raw_magnetic_field.y,
imu_raw.raw_magnetic_field.z
])
mag_raw = multiply(mag_raw, 1e-6) # Convert from microTesla to Tesla
mag_cal = M_matrix.dot((mag_raw - M_bias.T).T).T
mag.magnetic_field.x = mag_cal[0]
mag.magnetic_field.y = mag_cal[1]
mag.magnetic_field.z = mag_cal[2]
mag.header.stamp = now
## Publish topics
imu_publisher.publish(imu)
imu_mag_publisher.publish(mag)
buf_out.append(data)
try:
ser.write(buf_out)
buf_in = bytearray(ser.read(2))
# print("Writing, wr: ", binascii.hexlify(buf_out), " re: ", binascii.hexlify(buf_in))
except:
return False
if (buf_in.__len__() != 2) or (buf_in[1] != 0x01):
#rospy.logerr("Incorrect Bosh IMU device response.")
return False
return True
imu_data = Imu() # Filtered data
imu_raw = Imu() # Raw IMU data
temperature_msg = Temperature() # Temperature
mag_msg = MagneticField() # Magnetometer data
# Main function
if __name__ == '__main__':
rospy.init_node("bosch_imu_node")
# Sensor measurements publishers
pub_data = rospy.Publisher('imu/data', Imu, queue_size=1)
pub_raw = rospy.Publisher('imu/raw', Imu, queue_size=1)
pub_mag = rospy.Publisher('imu/mag', MagneticField, queue_size=1)
pub_temp = rospy.Publisher('imu/temp', Temperature, queue_size=1)
# srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback
#!/usr/bin/env python
import sys
import rospy
import numpy
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Vector3Stamped, Vector3
import tf.transformations as trans
import time
imu_data = Imu()
gps_vel = Vector3Stamped()
def imu_cb(msg):
global imu_data
imu_data = msg
def vel_cb(msg):
global gps_vel
gps_vel = msg
def vel_fix(vel):
q = imu_data.orientation
q = numpy.array([q.x, q.y, q.z, q.w])
org_vel = numpy.array([vel.x, vel.y, vel.z])
trans_vel = trans.quaternion_matrix(q).transpose()[0:3, 0:3].dot(org_vel)
corr_vel = Vector3(trans_vel[0], trans_vel[1], trans_vel[2])
# print ("corrected vel: \n{}".format(corr_vel))
# Date :22/11/2106
# This code merges stereo odometry with IMU and command velocities to produce an estimate of the HUSKY's pose and heading
import rospy
import tf.transformations
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
import math
import numpy as np
pub = rospy.Publisher("/odometry/filtered/self", Odometry, queue_size=10)
br = tf.TransformBroadcaster()
odom = Odometry()
imu = Imu()
yawViso = 0.0
vYawViso = 0.0
vX = 0.0
yawImu = 0.0
yawImu1 = 0.0
yaw = 0.0
i = 0
vYawCov = 0.09
ImuYawCov = 0.15707963267948966
yawCov = 10
cmd = np.array([(0.0, 0.0), (0.0, 0.0)])
yaw = np.array([0.0, 0.0])
meas = np.array([(0.0, 0.0), (0.0, 0.0)])
# meas = np.array ([0.0,0.0])
inp = np.array([0.0, 0.0])
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.current_time = time.time()
self.last_tiem = self.current_time
# self.zumy.cmd(0.1,0.1)
# print imu and enc data
# self.zumy.read_data()
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
# imu_data = [0,0,0,0,0,0]
# enc_data = [0,0]
self.lock.release()
twistImu = Twist()
kalman_msg = kalman()
if len(imu_data) == 6:
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
#twistImu.angular.x = 0
#twistImu.angular.y = 0
#twistImu.angular.z = imu_msg.angular_velocity.z
self.imu_pub.publish(imu_msg)
# kalman_msg = kalman()
kalman_msg.keyboard_linear = self.v
kalman_msg.keyboard_angular = -self.a*3.14
vvv = self.v
aaa = self.a*3.14
self.angular_pub.publish(-aaa)
self.linear_pub.publish(vvv)
kalman_msg.measure_angular = imu_msg.angular_velocity.z
# self.kalman_pub.publish(kalman_msg)
self.nonekalman_angular.publish(kalman_msg.measure_angular)
if len(enc_data) == 2:
enc_msg = Int16()
enc_dif_r = Int16()
enc_dif_l = Int16()
#global enc_data_r
#global enc_data_l
self.lock.acquire()
ly_daenc = numpy.load('ly_enc.npy')
enc_msg.data = enc_data[0]
self.lock.release()
self.r_enc_pub.publish(enc_msg)
#ly_daenc[0] = enc_data[0]
self.lock.acquire()
enc_data_r = ly_daenc[0]
enc_dif_r.data = enc_msg.data - enc_data_r # difference value
enc_msg.data = enc_data[1]
self.lock.release()
self.l_enc_pub.publish(enc_msg)
#ly_daenc[1] = enc_data[1]
self.lock.acquire()
enc_data_l = ly_daenc[1]
enc_dif_l.data = enc_msg.data - enc_data_l # difference value
ly_daenc = enc_data
numpy.save('ly_enc.npy',ly_daenc)
#this get the robocar`s wheels of velocity
# velocity =(( encoder`s value of change in one cycle ) / total number of encoder) * wheel`s perimeter
enc_sr = (float(enc_dif_r.data)/120.0)*30*3.14
# enc_vr =float(enc_dif_r /1167)*17.25 # it don`t work!!!wtf
enc_vr = enc_sr * 0.01725
enc_sl = (float(enc_dif_l.data)/120.0)*30*3.14
# enc_vl =float(enc_dif_l /1167)*17.25
enc_vl = enc_sl * 0.01725
#print enc_dif_r,'dif_r'
#print enc_dif_l,'dif_l'
#print enc_sr,'sr'
#print enc_sl,'sl'
#print enc_vr,'vr'
#print enc_vl,'vl'
self.lock.release()
enc_v_v = self.enc_vel(enc_vr,enc_vl)
enc_v = enc_v_v/4.4
if self.v > self.hey or self.v < self.hey:
if enc_v == self.hey:
enc_v = self.enc_v_last
self.enc_v_last = enc_v
print self.enc_v_last ,'enc_v'
enc_r_r = self.enc_rad(enc_vr,enc_vl)
enc_r = enc_r_r/3
if self.a > self.hey or self.a < self.hey:
if enc_r == 0:
enc_r = self.enc_r_last
self.enc_r_last = enc_r
print self.enc_r_last ,'enc_r'
twistImu.linear.x = self.enc_v_last
twistImu.linear.y = 0
twistImu.linear.z = 0
twistImu.angular.x = 0
twistImu.angular.y = 0
twistImu.angular.z = self.enc_r_last
kalman_msg.linear_vel = self.enc_v_last
self.kalman_pub.publish(kalman_msg)
self.enc_angular.publish(self.enc_r_last)
self.nonekalman_linear.publish(self.enc_v_last)
self.current_time = time.time()
# time_dif = self.current_time - self.last_time
# self.last_time = self.current_time
# print time_dif,'tiem_dif'
# numpy.save('ly_enc.npy',ly_daenc)
#self.ly_twist_pub.publish(twistImu)
#enc_data_r = enc_data[0]#keep last value
# enc_data_l = enc_data[1]
# print enc_data_r
# print enc_data_l
# self.lock.acquire()
# linear_output = self.PID.update(self.enc_v_last)
# angular_output = self.PID_another.update(enc_r)
# print output,'output'
# print linear_output,'linear_output'
# v = linear_output/2
# r = v + (0.2*self.a)
# l =(v/1.15) - (0.2*self.a)
# print a,'a'
# self.lock.acquire()
# vv = v*2
# self.sudu_pub.publish(vv)
# self.lock.acquire()
# self.zumy.cmd(l,r)
# self.lock.release()
time_dif = self.current_time - self.last_time
self.last_time = self.current_time
# print time_dif,'tiem_dif'
# self.sudu_pub.publish(vv)
#self.ly_twist_pub.publish(twistImu)
self.rate.sleep()
