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 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 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 _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 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(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 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 _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 _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 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 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 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 _loop(self): while not rospy.is_shutdown(): rospy.sleep(self._measure_interval) try: temp = ds18b20_read_temp(self._device_name) msg = Temperature() msg.temperature = temp msg.header.stamp.secs = rospy.get_time() self._temp_pub.publish(msg) except Exception as e: msg = Temperature() msg.temperature = self._lost_data_marker self._logger.warning( "ds18b20_device_server {} error : {}".format( self._device_name, e)) msg.header.stamp.secs = rospy.get_time() self._temp_pub.publish(msg)
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 _loop(self): while not rospy.is_shutdown(): rospy.sleep(self._measure_interval) try: temp, hum = get_si7021_data() msg = Temperature() msg.temperature = temp msg.header.stamp.secs = rospy.get_time() self._temp_pub.publish(msg) msg = Temperature() msg.temperature = hum msg.header.stamp.secs = rospy.get_time() self._hum_pub.publish(msg) except Exception as e: msg = Temperature() msg.temperature = self._lost_data_marker msg.header.stamp.secs = rospy.get_time() self._temp_pub.publish(msg) self._hum_pub.publish(msg)
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 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 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 _loop(self): rospy.sleep(self._measure_interval) while not rospy.is_shutdown(): try: temp, pressure, altitude = get_bmp180_data() msg = Temperature() msg.temperature = temp msg.header.stamp.secs = rospy.get_time() self._temp_pub.publish(msg) msg = Temperature() msg.temperature = pressure msg.header.stamp.secs = rospy.get_time() self._pressure_pub.publish(msg) except Exception as e: msg = Temperature() msg.temperature = self._lost_data_marker msg.header.stamp.secs = rospy.get_time() self._temp_pub.publish(msg) self._pressure_pub.publish(msg)
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 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 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 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 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(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 start(self): r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): self.ms5837.read() tempmsg = Temperature() tempmsg.header = Header() tempmsg.header.stamp = rospy.Time.now() tempmsg.temperature = self.ms5837.temperature() self.pub_temp.publish(tempmsg) depthmsg = FluidDepth() depthmsg.header = Header() depthmsg.header.stamp = rospy.Time.now() depthmsg.header.frame_id = "pressure_link" depthmsg.depth = self.ms5837.depth() self.pub_depth.publish(depthmsg) r.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 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.temperature=wifi_strength distribution=PoseStamped() distribution.pose.position.x =sigma_x distribution.pose.position.y =sigma_y distribution.pose.position.z =sigma_z if wifi_aware==True: distribution.pose.orientation.w = 1 #wifi_pub.publish(wifi) wifi_pub.publish(distribution)
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)
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()
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)
imu_data.angular_velocity.x = float(st.unpack('h', st.pack('BB', buf[12], buf[13]))[0]) / gyr_fact imu_data.angular_velocity.y = float(st.unpack('h', st.pack('BB', buf[14], buf[15]))[0]) / gyr_fact imu_data.angular_velocity.z = float(st.unpack('h', st.pack('BB', buf[16], buf[17]))[0]) / gyr_fact imu_data.angular_velocity_covariance[0] = -1 pub_data.publish(imu_data) # Publish magnetometer data mag_msg.header.stamp = rospy.Time.now() mag_msg.header.frame_id = frame_id mag_msg.header.seq = seq mag_msg.magnetic_field.x = float(st.unpack('h', st.pack('BB', buf[6], buf[7]))[0]) / mag_fact mag_msg.magnetic_field.y = float(st.unpack('h', st.pack('BB', buf[8], buf[9]))[0]) / mag_fact mag_msg.magnetic_field.z = float(st.unpack('h', st.pack('BB', buf[10], buf[11]))[0]) / mag_fact pub_mag.publish(mag_msg) # Publish temperature temperature_msg.header.stamp = rospy.Time.now() temperature_msg.header.frame_id = frame_id temperature_msg.header.seq = seq temperature_msg.temperature = buf[44] pub_temp.publish(temperature_msg) #yaw = float(st.unpack('h', st.pack('BB', buf[18], buf[19]))[0]) / 16.0 #roll = float(st.unpack('h', st.pack('BB', buf[20], buf[21]))[0]) / 16.0 #pitch = float(st.unpack('h', st.pack('BB', buf[22], buf[23]))[0]) / 16.0 # print "RPY=(%.2f %.2f %.2f)" %(roll, pitch, yaw) seq = seq + 1 rate.sleep() ser.close()
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()