本文整理汇总了Python中sensor_msgs.msg.Imu.header方法的典型用法代码示例。如果您正苦于以下问题：Python Imu.header方法的具体用法？Python Imu.header怎么用？Python Imu.header使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg.Imu的用法示例。
在下文中一共展示了Imu.header方法的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: talker
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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()示例2: run
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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)示例3: Publish
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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)示例4: sensor_interface
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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 (fuck 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示例5: run
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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)示例6: unpackIMU
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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 示例7: parse_imu
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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示例8: _HandleReceivedLine
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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示例9: handleIMU
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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)示例10: callback
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
def callback(raw_data, pub):
imu = Imu()
imu.header = raw_data.header
# set imu.orientation quaternion
# Btw, robot_pose_ekf expects the Roll and Pitch angles to be
# absolute, but the Yaw angle to be relative. Compute the relative
# angle from the currently reported and previously reported absolute
# angle
r = raw_data.roll
p = raw_data.pitch
y = raw_data.yaw - callback.old_yaw
callback.old_yaw = raw_data.yaw
quat = quaternion_from_euler(r,p,y,"sxyz")
imu.orientation.x = quat[0]
imu.orientation.y = quat[1]
imu.orientation.z = quat[2]
imu.orientation.w = quat[3]
#TODO: set imu.orientation_covariance
#row major about x, y, z
# According to OS5000 datasheet, accuracy
# depends on tilt:
# 0.5 deg RMS level heading,
# 1 deg typical RMS accuracy < +-30deg tilt,
# 1.5deg < +-60deg tilt
# Roll and Pitch:
# Typical 1deg accuracy for tilt < +-30 deg
# Assume tilt < +- 30 deg, set all standard
# deviations to 1 degree
std_dev = from_degrees(1)
imu.orientation_covariance[0] = std_dev**2
imu.orientation_covariance[4] = std_dev**2
# standard deviation on yaw is doubled since
# it's computed as a difference of two measurements,
# each with a std_dev of 1 degree
imu.orientation_covariance[8] = (2*std_dev)**2
# no angular velocity data is available, so set
# element 0 of covariance matrix to -1
# alternatively, set the covariance really high
imu.angular_velocity = Vector3(0,0,0)
imu.angular_velocity_covariance[0] = -1
# Set linear acceleration
imu.linear_acceleration = raw_data.acceleration
# TODO: Set linear acceleration covariance
imu.linear_acceleration_covariance[0] = -1
pub.publish(imu)示例11: convert_to_baselink
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
def convert_to_baselink(self, imu_msg, imu_model, transform):
# convert imu_msg from its frame to baselink frame
# Assumption! TODO: I'm going to assume that the axis are aligned between frames to start
# come back to this later, and rotate to align axis first
# proposed path:
# -> rorate-transform data (orientation, angular velocity, linear acceleration) to align with base_link
# -> run the same code below
'''
[sensor_msgs/Imu]:
std_msgs/Header header - DONE
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration - DONE
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance - DONE
'''
new_msg = Imu()
# Header
new_msg.header = imu_msg.header
new_msg.header.frame_id = '/base_link'
# Orientation (same based on Assumption! above)
new_msg.orientation = imu_msg.orientation
# including covariance, because same. will likely drop for rotation
new_msg.orientation_covariance = imu_msg.orientation_covariance
# Angular Velocity (same based on Assumption! above)
new_msg.angular_velocity = imu_msg.angular_velocity
# including covariance, because same. will likely drop for rotation
new_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance
# Linear Acceleration (not the same, even with assumption)
new_msg.linear_acceleration = self.solid_body_translate_lin_acc(imu_msg.linear_acceleration, imu_model, transform)
return new_msg示例12: _HandleReceivedLine
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
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示例13: default
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
def default(self, ci='unused'):
imu = Imu()
imu.header = self.get_ros_header()
imu.orientation = self.component_instance.bge_object.worldOrientation.to_quaternion()
imu.angular_velocity.x = self.data['angular_velocity'][0]
imu.angular_velocity.y = self.data['angular_velocity'][1]
imu.angular_velocity.z = self.data['angular_velocity'][2]
imu.linear_acceleration.x = self.data['linear_acceleration'][0]
imu.linear_acceleration.y = self.data['linear_acceleration'][1]
imu.linear_acceleration.z = self.data['linear_acceleration'][2]
self.publish(imu)示例14: _HandleReceivedLine
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
while len(line) > 0:
lineParts = line.split("\t")
try:
if lineParts[0] == "e":
self.angle_z = float(lineParts[1])
self._qx = 0.0
self._qy = 0.0
self._qz = math.sin(math.pi * self.angle_z / 360.0)
self._qw = math.cos(math.pi * self.angle_z / 360.0)
#######################################################################################################################
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.0,) * 9
imu_msg.angular_velocity_covariance = (-1.0,) * 9
imu_msg.linear_acceleration_covariance = (-1.0,) * 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示例15: subCB
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import header [as 别名]
def subCB(msg_in):
global pub
msg_out = Imu()
msg_out.header = msg_in.header
q = quaternion_from_euler(msg_in.RPY.x/180.0 * pi,
msg_in.RPY.y/180.0 * pi,
msg_in.RPY.z/180.0 * pi)
msg_out.orientation.x = q[0]
msg_out.orientation.y = q[1]
msg_out.orientation.z = q[2]
msg_out.orientation.w = q[3]
pub.publish(msg_out)