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 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 __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 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 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 _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 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 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()
