def publishImuData(self): imu_msg = Imu() imu_msg.header.stamp = self.frameTime #imu_msg.linear_acceleration = self.Acc imu_msg.linear_acceleration = self.AccCart if self.ref_mode: imu_msg.orientation = self.refQuat else: imu_msg.orientation = self.Quat imu_msg.angular_velocity = self.Acc #print imu_msg self.imuPub.publish(imu_msg)
def rawimu_handler(self, rawimus): imu = Imu() imu.header.stamp = rospy.Time.now() imu.header.frame_id = self.base_frame # Populate orientation field with one from inspvax message. imu.orientation = Quaternion(*self.orientation) imu.orientation_covariance = self.orientation_covariance # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = rawimus.x_gyro * 0.1 / (3600.0 * 256.0) * self.imu_rate imu.angular_velocity.y = -rawimus.my_gyro * 0.1 / ( 3600.0 * 256.0) * self.imu_rate imu.angular_velocity.z = rawimus.z_gyro * 0.1 / (3600.0 * 256.0) * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = rawimus.x_accel * 0.05 / 2**15 * self.imu_rate imu.linear_acceleration.y = rawimus.my_accel * 0.05 / 2**15 * self.imu_rate imu.linear_acceleration.z = rawimus.z_accel * 0.05 / 2**15 * self.imu_rate imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
def talker(): imu_data = Imu() odom = Odometry() imu_pub = rospy.Publisher('Imu', Imu, queue_size=1) odom_pub = rospy.Publisher('Odom', Odometry, queue_size=1) rospy.init_node('LISTEN_ODOM_IMU') rate = rospy.Rate(100) while not rospy.is_shutdown(): h = Header() h.stamp = rospy.Time.now() h.frame_id = 'IMU (BASE)' imu_data.header = h q, g, a, twist_linear, twist_angular, pose_pt = serial_listen() imu_data.orientation = q imu_data.angular_velocity = g imu_data.linear_acceleration = a covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] imu_data.orientation_covariance = covariance imu_data.angular_velocity_covariance = covariance imu_data.linear_acceleration_covariance = covariance odom.twist.twist.linear = twist_linear odom.twist.twist.angular = twist_angular odom.pose.pose.position = pose_pt imu_pub.publish(imu_data) odom_pub.publish(odom) rospy.loginfo('PUBLISHED...') rate.sleep()
def corrimudata_handler(self, corrimudata): # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body imu = Imu() imu.header.stamp = rospy.Time.now() # imu.header.frame_id = self.base_frame imu.header.frame_id = 'imu_link' # Populate orientation field with one from inspvax message. imu.orientation = Quaternion(*self.orientation) imu.orientation_covariance = self.orientation_covariance # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = corrimudata.pitch_rate * self.imu_rate imu.angular_velocity.y = corrimudata.roll_rate * self.imu_rate imu.angular_velocity.z = corrimudata.yaw_rate * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = corrimudata.x_accel * self.imu_rate imu.linear_acceleration.y = corrimudata.y_accel * self.imu_rate imu.linear_acceleration.z = corrimudata.z_accel * self.imu_rate + 9.7803 imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu)
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 convert_imu(imu): ret = Imu() ret.header = convert_utime(imu.utime) ret.orientation = rpy_to_quat(*imu.tb_angles) ret.orientation_covariance = [0]*9 gyro = imu.gyro w = ret.angular_velocity w.x, w.y, w.z = gyro # 5 dps tolerance at 0 with 3 percent linear tolerance # 2 percent cross axis sensitivity rps = 5/180*math.pi cross = 0.02**2 ret.angular_velocity_covariance = [ (rps + gyro[0]*.03)**2, cross*(gyro[0]*gyro[1]), cross*(gyro[0]*gyro[2]), cross*(gyro[0]*gyro[1]), (rps + gyro[1]*.03)**2, cross*(gyro[1]*gyro[2]), cross*(gyro[0]*gyro[2]), cross*(gyro[1]*gyro[2]), (rps + gyro[2]*.03)**2 ] accel = imu.accel a = ret.linear_acceleration a.x, a.y, a.z = accel # 3 percent sensitivity with 2 percent cross axis sensitivity ret.linear_acceleration_covariance = [ (accel[0]*0.03)**2, cross*(accel[0]*accel[1]), cross*(accel[0]*accel[2]), cross*(accel[0]*accel[1]), (accel[1]*0.03)**2, cross*(accel[1]*accel[2]), cross*(accel[0]*accel[2]), cross*(accel[1]*accel[2]), (accel[2]*0.03)**2 ] return ret
def imu_publisher(): global accl_x, accl_y,accl_z, gyro_x, gyro_y, gyro_z, comp_x, comp_y, comp_z, quad_x, quad_y, quad_z, quad_w; rospy.init_node('Imu_bno_node', anonymous = False) # if anonymous=True is to ensure that node is unique by adding random number in the end r = rospy.Rate(20.0) current_time = rospy.Time.now() curn_time = time.time() last_time = time.time() new_time = 0.0 rospy.loginfo(" Streaming IMU Data ..") while not rospy.is_shutdown(): current_time = rospy.Time.now() read_imu_raw_data() imu = Imu() imu.header.stamp = rospy.Time.now() imu.orientation = Quaternion(quad_x, quad_y, quad_z, quad_w) imu.angular_velocity = Vector3(gyro_x, gyro_y, gyro_z) imu.linear_acceleration = Vector3(accl_x, accl_y, accl_z) imu_data_pub.publish(imu) mag = MagneticField() mag.header.stamp = current_time mag.header.frame_id = "imu/mag" mag.magnetic_field = Vector3(comp_x, comp_y, comp_z) imu_mag_pub.publish(mag) r.sleep()
def update_sensors(self): # Accelerometer accel = self._driver.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)
def imuResponseCallback(self, msg): ############################################################################# imu_msg = Imu() imu_msg.header = msg.header imu_msg.orientation = msg.orientation imu_msg.angular_velocity = msg.angular_velocity imu_msg.linear_acceleration = msg.linear_acceleration imu_msg.orientation_covariance[0] = msg.orientation_covariance[0] imu_msg.orientation_covariance[4] = msg.orientation_covariance[4] if self.rtkEnabled == 0: imu_msg.orientation_covariance[8] = msg.orientation_covariance[8] else: imu_msg.orientation_covariance[8] = msg.orientation_covariance[ 8] + 99999 / (1 + math.exp(-8 * (abs(self.totalVel) - 2)) ) # sigmoid function to mix yaw from the IMU imu_msg.angular_velocity_covariance[ 0] = msg.angular_velocity_covariance[0] imu_msg.angular_velocity_covariance[ 4] = msg.angular_velocity_covariance[4] imu_msg.angular_velocity_covariance[ 8] = msg.angular_velocity_covariance[8] imu_msg.linear_acceleration_covariance[ 0] = msg.linear_acceleration_covariance[0] imu_msg.linear_acceleration_covariance[ 4] = msg.linear_acceleration_covariance[4] imu_msg.linear_acceleration_covariance[ 8] = msg.linear_acceleration_covariance[8] #rospy.logerr("Vel: %f Z Rot Covariance: %f", self.totalVel, imu_msg.orientation_covariance[8]) self.imuPub.publish(imu_msg)
def publish_imu(self, pose, twist): msg = Imu() msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'imu_link' msg.orientation = pose.orientation msg.angular_velocity = twist.angular self.imu_pub.publish(msg)
def init_values(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): i = Imu() i.header.stamp = rospy.Time.now() i.header.frame_id = 'Riptide_INS' i.orientation = create_quaternion(0, 0, 0, 0) i.angular_velocity = create_vector3(1, 2, 3) self.imu_pub.publish(i) gpsmsg = NavSatFix() gpsmsg.header.stamp = rospy.Time.now() gpsmsg.header.frame_id = "gps" gpsmsg.latitude = float(0) # /!\ change here gpsmsg.longitude = float(0) # /!\ change here self.gps_pub.publish(gpsmsg) depth = Float32() depth.data = 1 # /!\ change here self.depth_pub.publish(Float32(data=1)) rate.sleep()
def odom_subscriber_cb(data): imu_msg = Imu() imu_msg.header = data.header imu_msg.header.frame_id = 'base_link' imu_msg.orientation = data.pose.pose.orientation imu_msg.linear_acceleration.z = -10 pub.publish(imu_msg)
def publish(self, theta, omega): msg = Imu() msg.header = self.header msg.angular_velocity.x = omega[0]*1 msg.angular_velocity.y = omega[1]*1 msg.angular_velocity.z = omega[2]*1 msg.orientation = self.thetaToOrientation(theta) grav = np.array([0,0,self.gravity], dtype=np.float64) r = self.thetaToRotation(theta) grav = r.apply(grav, True) pureAcceleration = self.accel - grav msg.linear_acceleration.x = pureAcceleration[0]*1 msg.linear_acceleration.y = pureAcceleration[1]*1 msg.linear_acceleration.z = pureAcceleration[2]*1 cov = np.zeros(9, dtype=np.float64) msg.angular_velocity_covariance = cov msg.orientation_covariance = cov msg.linear_acceleration_covariance = cov self.publisher.publish(msg)
def read_from_imu(self): while True: try: imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = 'imu' orientation = self.bno.read_quaternion() linear_acceleration = self.bno.read_linear_acceleration() angular_velocity = self.bno.read_gyroscope() imu_msg.orientation_covariance[0] = -1 imu_msg.linear_acceleration_covariance[0] = -1 imu_msg.angular_velocity_covariance[0] = -1 imu_msg.orientation = Quaternion(orientation[1], orientation[2], orientation[3], orientation[0]) imu_msg.linear_acceleration = Vector3(linear_acceleration[0], linear_acceleration[1], linear_acceleration[2]) imu_msg.angular_velocity = Vector3(np.deg2rad(angular_velocity[0]), np.deg2rad(angular_velocity[1]), np.deg2rad(angular_velocity[2])) return imu_msg except IOError: pass
def on_imu(channel, data): m = ins_t.decode(data) pyTime = rospy.Time.from_sec(m.utime * 1E-6) #print "Received ins message", pyTime pose = PoseStamped() t = rospy.get_rostime() pose.header.stamp = pyTime pose.header.frame_id = 'imu_link' pose.pose.orientation.w = m.quat[0] pose.pose.orientation.x = m.quat[1] pose.pose.orientation.y = m.quat[2] pose.pose.orientation.z = m.quat[3] imuRosPosePub.publish(pose) imu = Imu() imu.header = pose.header imu.orientation = pose.pose.orientation imu.angular_velocity.x = m.gyro[0] imu.angular_velocity.y = m.gyro[1] imu.angular_velocity.z = m.gyro[2] imu.linear_acceleration.x = m.accel[0] imu.linear_acceleration.y = m.accel[1] imu.linear_acceleration.z = m.accel[2] imuRosPub.publish(imu)
def publish_imu(pub, ego_vehicle, sensor_file): if not hasattr(publish_imu, 'imu_rot'): with open(sensor_file) as f: json_sensors = json.loads(f.read()) for sensor_spec in json_sensors['sensors']: if 'imu' not in sensor_spec['type']: continue temp = np.array([sensor_spec['roll'], -sensor_spec['pitch'], -sensor_spec['yaw']]) publish_imu.imu_rot = Rotation.from_euler('xyz', temp, degrees=True) carla_accel = ego_vehicle.get_acceleration() carla_omega = ego_vehicle.get_angular_velocity() carla_rot = ego_vehicle.get_transform().rotation msg = Imu() msg.header.frame_id = "tamagawa/imu_link" msg.header.stamp = increment_clock.time r_body = Rotation.from_euler('xyz', transforms.carla_rotation_to_RPY(carla_rot), degrees=False) r = r_body * publish_imu.imu_rot imu_accel = r.inv().apply(transforms.carla_velocity_to_numpy_vector(carla_accel)) # func wants vel, but its the same for accel imu_omega = r.inv().apply(np.deg2rad(np.array([carla_omega.x, -carla_omega.y, -carla_omega.z]))) msg.linear_acceleration.x = imu_accel[0] msg.linear_acceleration.y = imu_accel[1] msg.linear_acceleration.z = imu_accel[2] - 9.8 # they want gravity subtracted msg.angular_velocity.x = imu_omega[0] msg.angular_velocity.y = imu_omega[1] msg.angular_velocity.z = imu_omega[2] re = r.as_euler('xyz', degrees=False) quat = tf.transformations.quaternion_from_euler(re[0], re[1], re[2]) ros_quaternion = transforms.numpy_quaternion_to_ros_quaternion(quat) msg.orientation = ros_quaternion pub.publish(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 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 publish_sensor_data(self, sensor_data): acc = np.array(sensor_data.acceleration.data) / 1000.0 * g ang_vel = np.array(sensor_data.angular_vel.data) / 180.0 * np.pi # ang_vel = rotate_v(ang_vel.tolist(), self.rotation) msg = Imu() msg.header.frame_id = self.base_frame_id msg.header.stamp = rospy.Time.now() msg.linear_acceleration = Vector3(*acc) # It happens that sensor_data.quaterion and pose.quaternion are NOT in the same frame q = sensor_data.quaternion.data q = [q[1], q[2], q[3], q[0]] msg.orientation = Quaternion(*q) # msg.orientation = Quaternion(*self.orientation) msg.angular_velocity = Vector3(*ang_vel) msg.linear_acceleration_covariance = self.linear_acceleration_cov msg.orientation_covariance = self.orientation_cov msg.angular_velocity_covariance = self.angular_velocity_cov self.imu_pub.publish(msg) mag = np.array(sensor_data.magnetic.data) * 1e-6 m_msg = MagneticField() m_msg.header = msg.header m_msg.magnetic_field = Vector3(*mag) m_msg.magnetic_field_covariance = self.magnetic_field_cov self.mag_pub.publish(m_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 sensor_data_updated(self, carla_imu_measurement): """ Function to transform a received imu measurement into a ROS Imu message :param carla_imu_measurement: carla imu measurement object :type carla_imu_measurement: carla.IMUMeasurement """ imu_msg = Imu() imu_msg.header = self.get_msg_header(timestamp=carla_imu_measurement.timestamp) imu_msg.angular_velocity.x = carla_imu_measurement.gyroscope.x imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y imu_msg.angular_velocity.z = carla_imu_measurement.gyroscope.z imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x imu_msg.linear_acceleration.y = carla_imu_measurement.accelerometer.y imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z imu_rotation = carla_imu_measurement.transform.rotation quat = tf.transformations.quaternion_from_euler(math.radians(imu_rotation.roll), math.radians(imu_rotation.pitch), math.radians(imu_rotation.yaw)) imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat) self.publish_message( self.get_topic_prefix(), imu_msg)
def send_imu_msgs(self): """ send imu messages with respect to ego_vehicle frame :return: """ vehicle_acc_wrt_world_carla = self.carla_actor.get_acceleration() vehicle_ang_vel_wrt_world_carla = self.carla_actor.get_angular_velocity( ) vehicle_orientation_wrt_world_carla = self.carla_actor.get_transform( ).rotation vehicle_orientation_wrt_world_ros = transforms.carla_rotation_to_ros_quaternion( vehicle_orientation_wrt_world_carla) tmp_twist = transforms.carla_velocity_to_ros_twist( vehicle_acc_wrt_world_carla, vehicle_ang_vel_wrt_world_carla, vehicle_orientation_wrt_world_carla) vehicle_acc_wrt_body_ros, vehicle_ang_vel_wrt_body_ros = tmp_twist.linear, tmp_twist.angular imu_info = Imu(header=self.get_msg_header()) imu_info.header.frame_id = self.get_prefix() imu_info.orientation = vehicle_orientation_wrt_world_ros imu_info.angular_velocity = vehicle_ang_vel_wrt_body_ros imu_info.linear_acceleration = vehicle_acc_wrt_body_ros imu_info.linear_acceleration.z += 9.8 self.publish_message(self.get_topic_prefix() + "/imu", imu_info)
def sensor_data_updated(self, carla_imu_measurement): """ Function to transform a received imu measurement into a ROS Imu message :param carla_imu_measurement: carla imu measurement object :type carla_imu_measurement: carla.IMUMeasurement """ imu_msg = Imu() imu_msg.header = self.get_msg_header(timestamp=carla_imu_measurement.timestamp) # Carla uses a left-handed coordinate convention (X forward, Y right, Z up). # Here, these measurements are converted to the right-handed ROS convention # (X forward, Y left, Z up). imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z imu_rotation = carla_imu_measurement.transform.rotation quat = trans.carla_rotation_to_numpy_quaternion(imu_rotation) imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat) self.publish_message( self.get_topic_prefix(), imu_msg)
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 publish_marker(self, e): # create IMU message and publish imu_msg = Imu() imu_msg.header.stamp = rospy.get_rostime() imu_msg.header.frame_id = "imu" imu_msg.orientation = self.pose.orientation self.imu_publisher.publish(imu_msg)
def publishImuData(self): imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.linear_acceleration = self.Acc imu_msg.orientation = self.normQuat imu_msg.angular_velocity = self.Gyro self.imuPub.publish(imu_msg)
def pub_imu(broadcast): global theta imu_msg = Imu() imu_msg.header.stamp = rospy.get_rostime() robot_id = rospy.get_namespace().replace('/', '') imu_msg.header.frame_id = '{}_yaw_earth'.format(robot_id) #it contain only yaw quaternion = Quaternion(*tf.transformations.quaternion_from_euler(0,0,theta)) imu_msg.orientation = quaternion return imu_msg
def publish_imu_data(self): imu_msg = Imu() imu_msg.orientation = self.__get_quaternions() # imu_msg.orientation_covariance= imu_msg.angular_velocity = self.__get_angular_velocity() # imu_msg.angular_velocity_covariance= imu_msg.linear_acceleration = self.__get_linear_acceleration() # imu_msg.linear_acceleration_covariance= return imu_msg
def angles(self, data): orientation_and_pose = Imu() orientation_and_pose.orientation = data.orientation orientation_list = [ data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w ] (R, P, Y) = euler_from_quaternion(orientation_list) self.RPY[0] = degrees(R) self.RPY[1] = degrees(P) self.RPY[2] = degrees(Y)
def imuCallback(data): imuData = Imu() imuData.header.stamp = rospy.Time.now() imuData.header.frame_id = 'imu_link' imuData.orientation = data.orientation imuData.orientation_covariance = data.orientation_covariance imuData.angular_velocity = data.angular_velocity imuData.angular_velocity_covariance = data.angular_velocity_covariance imuData.linear_acceleration = data.linear_acceleration imuData.linear_acceleration_covariance = data.linear_acceleration_covariance pub.publish(imuData)
def __update__(self, imu:Imu): ''' Update routine, to be called by subscribers. Args: imu [Imu]: The ROS message 'sensor_msgs.msg.Imu'. ''' o = imu.orientation source = R.from_quat([o.x, o.y, o.z, o.w]) remapped = np.matmul(self.target.as_matrix(), source.as_matrix()) imu.orientation = Quaternion(*R.from_matrix(remapped).as_quat()) self.pub.publish(imu)
def talker(): pub = rospy.Publisher('Imu', Imu, queue_size=10) rospy.init_node('talker') rate = rospy.Rate(50) msg = Imu() for i in range(9): msg.orientation_covariance[i] = -1 while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() # rad_to_deg = 180/pi #getting linear_accleration #converts G's to m/s^2 as per ros spec accel = mpu9250.readAccel() ax = (accel['x']) / 9.8 ay = (accel['y']) / 9.8 az = (accel['z']) / 9.8 #getting angular velocity # gyro is set to deg/s to rad/s as per ros spec gyro = mpu9250.readGyro() gx = (gyro['x']) * deg_to_rad gy = (gyro['y']) * deg_to_rad gz = (gyro['z']) * deg_to_rad #getting quaternion ''' x = math.sin(gy)*math.sin(gz)*math.cos(gx)+math.cos(gy)*math.cos(gz)*math.sin(gx) y = math.sin(gy)*math.cos(gz)*math.cos(gx)+math.cos(gy)*math.sin(gz)*math.sin(gx) z = math.cos(gy)*math.sin(gz)*math.cos(gx)-math.sin(gy)*math.cos(gz)*math.sin(gx) w = math.cos(gy)*math.cos(gz)*math.cos(gx)-math.sin(gy)*math.sin(gz)*math.sin(gx) ''' x = 0 y = 0 z = 0 w = 0 #setting av msg.angular_velocity = Vector3(gx, gy, gz) #setting la msg.linear_acceleration = Vector3(ax, ay, az) #setting quate msg.orientation = Quaternion(x, y, z, w) rospy.loginfo(msg) pub.publish(msg) rate.sleep()
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
def run(self): pub = rospy.Publisher('/um7', Imu, queue_size=10) rate = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): self.UM7(self.um7_serial) mesg = Imu() mesg.header.stamp = rospy.Time.now() mesg.header.frame_id = 'UM7' mesg.orientation = self.q mesg.angular_velocity = Vector3(*self.velocities) pub.publish(mesg) rate.sleep()
def publish(self, data): msg = Imu() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.orientation = data.getOrientation() 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 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)
def map_imu(values): """ Map the values generated with the hypothesis-ros imu strategy to a rospy imu type. """ if not isinstance(values, _Imu): raise TypeError('Wrong type. Use appropriate hypothesis-ros type.') ros_imu = Imu() ros_imu.header = values.header ros_imu.orientation = values.orientation ros_imu.orientation_covariance = values.orientation_covariance ros_imu.angular_velocity = values.angular_velocity ros_imu.angular_velocity_covariance = values.angular_velocity_covariance ros_imu.linear_acceleration = values.linear_acceleration ros_imu.linear_acceleration_covariance = values.linear_acceleration_covariance return ros_imu
def publishImuMsg(self): #This code do not consider covariance tmp = Imu() tmp.header.frame_id = "base_link" tmp.orientation = self.orientation tmp.linear_acceleration.x = self.twist.linear.x - self.last_values[0] tmp.linear_acceleration.y = self.twist.linear.y - self.last_values[1] tmp.angular_velocity.z = self.twist.angular.z - self.last_values[2] self.imu_Publisher.publish(tmp) self.last_values = [ tmp.linear_acceleration.x, tmp.linear_acceleration.y, tmp.angular_velocity.z ]
def imu_cb(self, imu_msg): new_imu_msg = Imu() new_imu_msg.header = imu_msg.header new_imu_msg.orientation = imu_msg.orientation new_imu_msg.orientation_covariance = imu_msg.orientation_covariance new_imu_msg.angular_velocity = imu_msg.angular_velocity new_imu_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance new_imu_msg.linear_acceleration = imu_msg.linear_acceleration new_imu_msg.linear_acceleration_covariance = imu_msg.linear_acceleration_covariance self.imu_pub.publish(new_imu_msg)
def default(self, ci='unused'): """ Publish the data of the IMU sensor as a custom ROS Imu message """ imu = Imu() imu.header = self.get_ros_header() imu.orientation = self.orientation 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)
def pub_imu(msg_type, msg, bridge): pub = bridge.get_ros_pub("imu", Imu, queue_size=1) imu = Imu() imu.header.stamp = bridge.project_ap_time(msg) imu.header.frame_id = 'base_footprint' # Orientation as a Quaternion, with unknown covariance quat = quaternion_from_euler(msg.roll, msg.pitch, msg.yaw, 'sxyz') imu.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) imu.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) # Angular velocities, with unknown covariance imu.angular_velocity.x = msg.rollspeed imu.angular_velocity.y = msg.pitchspeed imu.angular_velocity.z = msg.yawspeed imu.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) # Not supplied with linear accelerations imu.linear_acceleration_covariance[0] = -1 pub.publish(imu)
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu' imu_msg.orientation = Quaternion(*SF9DOF_UKF.recover_quat(self.kalman_state)) imu_msg.orientation_covariance = \ list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.angular_velocity.x = self.kalman_state[3,0] imu_msg.angular_velocity.y = self.kalman_state[4,0] imu_msg.angular_velocity.z = self.kalman_state[5,0] imu_msg.angular_velocity_covariance = \ list(self.kalman_covariance[3:6,3:6].flatten()) imu_msg.linear_acceleration.x = self.kalman_state[6,0] imu_msg.linear_acceleration.y = self.kalman_state[7,0] imu_msg.linear_acceleration.z = self.kalman_state[8,0] imu_msg.linear_acceleration_covariance = \ list(self.kalman_covariance[6:9,6:9].flatten()) self.pub.publish(imu_msg)
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = 'base_link' #Orientation from Accelerometer roll = self.accel_data["roll"] pitch = self.accel_data["pitch"] yaw = 0 q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) imu_msg.orientation = Quaternion(*q) #Angular Velocity from Gyroscope imu_msg.angular_velocity.x = self.gyro_data["x"] imu_msg.angular_velocity.y = self.gyro_data["y"] imu_msg.angular_velocity.z = self.gyro_data["z"] #Linear Acceleration from Accelerometer imu_msg.linear_acceleration.x = self.accel_data["x"] imu_msg.linear_acceleration.y = self.accel_data["y"] imu_msg.linear_acceleration.z = self.accel_data["z"] self.imu_pub.publish(imu_msg)
def talker(): #Create publisher ('Topic name', msg type) pub = rospy.Publisher('IMU', Imu) #Tells rospy name of the node to allow communication to ROS master rospy.init_node('IMUtalker') while not rospy.is_shutdown(): #Grab relevant information from parse() try: (accel,gyro,magne) = parse() #If data is bad, uses presvious data except: continue #Define IMUmsg to be of the Imu msg type IMUmsg = Imu() #Set header time stamp IMUmsg.header.stamp = rospy.Time.now() #Set orientation parameters IMUmsg.orientation = Quaternion() IMUmsg.orientation.x = magne[0] IMUmsg.orientation.y = magne[1] IMUmsg.orientation.z = magne[2] IMUmsg.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Set angular velocity parameters IMUmsg.angular_velocity = Vector3() IMUmsg.angular_velocity.x = gyro[0] IMUmsg.angular_velocity.y = gyro[1] IMUmsg.angular_velocity.z = gyro[2] IMUmsg.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Set linear acceleration parameters IMUmsg.linear_acceleration = Vector3() IMUmsg.linear_acceleration.x = accel[0] IMUmsg.linear_acceleration.y = accel[1] IMUmsg.linear_acceleration.z = accel[2] IMUmsg.linear_acceleration_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Publish the data pub.publish(IMUmsg)
def update_sensors(self): # Accelerometer accel = self._driver.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)
def run(self): while not rospy.is_shutdown(): # Get and publish all the sensors data for sensor, id_number in SENSORS.items(): imu_msg = Imu() mag_msg = Vector3Stamped() # Order: GyroRate[0:2], AccelerometerVector[3:5], CompassVector[6:8] #~ raw_data = self.pvr_system.getAllCorrectedComponentSensorData(id_number) tared_quat = self.pvr_system.getTaredOrientationAsQuaternion(id_number) #~ if raw_data and tared_quat: if tared_quat: imu_msg.orientation = Quaternion(*tared_quat) #~ imu_msg.angular_velocity = Vector3(*raw_data[0:3]) #~ imu_msg.linear_acceleration = Vector3(*raw_data[3:6]) #~ mag_msg.vector = Vector3(*raw_data[6:9]) # Publish the data imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = 'world' mag_msg.header = imu_msg.header self.mag_pub[sensor].publish(mag_msg) self.imu_pub[sensor].publish(imu_msg)
def magnetic_cb(msg): global x_max, x_min, y_max, y_min, x_center, y_center, x_scale, y_scale if autocompute: x_max = max(x_max, msg.vector.x) x_min = min(x_min, msg.vector.x) y_max = max(y_max, msg.vector.y) y_min = min(y_min, msg.vector.y) x_center = (x_max + x_min) / 2.0 y_center = (y_max + y_min) / 2.0 x_scale = x_max - x_min y_scale = y_max - y_min x = (msg.vector.x - x_center) / x_scale y = (msg.vector.y - y_center) / y_scale heading = atan2(x, y) imu = Imu() imu.header = msg.header imu.header.frame_id = 'odom' q = tf.transformations.quaternion_from_euler(0, 0, heading) imu.orientation = Quaternion(*q) imu.orientation_covariance = [ compass_var, 0.0, 0.0, 0.0, compass_var, 0.0, 0.0, 0.0, compass_var ] imu.angular_velocity_covariance = [ -1, 0, 0, 0, 0, 0, 0, 0, 0 ] imu.linear_acceleration_covariance = [ -1, 0, 0, 0, 0, 0, 0, 0, 0 ] imu_pub.publish(imu)
def scan_matcher_callback(self, data): self.current_time = rospy.Time.now() dt = (self.current_time - self.last_time).to_sec() # Calculate Relative Yaw if self.init_imu_angle: self.last_imu_angle = data.theta self.last_time = self.current_time self.init_imu_angle = False return current_imu_angle = data.theta dyaw = 0.0 if (-3.1416 <= self.last_imu_angle < 0) and (0 < current_imu_angle <= 3.1416): dyaw = -((-3.1416 - self.last_imu_angle) + (3.1416 - current_imu_angle)) elif (0 <= self.last_imu_angle < 3.1416) and (-3.1416 < current_imu_angle < 0): dyaw = ((3.1416- self.last_imu_angle) + (-3.1416 - current_imu_angle)) else: dyaw = current_imu_angle - self.last_imu_angle # inverse direction to match imu_link # dyaw = -dyaw imu_msg = Imu() imu_msg.header.frame_id = "imu_link" imu_msg.header.stamp = rospy.Time.now() q_tmp = tf.transformations.quaternion_from_euler(0, 0, data.theta) imu_msg.orientation = Quaternion(q_tmp[0], q_tmp[1], q_tmp[2], q_tmp[3]) imu_msg.angular_velocity.x = 0 imu_msg.angular_velocity.y = 0 if dt == 0.0: imu_msg.angular_velocity.z = 0.0 else: imu_msg.angular_velocity.z = dyaw/dt imu_noises = pow(0.00017, 2) # = 0.01 degrees / sec imu_msg.orientation_covariance = [1e3, 0, 0, 0, 1e3, 0, 0, 0, imu_noises] self.last_imu_angle = current_imu_angle self.last_time = self.current_time self.imu_pub.publish(imu_msg)
def corrimudata_handler(self, corrimudata): # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body imu = Imu() imu.header.stamp == rospy.Time.now() imu.header.frame_id = self.base_frame # Populate orientation field with one from inspvax message. imu.orientation = Quaternion(*self.orientation) imu.orientation_covariance = self.orientation_covariance # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = corrimudata.pitch_rate * self.imu_rate imu.angular_velocity.y = corrimudata.roll_rate * self.imu_rate imu.angular_velocity.z = corrimudata.yaw_rate * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = corrimudata.x_accel * self.imu_rate imu.linear_acceleration.y = corrimudata.y_accel * self.imu_rate imu.linear_acceleration.z = corrimudata.z_accel * self.imu_rate imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu)
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)
import imu_ukf if __name__ == "__main__": rospy.init_node("imu_sim") omega = zeros((3,1)) state_pub = rospy.Publisher('state_sim', State) ground_truth_pub = rospy.Publisher('imu_ground_truth', Imu) gravity_vector = array([0.04,0.,9.8]) magnetic_vector = array([0.03,0.12,0.42]) yaw = 0.0 r = rospy.Rate(20) while not rospy.is_shutdown(): yaw += 0.01 i = Imu() i.header.stamp = rospy.Time.now() i.orientation =\ Quaternion(*tf_math.quaternion_from_euler(0,0,yaw,'sxyz')) i.angular_velocity.z = 0.01 s = State() s.header.stamp = rospy.Time.now() s.magnetometer.x = magnetic_vector[0] s.magnetometer.y = magnetic_vector[1] s.magnetometer.z = magnetic_vector[2] s.linear_acceleration.x = gravity_vector[0] s.linear_acceleration.y = gravity_vector[1] s.linear_acceleration.z = gravity_vector[2] s.angular_velocity.z = 0.01 state_pub.publish(s) ground_truth_pub.publish(i) r.sleep()
#!/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("/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 navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion() # UTM conversion (zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude) # Initialize starting point if we haven't yet # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid? if not self.init and self.zero_start: self.origin.x = easting self.origin.y = northing self.init = True # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y self.pub_origin.publish(p) # # Odometry # TODO: Work out these covariances properly from DOP # odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = easting - self.origin.x odom.pose.pose.position.y = northing - self.origin.y odom.pose.pose.position.z = data.altitude odom.pose.pose.orientation = Quaternion(*orient) odom.pose.covariance = POSE_COVAR # Twist is relative to /vehicle frame odom.twist.twist.linear.x = data.speed odom.twist.twist.linear.y = 0 odom.twist.twist.linear.z = -data.down_vel odom.twist.twist.angular.x = RAD(data.ang_rate_long) odom.twist.twist.angular.y = RAD(-data.ang_rate_trans) odom.twist.twist.angular.z = RAD(-data.ang_rate_down) odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform( (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), Quaternion(*orient), odom.header.stamp,odom.child_frame_id, odom.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status navsat.latitude = data.latitude navsat.longitude = data.longitude navsat.altitude = data.altitude navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.pub_navsatfix.publish(navsat) # # IMU # TODO: Work out these covariances properly # imu = Imu() imu.header.stamp == rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation imu.orientation = Quaternion(*orient) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates imu.angular_velocity.x = RAD(data.ang_rate_long) imu.angular_velocity.y = RAD(-data.ang_rate_trans) imu.angular_velocity.y = RAD(-data.ang_rate_down) imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration imu.linear_acceleration.x = data.long_accel imu.linear_acceleration.y = data.trans_accel imu.linear_acceleration.z = data.down_accel imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
# angular velocity imu_data.angular_velocity.x = Gx imu_data.angular_velocity.y = Gy imu_data.angular_velocity.z = Gz # acceleration imu_data.linear_acceleration.x = Ax imu_data.linear_acceleration.y = Ay imu_data.linear_acceleration.z = Az # temperature temp.data = T; # Quaternion message dataaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Imu_data.orientation = Quaternion() # orientation #ai = imu_data.orientation.x / 2.0 #aj = imu_data.orientation.y / 2.0 #ak = imu_data.orientation.z / 2.0 #ci = math.cos(ai) #si = math.sin(ai) #cj = math.cos(aj) #sj = math.sin(aj) #ck = math.cos(ak) #sk = math.sin(ak) #quaternion = numpy.empty((4, ), dtype=numpy.float64) Imu_data.orientation.x = x #si*cj*ck - ci*sj*sk Imu_data.orientation.y = y #ci*sj*ck + si*cj*sk Imu_data.orientation.z = z #ci*cj*sk - si*sj*ck