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