def handle(self): self.device.write(ord('\n')) temp = Temperature() temp.header.stamp = self.current_time temp.header.frame_id = 'temp_frame' buf = StringIO() while True: try: c = chr(self.device.read()) if c == '\n': break buf.write(c) except Exception: continue try: rospy.loginfo(buf.getvalue()) v = json.loads(buf.getvalue()) temp.temperature = v['temp'] except ValueError as e: rospy.loginfo(e) return except Exception as e: rospy.logerror(e) return self.pub.publish(temp)
def talker(self): # Class Instances pressure = FluidPressure() pressure.variance = 0 temp = Temperature() temp.variance = 0 depth = Float64() while not rospy.is_shutdown(): if self.sensor.read(): # Pressure reading (Pascal) pressure.header.stamp = rospy.get_rostime() pressure.fluid_pressure = self.sensor.pressure(ms5837.UNITS_Pa) # Temperature reading (Celsius) temp.header.stamp = rospy.get_rostime() temp.temperature = self.sensor.temperature( ms5837.UNITS_Centigrade) # Depth reading (Meters) depth.data = self.sensor.depth(ATM_PRESSURE_PA, LOCAL_GRAVITY) # Publishing self.pub_pressure.publish(pressure) self.pub_temp.publish(temp) self.pub_depth.publish(depth) else: rospy.roswarn('Failed to read.') rospy.Rate(10).sleep() # 10 hz
def deserialize(self, p_content): """ see Serializer.""" if len(p_content) < 12: return None msg1 = Temperature() msg2 = RelativeHumidity() msg3 = FluidPressure() org = struct.unpack('fff', p_content) msg1.header.stamp = rospy.Time.now() # round float value to double. msg1.temperature = Math.roundFloatToDouble(org[0]) msg1.variance = 0 msg2.header.stamp = rospy.Time.now() msg2.relative_humidity = Math.roundFloatToDouble(org[1]) msg2.variance = 0 msg3.header.stamp = rospy.Time.now() msg3.fluid_pressure = Math.roundFloatToDouble(org[2]) msg3.variance = 0 msgs = [msg1, msg2, msg3] if CommonConfig.DEBUG_FLAG is True: rospy.logdebug('temperature=' + str(msg1.temperature)) rospy.logdebug('humidity=' + str(msg2.relative_humidity)) rospy.logdebug('pressure=' + str(msg3.fluid_pressure)) return msgs
def _log_data_log2(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Temperature() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in degC msg.temperature = data["baro.temp"] self._pubTemp.publish(msg) # ToDo: it would be better to convert from timestamp to rospy time msg = MagneticField() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in Tesla msg.magnetic_field.x = data["mag.x"] msg.magnetic_field.y = data["mag.y"] msg.magnetic_field.z = data["mag.z"] self._pubMag.publish(msg) msg = Float32() # hPa (=mbar) msg.data = data["baro.pressure"] self._pubPressure.publish(msg) # V msg.data = data["pm.vbat"] self._pubBattery.publish(msg)
def publish_variables(self, event): t = rospy.get_time() for var in self._variables: pc_msg = PointCloud() pc_msg.header.stamp = rospy.Time.now() pc_msg.header.frame_id = 'world' for name in self._vehicle_pos: value = self._interpolate(var, self._vehicle_pos[name][0], self._vehicle_pos[name][1], self._vehicle_pos[name][2], t) # Updating the point cloud for this variable pc_msg.points.append( Point32(self._vehicle_pos[name][0], self._vehicle_pos[name][1], self._vehicle_pos[name][2])) pc_msg.channels.append(ChannelFloat32()) pc_msg.channels[-1].name = 'intensity' pc_msg.channels[-1].values.append(value) # Create the message objects if 'temperature' in var.lower(): msg = Temperature() msg.header.stamp = rospy.Time.now() msg.header.frame_id = '%s/base_link' % name # TODO Read from the unit of temperature from NC file # Converting to Celsius msg.temperature = value - 273.15 else: msg = FloatStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = '%s/base_link' % name msg.data = value self._topics[name][var].publish(msg) self._pc_variables[var].publish(pc_msg) return True
def _publish_sba5_measure(self, data): self._logger.debug("publish_sba5_measure: We got data: {}".format(data)) co2_msg = Temperature() co2_msg.header.stamp = rospy.Time.now() co2_msg.temperature = data self._logger.debug("Message: {}".format(co2_msg)) self._co2_pub.publish(co2_msg)
def timerCallback(self, event): if self.sensor.read(): pressure = self.sensor.pressure() temp = self.sensor.temperature() self.depth_raw = self.sensor.depth() depth = self.depth_raw - self.depth_offset self.depth = depth pressure_msg = FluidPressure() pressure_msg.header.frame_id = 'pressure_sensor' pressure_msg.header.seq = self.count pressure_msg.header.stamp = rospy.Time.now() pressure_msg.fluid_pressure = pressure self.fluid_pub.publish(pressure_msg) depth_stamped_msg = Vector3Stamped() depth_stamped_msg.header.frame_id = 'pressure_sensor' depth_stamped_msg.header.seq = self.count depth_stamped_msg.header.stamp = rospy.Time.now() #TODO fix this depth_stamped_msg.vector.z = depth # TODO this self.depth_stamped_pub.publish(depth_stamped_msg) depth_msg = Float64() depth_msg.data = depth self.depth_pub.publish(depth_msg) temp_msg = Temperature() temp_msg.header.frame_id = 'pressure_sensor' temp_msg.header.seq = self.count temp_msg.header.stamp = rospy.Time.now() temp_msg.temperature = temp self.temp_pub.publish(temp_msg) self.count += 1
def publish(self): pub = rospy.Publisher('actuators_update', Temperature, queue_size=1) rate = rospy.Rate(50) #50Hz msg_temp = Temperature(); msg_temp.header.frame_id = "robocape" while not rospy.is_shutdown(): now = rospy.get_rostime() msg_temp.header.stamp.secs = now.secs msg_temp.header.stamp.nsecs = now.nsecs msg_temp.temperature = self.ThrottleREMOTE #controller self.SteeringCmd = self.Kp*(self.Roll - self.RollCmd) + self.Kv*(self.RollRate - self.RollRateCmd) SteeringCmdPulse = ((self.SteeringCmd+90)*5.55555555555)+1000 if SteeringCmdPulse > 1900: msg_temp.variance = 1900 elif SteeringCmdPulse < 1100: msg_temp.variance = 1100 else: msg_temp.variance = SteeringCmdPulse pub.publish(msg_temp) rospy.loginfo("Publishing :") rospy.loginfo(msg_temp.variance) rate.sleep()
def imu_publish(): imu_publisher = rospy.Publisher('/mpu6050_imu', Imu, queue_size=10) temp_publisher = rospy.Publisher('/mpu6050_temp', Temperature, queue_size=10) rospy.init_node('imu_sensor') r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): imu_msg = Imu() acc_array = mpu6050.get_accel_data_g() imu_msg.linear_acceleration.x = acc_array[0] imu_msg.linear_acceleration.y = acc_array[1] imu_msg.linear_acceleration.z = acc_array[2] gyro_array = mpu6050.get_gyro_data_deg() imu_msg.angular_velocity.x = gyro_array[0] imu_msg.angular_velocity.y = gyro_array[1] imu_msg.angular_velocity.z = gyro_array[2] imu_publisher.publish(imu_msg) temp_msg = Temperature() temp_val = mpu6050.get_temp() temp_msg.temperature = temp_val temp_publisher.publish(temp_msg) r.sleep()
def main(): rospy.init_node("imu") port = rospy.get_param("~port", "GPG3_AD1") sensor = InertialMeasurementUnit(bus=port) pub_imu = rospy.Publisher("~imu", Imu, queue_size=10) pub_temp = rospy.Publisher("~temp", Temperature, queue_size=10) pub_magn = rospy.Publisher("~magnetometer", MagneticField, queue_size=10) br = TransformBroadcaster() msg_imu = Imu() msg_temp = Temperature() msg_magn = MagneticField() hdr = Header(stamp=rospy.Time.now(), frame_id="IMU") rate = rospy.Rate(rospy.get_param('~hz', 30)) while not rospy.is_shutdown(): q = sensor.read_quaternion() # x,y,z,w mag = sensor.read_magnetometer() # micro Tesla (µT) gyro = sensor.read_gyroscope() # deg/second accel = sensor.read_accelerometer() # m/s² temp = sensor.read_temperature() # °C msg_imu.header = hdr hdr.stamp = rospy.Time.now() msg_temp.header = hdr msg_temp.temperature = temp pub_temp.publish(msg_temp) msg_imu.header = hdr msg_imu.linear_acceleration.x = accel[0] msg_imu.linear_acceleration.y = accel[1] msg_imu.linear_acceleration.z = accel[2] msg_imu.angular_velocity.x = math.radians(gyro[0]) msg_imu.angular_velocity.y = math.radians(gyro[1]) msg_imu.angular_velocity.z = math.radians(gyro[2]) msg_imu.orientation.w = q[3] msg_imu.orientation.x = q[0] msg_imu.orientation.y = q[1] msg_imu.orientation.z = q[2] pub_imu.publish(msg_imu) msg_magn.header = hdr msg_magn.magnetic_field.x = mag[0] * 1e-6 msg_magn.magnetic_field.y = mag[1] * 1e-6 msg_magn.magnetic_field.z = mag[2] * 1e-6 pub_magn.publish(msg_magn) transform = TransformStamped(header=Header(stamp=rospy.Time.now(), frame_id="world"), child_frame_id="IMU") transform.transform.rotation = msg_imu.orientation br.sendTransformMessage(transform) rate.sleep()
def _loop(self): # rospy.sleep(1) while not rospy.is_shutdown(): res = self.get_mean_data(self._measure_interval) msg = Temperature() msg.temperature = res msg.header.stamp.secs = rospy.get_time() self._data_pub.publish(msg)
def read_cpu_temp(): with open("/sys/class/thermal/thermal_zone0/temp", 'r') as tempFile: cpu_temp = tempFile.read() msg = Temperature() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id_param msg.temperature = float(cpu_temp) / 1000 msg.variance = 0.0 return msg
def default(self, ci): """Publishes an Temperature message containing the current temperature and the variance (if a Gaussian noise modifier is applied. """ msg = Temperature() msg.header = self.get_ros_header() msg.temperature = self.data["temperature"] msg.variance = self.get_variance(ci, "temperature") self.publish(msg)
def pubTemperature(self): msg = Temperature() msg.header.stamp = rospy.Time.now() if (self.use_temperature): msg.temperature = altimeterObj.get_temperature() + 273.15 else: msg.temperature = 15 + 273, 15 msg.variance = 0 # rospy.loginfo(msg) self.temperaturePub.publish(msg) return
def handle(self): temp = Temperature() temp.header.stamp = self.current_time temp.header.frame_id = 'temp_frame' output = subprocess.check_output(['vcgencmd','measure_temp']).decode('utf-8') v = float(output[output.find('=') + 1:].strip().rstrip('\'C')) temp.temperature = v rospy.loginfo(v) self.pub.publish(temp)
def callback_gps(gps): curr_location_x = gps.pose.position.x curr_location_y = gps.pose.position.y curr_location_z = gps.pose.position.z wifi_strength = 1 / (3 * (1 + sigma_x * abs(curr_location_x - x_my))) + 1 / ( 3 * (1 + sigma_y * abs(curr_location_y - y_my)) ) + 1 / (3 * (1 + sigma_z * abs(curr_location_z - z_my))) wifi = Temperature() wifi.header.frame_id = "map" wifi.temperature = wifi_strength wifi_pub.publish(wifi)
def start(self): self.enable = True self.fluidPressurePub = rospy.Publisher(self.robot_host + '/meteorological/pressure', FluidPressure, queue_size=10) self.relativeHumidityPub = rospy.Publisher(self.robot_host + '/meteorological/humidity', RelativeHumidity, queue_size=10) self.temperaturePub = rospy.Publisher(self.robot_host + '/meteorological/temperature', Temperature, queue_size=10) sense = SenseHat() while not rospy.is_shutdown(): pressure = sense.get_pressure() humidity = sense.get_humidity() temp = sense.get_temperature() message_str = "Pressure: %s Millibars which are %s Pascal; Humidity: %s %%rH; Temperature: %s °C " % ( pressure, (pressure * 100), humidity, temp) rospy.loginfo(message_str) f = FluidPressure() f.header.stamp = rospy.Time.now() f.header.frame_id = "/base_link" f.fluid_pressure = pressure * 100 # Millibar to Pascal conversion f.variance = 0 h = RelativeHumidity() h.header.stamp = rospy.Time.now() h.header.frame_id = "/base_link" h.relative_humidity = humidity h.variance = 0 t = Temperature() t.header.stamp = rospy.Time.now() t.header.frame_id = "/base_link" t.temperature = temp t.variance = 0 self.fluidPressurePub.publish(f) self.relativeHumidityPub.publish(h) self.temperaturePub.publish(t) self.rate.sleep()
def talker(): pub = rospy.Publisher('temp', Temperature, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(2) # 2hz msg = Temperature() seq = 0 while not rospy.is_shutdown(): seq += 1 msg.header.seq = seq msg.temperature = random.random() * 30.0 msg.variance = 30.0 pub.publish(msg) rate.sleep()
def callback_setpoint(search_point): search_x = search_point.pose.position.x search_y = search_point.pose.position.y search_z = search_point.pose.position.z wifi_strength = A * math.exp(-((((search_x - x_my) * (search_x - x_my)) / (2 * sigma_x * sigma_x)) + (((search_y - y_my) * (search_y - y_my)) / (2 * sigma_y * sigma_y)) + (((search_z - z_my) * (search_z - z_my)) / (2 * sigma_z * sigma_z)))) wifi = Temperature() wifi.temperature = wifi_strength search_wifi_pub.publish(wifi)
def readSensor(sensor, datapin): humid, temp = Adafruit_DHT.read(sensor, datapin) tempMsg = Temperature() humidMsg = RelativeHumidity() time = rospy.Time.now() if temp is not None: tempMsg.header.stamp = time tempMsg.temperature = temp tempMsg.variance = 0 if humid is not None: humidMsg.header.stamp = time humidMsg.relative_humidity = humid humidMsg.variance = 0 return (tempMsg, humidMsg)
def publish_imu_temperature(self): imu_temperature = Temperature() temperature = self.bno055.get_temperature() imu_temperature.header.stamp = rospy.Time.now() imu_temperature.header.frame_id = self.frame_id imu_temperature.header.seq = self.imu_temperature_seq_counter imu_temperature.temperature = temperature self.imu_temperature_seq_counter = +1 self.pub_imu_temperature.publish(imu_temperature)
def publish(): sensor = modbus(method='rtu', port='/dev/ucfsub/depth', baudrate=115200) sensor.connect() rospy.init_node('Depth') tempPub = rospy.Publisher('ExternalTemperature', Temperature, queue_size=1) depthPub = rospy.Publisher('/depth', Float32, queue_size=1) posePub = rospy.Publisher('/depth/pose', PoseWithCovarianceStamped, queue_size=1) diagPub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) temp = Temperature() depth = Float32() diag = DiagnosticArray() updateRate = rospy.get_param("/updateRate", 30) freq = rospy.Rate(updateRate) loop = 0 pose = PoseWithCovarianceStamped() pose.header.frame_id = "odom" pose.pose.covariance = [0.0]*36 pose.pose.covariance[14] = 0.01 pose.pose.pose.orientation.x = 0.0 pose.pose.pose.orientation.y = 0.0 pose.pose.pose.orientation.z = 0.0 pose.pose.pose.orientation.w = 1.0 pose.pose.pose.position.x = 0.0 pose.pose.pose.position.y = 0.0 while not rospy.is_shutdown(): if loop >= updateRate: rr = sensor.read_holding_registers(address=8, count=2, unit=1) if type(rr) is not type(ModbusIOException): temp.temperature = struct.unpack('>f',struct.pack('>HH', *rr.registers))[0] tempPub.publish(temp) loop = 0 loop += 1 rr = sensor.read_holding_registers(address=2, count=2, unit=1) if type(rr) is not type(ModbusIOException): depth.data = 10.2*struct.unpack('>f',struct.pack('>HH', *rr.registers))[0] pose.pose.pose.position.z = depth.data pose.header.stamp = rospy.Time.now() posePub.publish(pose) depthPub.publish(depth) diag.status = [DiagnosticStatus(name='Depth', message=str(type(rr) is type(ModbusIOException)), level=int(rr == 'true')*2)] freq.sleep()
def __init__(self, i2c=None): super().__init__('rtf_lps22') if i2c is None: self.i2c = busio.I2C(board.SCL, board.SDA) else: self.i2c = i2c self.lps = adafruit_lps2x.LPS22(self.i2c) self.timer_temp = self.create_timer(1.0, self.callback) self.pub_temp = self.create_publisher(Temperature, 'temp', 10) self.pub_pressure = self.create_publisher(FluidPressure, 'pressure', 10) frame_id = self.declare_parameter('frame_id', "base_imu_link").value self.temp_msg = Temperature() self.temp_msg.header.frame_id = frame_id self.temp_msg.variance = 0.01 self.pressure_msg = FluidPressure() self.pressure_msg.header.frame_id = frame_id self.pressure_msg.variance = 0.01
def talker(): humidity_pub = rospy.Publisher('cassiopeia/environmental/humidity', RelativeHumidity, queue_size=10) temperature_pub = rospy.Publisher('cassiopeia/environmental/temperature', Temperature, queue_size=10) pressure_pub = rospy.Publisher('cassiopeia/environmental/pressure', FluidPressure, queue_size=10) altitude_pub = rospy.Publisher('cassiopeia/altitude/', Altitude, queue_size=10) rospy.init_node('environmental_sensor', anonymous=False) rate = rospy.Rate( 1) # Max rate of BME280 sensor in normal mode in theory is 13.51 Hz while not rospy.is_shutdown(): h = Header() h.stamp = rospy.Time.now() humidity_pub.publish( RelativeHumidity( relative_humidity=environmental_sensor.get_humidity(), header=h)) pressure_pub.publish( FluidPressure(fluid_pressure=environmental_sensor.get_pressure(), header=h)) temperature_pub.publish( Temperature(temperature=environmental_sensor.get_temperature(), header=h)) altitude_pub.publish( Altitude(altitude=environmental_sensor.get_altitude(), header=h)) rate.sleep()
def main(): sensor = TempHumPress() rospy.init_node("temperature_sensor") pub_temperature = rospy.Publisher("~temperature", Temperature, queue_size=10) pub_humidity = rospy.Publisher("~humidity", RelativeHumidity, queue_size=10) pub_pressure = rospy.Publisher("~pressure", FluidPressure, queue_size=10) rate = rospy.Rate(rospy.get_param('~hz', 2)) while not rospy.is_shutdown(): hdr = Header(frame_id="temperature", stamp=rospy.Time.now()) msg_temp = Temperature(header=hdr, temperature=sensor.get_temperature_celsius()) pub_temperature.publish(msg_temp) msg_hum = RelativeHumidity(header=hdr, relative_humidity=sensor.get_humidity() / 100.0) pub_humidity.publish(msg_hum) msg_press = FluidPressure(header=hdr, fluid_pressure=sensor.get_pressure()) pub_pressure.publish(msg_press) rate.sleep()
def callback(data): global average, variance, pub rospy.loginfo('Temperature Received: %f', data.temperature) average = (average + data.temperature)/2 variance = (variance + data.variance)/2 t = Temperature() h = std_msgs.msg.Header() h.stamp = rospy.Time.now() t.header = h t.temperature = average t.variance = variance pub.publish(t)
def __init__(self, vehicle_name, **kwargs): self.vehicle_name = vehicle_name # Initialize node rospy.init_node("internal_temp_monitor_%s" % vehicle_name, anonymous=True) # Create system status publisher self.status_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10) # Get parameters self.temp_level_warn = rospy.get_param('~temp_level_warn') self.temp_level_error = rospy.get_param('~temp_level_error') self.topic = rospy.get_param('~temp_level_topic', default='/temperature') # Create subscriber for temperature rospy.Subscriber(self.topic, Temperature, callback=self.temp_callback) # Temperature and diagnostics message self.temperature = Temperature() # Rate self.rate = rospy.Rate(1) # Time for last received message self.last_update = 0
def reset_vars(self): self.imu_msg = Imu() self.imu_msg.orientation_covariance = (-1., ) * 9 self.imu_msg.angular_velocity_covariance = (-1., ) * 9 self.imu_msg.linear_acceleration_covariance = (-1., ) * 9 self.pub_imu = False self.pub_syncout_timeref = False self.raw_gps_msg = NavSatFix() self.pub_raw_gps = False self.pos_gps_msg = NavSatFix() self.pub_pos_gps = False self.vel_msg = TwistStamped() self.pub_vel = False self.mag_msg = MagneticField() self.mag_msg.magnetic_field_covariance = (0, ) * 9 self.pub_mag = False self.temp_msg = Temperature() self.temp_msg.variance = 0. self.pub_temp = False self.press_msg = FluidPressure() self.press_msg.variance = 0. self.pub_press = False self.anin1_msg = UInt16() self.pub_anin1 = False self.anin2_msg = UInt16() self.pub_anin2 = False self.ecef_msg = PointStamped() self.pub_ecef = False self.pub_diag = False
def __init__(self): rospy.init_node('jmoab_ros_sht31_node', anonymous=True) rospy.loginfo("Start JMOAB-ROS-Sensor Temeprature & Humidity node") self.bus = SMBus(1) self.temp_pub = rospy.Publisher("/jmoab_temp", Temperature, queue_size=10) self.temp_msg = Temperature() self.humid_pub = rospy.Publisher("/jmoab_humid", RelativeHumidity, queue_size=10) self.humid_msg = RelativeHumidity() rospy.loginfo( "Publishing Temperature data [degree celsius] on /jmoab_temp topic" ) rospy.loginfo( "Publishing Relative Humidity [percentage 0.0->1.0] on /jmoab_humid topic" ) ## BNO055 address and registers self.SENSOR_ADDR = 0x44 self.DATA_REG = 0x00 self.loop() rospy.spin()
def __init__(self, _threaded_mode = False): self.data_dir = "/tmp/BHG_DATA" self.csv_filename = "default_data.csv" self.datetimeData = "" self.is_recording = False self.rel_alt = Altitude() self.gps_fix = NavSatFix() self.odom = Odometry() self.imu_mag = MagneticField() self.imu_data = Imu() self.vel_gps = TwistStamped() self.temp_imu = Temperature() self.trigtime = "TrigTimeNotSet" self.arduino_dev = rospy.get_param('~arduino_dev', '/dev/ttyACM0') self.arduino_timeout = rospy.get_param('~arduino_timeout', 2) self.ardunio_ser = '' self.pub = rospy.Publisher('trig_timer', String, queue_size=10) rospy.Subscriber('/directory', String, self.directory_callback) rospy.Subscriber("/record", Bool, self.record_callback) rospy.Subscriber("/mavros/altitude", Altitude, self.alt_cb) # change to /global_position/rel_alt Float64 rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_cb) rospy.Subscriber("/mavros/global_position/local", Odometry, self.odom_cb) rospy.Subscriber("/mavros/imu/mag", MagneticField, self.mag_cb) rospy.Subscriber("/mavros/imu/data", Imu, self.imu_cb) rospy.Subscriber("/mavros/global_position/raw/gps_vel", TwistStamped, self.vel_cb) rospy.Subscriber("/mavros/imu/temperature_imu", Temperature, self.temp_cb)
def publish(): tempPub = rospy.Publisher('InternalTemperature', Temperature, queue_size=1) humidPub = rospy.Publisher('InternalHumidity', RelativeHumidity, queue_size=1) rospy.init_node('InternalEnvironment') sensor = HIH6130.HIH6130(bus=1) temp = Temperature() temp.header.frame_id = "base_link" humid = RelativeHumidity() humid.header.frame_id = "base_link" freq = rospy.Rate(5) while not rospy.is_shutdown(): sensor.read() temp.temperature = sensor.t humid.relative_humidity = sensor.rh tempPub.publish(temp) humidPub.publish(humid) freq.sleep()
def callback_gps(gps): curr_location_x = gps.pose.position.x curr_location_y = gps.pose.position.y curr_location_z = gps.pose.position.z wifi_strength = A * math.exp(-((((curr_location_x - x_my) * (curr_location_x - x_my)) / (2 * sigma_x * sigma_x)) + (((curr_location_y - y_my) * (curr_location_y - y_my)) / (2 * sigma_y * sigma_y)) + (((curr_location_z - z_my) * (curr_location_z - z_my)) / (2 * sigma_z * sigma_z)))) wifi = Temperature() wifi.header.frame_id = "map" wifi.temperature = wifi_strength wifi_pub.publish(wifi)
def _log_data_log2(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Temperature() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in degC msg.temperature = data["baro.temp"] self._pubTemp.publish(msg) # ToDo: it would be better to convert from timestamp to rospy time msg = MagneticField() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in Tesla msg.magnetic_field.x = data["mag.x"] msg.magnetic_field.y = data["mag.y"] msg.magnetic_field.z = data["mag.z"] self._pubMag.publish(msg) msg = Float32() # hPa (=mbar) msg.data = data["baro.pressure"] self._pubPressure.publish(msg) # V msg.data = data["pm.vbat"] self._pubBattery.publish(msg) # Test msg.data = data["test.tval"] self._pubTest.publish(msg)
def publish(self): pub = rospy.Publisher('controller_readings', Temperature, queue_size=1) rate = rospy.Rate(40) msg_temp = Temperature() msg_temp.header.frame_id = "robocape" while not rospy.is_shutdown(): now = rospy.get_rostime() #read remote commands while self.dev.inWaiting() <= 11: pass UART_READ = self.dev.readline() self.dev.flushInput() self.SteeringREMOTE = int(UART_READ[0:4]) self.ThrottleREMOTE = int(UART_READ[4:8]) rospy.loginfo([self.ThrottleREMOTE, self.SteeringREMOTE]) #controller self.pid.update(self.Roll) self.SteeringCONTROLdeg = self.pid.output self.SteeringCONTROLpulse = ((self.SteeringCONTROLdeg + 66)*7.35294118)+1000 #steering saturation if self.SteeringCONTROLpulse > 1800: msg_temp.variance = 1800 elif self.SteeringCONTROLpulse < 1200: msg_temp.variance = 1200 else: msg_temp.variance = self.SteeringCONTROLpulse #publish messages to the actuators msg_temp.header.stamp.secs = now.secs msg_temp.header.stamp.nsecs = now.nsecs msg_temp.temperature = self.ThrottleREMOTE pub.publish(msg_temp) rospy.loginfo("Publishing :") rospy.loginfo(msg_temp.variance) #update actuators self.dev1.pulsewidth_us(int(msg_temp.temperature)) self.dev2.pulsewidth_us(int(msg_temp.variance)) rate.sleep()
def publish_temp(): pub = rospy.Publisher('henri/temperature', Temperature, queue_size=10) rospy.init_node('temperature_sensor_py', anonymous=True) rate = rospy.Rate(1) while not rospy.is_shutdown(): t = Temperature() h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = 'henri_link' t.header = h t.temperature = random.uniform(0.0, 30.0) t.variance = 0 pub.publish(t) rospy.loginfo('Published Temp: %f', t.temperature) rate.sleep()
def publish(self): pub = rospy.Publisher('remote_readings', Temperature, queue_size=1) rate = rospy.Rate(40) #50Hz msg_temp = Temperature(); msg_temp.header.frame_id = "robocape" while not rospy.is_shutdown(): now = rospy.get_rostime() while self.dev.inWaiting() <= 11: pass UART_read = self.dev.readline() self.dev.flushInput() self.SteeringREMOTE = int(UART_read[0:4]) self.ThrottleREMOTE = int(UART_read[0:4]) RollCmd_temp = self.RollCmd self.RollCmd = ((self.SteeringREMOTE-1000)/5.555555555555)-90 self.RollRateCmd = self.RollCmd - RollCmd_temp rospy.loginfo([self.ThrottleREMOTE, self.SteeringREMOTE]) msg_temp.header.stamp.secs = now.secs msg_temp.header.stamp.nsecs = now.nsecs msg_temp.temperature = self.ThrottleREMOTE #controller self.SteeringCmd = self.Kp*(self.Roll - self.RollCmd) + self.Kv*(self.RollRate - self.RollRateCmd) SteeringCmdPulse = ((self.SteeringCmd+90)*5.55555555555)+1000 if SteeringCmdPulse > 1900: msg_temp.variance = 1900 elif SteeringCmdPulse < 1100: msg_temp.variance = 1100 else: msg_temp.variance = SteeringCmdPulse pub.publish(msg_temp) rospy.loginfo("Publishing :") rospy.loginfo(msg_temp.variance) rate.sleep()
def publish(): sensor = modbus(method='rtu', port='/dev/ucfsub/depth', baudrate=115200) sensor.connect() tempPub = rospy.Publisher('ExternalTemperature', Temperature, queue_size=1) depthPub = rospy.Publisher('depth', Float32, queue_size=1) posePub = rospy.Publisher('/depth/pose', PoseWithCovarianceStamped, queue_size=1) rospy.init_node('Depth') temp = Temperature() depth = Float32() freq = rospy.Rate(20) loop = 0 pose = PoseWithCovarianceStamped() pose.header.frame_id = "odom" pose.pose.covariance = [0.0]*36 pose.pose.covariance[14] = 0.01 pose.pose.pose.orientation.x = 0.0 pose.pose.pose.orientation.y = 0.0 pose.pose.pose.orientation.z = 0.0 pose.pose.pose.orientation.w = 1.0 pose.pose.pose.position.x = 0.0 pose.pose.pose.position.y = 0.0 while not rospy.is_shutdown(): if loop >= 20: rr = sensor.read_holding_registers(address=8, count=2, unit=1) temp.temperature = struct.unpack('>f',struct.pack('>HH', *rr.registers))[0] tempPub.publish(temp) loop = 0 loop += 1 rr = sensor.read_holding_registers(address=2, count=2, unit=1) depth.data = struct.unpack('>f',struct.pack('>HH', *rr.registers))[0] depthPub.publish(depth) pose.pose.pose.position.z = depth.data * 10.2 pose.header.stamp = rospy.Time.now() posePub.publish(pose) freq.sleep()
def sub_imuCB(msg_in): global pub_imu global pub_mag global pub_temp global pub_baro global msg_imu msg_imu.header.stamp = msg_in.header.stamp msg_imu.header.frame_id = msg_in.header.frame_id msg_imu.angular_velocity.x = msg_in.Accel.x msg_imu.angular_velocity.y = msg_in.Accel.y msg_imu.angular_velocity.z = msg_in.Accel.z msg_imu.linear_acceleration.x = msg_in.Gyro.x msg_imu.linear_acceleration.y = msg_in.Gyro.y msg_imu.linear_acceleration.z = msg_in.Gyro.z pub_imu.publish(msg_imu) msg_mag = MagneticField() msg_mag.header.stamp = msg_in.header.stamp msg_mag.header.frame_id = msg_in.header.frame_id msg_mag.magnetic_field.x = msg_in.Mag.x msg_mag.magnetic_field.y = msg_in.Mag.y msg_mag.magnetic_field.z = msg_in.Mag.z pub_mag.publish(msg_mag) msg_temp = Temperature() msg_temp.header.stamp = msg_in.header.stamp msg_temp.header.frame_id = msg_in.header.frame_id msg_temp.temperature = msg_in.Temp pub_temp.publish(msg_temp) msg_baro = FluidPressure() msg_baro.header.stamp = msg_in.header.stamp msg_baro.header.frame_id = msg_in.header.frame_id msg_baro.fluid_pressure = msg_in.Pressure / 1000.0 pub_baro.publish(msg_baro)
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 # Get parameters values
def imuTalker(): global ser pubImu = rospy.Publisher('sensors/imu', Imu, queue_size=10) pubTemp = rospy.Publisher('sensors/temperature', Temperature, queue_size=10) rospy.init_node('imu') rate = rospy.Rate(100)#Update at GEDC-6E update rate imuMsg = Imu() imuMsg.header.seq = 0 imuMsg.header.frame_id = "imu0" tempMsg = Temperature() tempMsg.header.seq = 0 tempMsg.header.frame_id = "imu0" imuMsg.orientation_covariance = [.000001,0.0,0.0, 0.0,.000001,0.0, 0.0,0.0,.000001] imuMsg.angular_velocity_covariance = [.000001,0.0,0.0, 0.0,.000001,0.0, 0.0,0.0,.000001] imuMsg.linear_acceleration_covariance = [.00117,0.0,0.0, 0.0,.00277,0.0, 0.0,0.0,.00034] try: ser = serial.Serial(imuPort, imuBaud) ser.flush()#make sure buffer is empty before we start looping except serial.SerialException: rospy.logerr("Error connecting to IMU.") while not rospy.is_shutdown(): if ser is None: continue imuMsg.header.stamp = rospy.get_rostime() imuMsg.header.seq += 1 #Quaternion mag data data = getImuData("$PSPA,QUAT\r\n") imuMsg.orientation.w = data[0] imuMsg.orientation.x = data[1] imuMsg.orientation.y = data[2] imuMsg.orientation.z = data[3] #Gyro data #Converted from millidegrees to radians data = getImuData("$PSPA,G\r\n") imuMsg.angular_velocity.x = data[0] * math.pi/180/1000 imuMsg.angular_velocity.y = data[1] * math.pi/180/1000 imuMsg.angular_velocity.z = data[2] * math.pi/180/1000 #Accelerometer data #Converted from milli-g's to m/s^2 data = getImuData("$PSPA,A\r\n") imuMsg.linear_acceleration.x = data[0] * 9.80665/1000 imuMsg.linear_acceleration.y = data[1] * 9.80665/1000 imuMsg.linear_acceleration.z = data[2] * 9.80665/1000 #Temperature data data = getImuData("$PSPA,Temp\r\n") tempMsg.header.stamp = rospy.get_rostime() tempMsg.header.seq += 1 tempMsg.temperature = data[0] pubImu.publish(imuMsg) pubTemp.publish(tempMsg) rate.sleep()
def publish(self): #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10) #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1) pub = rospy.Publisher('remote_readings', Temperature, queue_size=1) rate = rospy.Rate(40) #10Hz msg_temp = Temperature(); msg_temp.header.frame_id = "robocape" start_state = 0x7D0; lfsr = start_state; steer_low = 1440; steer_high= 1560; counterp = 0; steer_prbs = 1500 #a = [0.0, 0.0] #dim = MultiArrayDimension() #dim.size = len(a) #dim.label = "REMOTEmsg" #dim.stride = len(a) #apub = Float64MultiArray() #apub.data = a #apub.layout.dim.append(dim) #apub.layout.data_offset = 0 #cnt=0 while not rospy.is_shutdown(): #begin = time.time() #UART_read = ser.readline() #UART_read = ser.Serial.readline() #self.dev.flushInput() while self.dev.inWaiting() <= 11: pass UART_read = self.dev.readline() self.dev.flushInput() #if self.dev.inWaiting() > 80: # self.dev.flushInput() #if self.dev.inWaiting() > 50: # self.dev.flushInput() # cnt=0 #self.dev.flushInput() #rospy.loginfo(UART_read) steer_cmd = int(UART_read[0:4]) throt_cmd = int(UART_read[4:8]) #self.dev.flushInput() if counterp == 4 : bit = ((lfsr >> 0) ^ (lfsr >> 2)) & 1; lfsr = (lfsr >> 1) | (bit << 10); if bit == 1: steer_prbs = steer_high elif bit == 0: steer_prbs = steer_low else: steer_prbs = steer_cmd counterp = 0 if steer_cmd > steer_high: steer_cmd = steer_cmd elif steer_cmd < steer_low: steer_cmd = steer_cmd else: steer_cmd = steer_prbs counterp = counterp + 1 now = rospy.get_rostime() msg_temp.header.stamp.secs = now.secs msg_temp.header.stamp.nsecs = now.nsecs msg_temp.temperature = throt_cmd msg_temp.variance = steer_cmd self.dev1.pulsewidth_us(throt_cmd) self.dev2.pulsewidth_us(steer_cmd) pub.publish(msg_temp) rospy.loginfo([throt_cmd, steer_cmd]) #a = [throt_cmd, steer_cmd] #rospy.loginfo(a) #apub.data = a #msg_joint.position = apub #pub.publish(msg_joint) #buffer = self.dev.inWaiting() #rospy.loginfo(buffer) #cnt=cnt+1 #self.dev.flushInput() #length = time.time() - begin #rospy.loginfo(length) rate.sleep()
def publish_temp(timer_event): temp_msg = Temperature() temp_msg.header.frame_id = IMU_FRAME temp_msg.temperature = read_word_2c(TEMP_H)/340.0 + 36.53 temp_msg.header.stamp = rospy.Time.now() temp_pub.publish(temp_msg)
def publish(self): #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10) #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1) pub = rospy.Publisher('remote_readings', Temperature, queue_size=1) rate = rospy.Rate(40) #10Hz msg_temp = Temperature(); msg_temp.header.frame_id = "robocape" #a = [0.0, 0.0] #dim = MultiArrayDimension() #dim.size = len(a) #dim.label = "REMOTEmsg" #dim.stride = len(a) #apub = Float64MultiArray() #apub.data = a #apub.layout.dim.append(dim) #apub.layout.data_offset = 0 #cnt=0 while not rospy.is_shutdown(): #begin = time.time() #UART_read = ser.readline() #UART_read = ser.Serial.readline() #self.dev.flushInput() while self.dev.inWaiting() <= 11: pass UART_read = self.dev.readline() self.dev.flushInput() #if self.dev.inWaiting() > 80: # self.dev.flushInput() #if self.dev.inWaiting() > 50: # self.dev.flushInput() # cnt=0 #self.dev.flushInput() #rospy.loginfo(UART_read) steer_cmd = int(UART_read[0:4]) throt_cmd = int(UART_read[4:8]) #self.dev.flushInput() now = rospy.get_rostime() msg_temp.header.stamp.secs = now.secs msg_temp.header.stamp.nsecs = now.nsecs msg_temp.temperature = throt_cmd msg_temp.variance = steer_cmd self.dev1.pulsewidth_us(throt_cmd) self.dev2.pulsewidth_us(steer_cmd) pub.publish(msg_temp) rospy.loginfo([throt_cmd, steer_cmd]) #a = [throt_cmd, steer_cmd] #rospy.loginfo(a) #apub.data = a #msg_joint.position = apub #pub.publish(msg_joint) #buffer = self.dev.inWaiting() #rospy.loginfo(buffer) #cnt=cnt+1 #self.dev.flushInput() #length = time.time() - begin #rospy.loginfo(length) rate.sleep()