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 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 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 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 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 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 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 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 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 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 talker(self):
pressure_msg = FluidPressure()
pressure_msg.variance = 0
temp_msg = Temperature()
temp_msg.variance = 0
while not rospy.is_shutdown():
if self.ms5837.read():
pressure_msg.header.stamp = rospy.get_rostime()
pressure_msg.fluid_pressure = self.ms5837.pressure(
ms5837.UNITS_Pa)
temp_msg.header.stamp = rospy.get_rostime()
temp_msg.temperature = self.ms5837.temperature()
self.pub_pressure.publish(pressure_msg)
self.pub_temp.publish(temp_msg)
else:
rospy.roswarn('Failed to read pressure sensor!')
rospy.Rate(10).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(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) #40Hz
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() <= 40:
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(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()
elif sun_error >= 60*math.pi/180 and sun_error< 75*math.pi/180:
battery_in = battery_solar*math.cos(67.5*math.pi/180) #38%
elif sun_error >= 75*math.pi/180 and sun_error< 90*math.pi/180:
battery_in = battery_solar*math.cos(82.5*math.pi/180) #13%
#calc battery difference
battery_charge_increment = battery_in-battery_use-battery_drive
if (battery_charge+battery_charge_increment>total_battery_charge): battery_charge = total_battery_charge
else: battery_charge += battery_charge_increment
msg = Temperature()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "base_link"
msg.temperature = battery_charge/total_battery_charge*100
msg.variance = 0
battery_charge_publisher.publish(msg)
rospy.set_param("/battery/charge",battery_charge/total_battery_charge)
prev_second = time
#print("published",battery_charge/total_battery_charge)
total_sun_in += battery_in
total_drive_energy += battery_drive
total_hotel_energy += battery_use
time_between_charges += 1
if (battery_charge <= total_battery_charge*.2 and not charging):
number_charges += 1
print('Charge number: ', number_charges)
print('total_sun_in: ',total_sun_in)
print('total_drive_energy: ',total_drive_energy)
exit(1)
instant_temperature = rospy.Publisher('Instant/Temperature',
Temperature,
queue_size=1)
pub1 = rospy.Publisher('Altimeter', Float32, queue_size=1)
pub_pressure = rospy.Publisher('sensors/Pressure',
FluidPressure,
queue_size=20)
pub_temp = rospy.Publisher('sensors/Temperature', Temperature, queue_size=20)
rospy.init_node('Pressure_node', anonymous=True)
pressure_message = FluidPressure()
pressure_message.variance = 0
temp_message = Temperature()
temp_message.variance = 0
instant_temp = Temperature()
instant_temp.variance = 0
rate = rospy.Rate(1)
while True:
if not sensor_data.read():
print("Error reading sensor")
exit(1)
instant_temp.header.stamp = rospy.get_rostime()
instant_temp.temperature = sensor_data.temperature()
instant_temperature.publish(instant_temp)
if pressure_data.read():
def talker(self):
rospy.Subscriber(self.imu_topic, Imu, self.imu_callback)
odom_broadcaster = tf.TransformBroadcaster()
odom_pub = rospy.Publisher(self.odom_topic, Odometry, queue_size=10)
depth_pub = rospy.Publisher(self.depth_topic, Range, queue_size=10)
temperature_pub = rospy.Publisher(self.temperature_topic,
Temperature,
queue_size=10)
rospy.init_node('sonar_publisher', anonymous=True)
odom_msg = Odometry()
odom_quart = Quaternion()
depth_msg = Range()
temperature_msg = Temperature()
odom_msg.pose.covariance = [0] * 36
odom_msg.twist.covariance = [0] * 36
r = rospy.Rate(20)
self.x_pos = 0.0
self.y_pos = 0.0
self.theta = 0.0
while not rospy.is_shutdown():
# get linear velocity
data = self.DST.next()
# print data
if data is not None:
# print data
for key, value in data.iteritems():
if key == "speed":
self.v_x = value
rospy.loginfo("speed is %f", self.v_x)
elif key == "depth":
self.depth = value
rospy.loginfo("depth is %f", self.depth)
elif key == "temperature":
self.temperature = value
rospy.loginfo("temperature is %f", self.temperature)
else:
rospy.loginfo("data received is invalid")
else:
rospy.loginfo("no data is reveived")
# get current time
self.current_time = rospy.Time.now()
# change in time
self.DTT = (self.current_time - self.last_time).to_sec()
rospy.loginfo("time is %f", self.DTT)
# angular displacement
self.delta_theta = self.imu_z * self.DTT
# linear displacement in x
self.delta_x = (self.v_x * math.cos(self.theta)) * self.DTT
# linear displacement in y
self.delta_y = (self.v_x * math.sin(self.theta)) * self.DTT
# current position
self.x_pos += self.delta_x
self.y_pos += self.delta_y
self.theta += self.delta_theta
# calculate heading in quaternion
# rotate z upside down
odom_quart = tf.transformations.\
quaternion_from_euler(math.pi, 0, self.theta)
# rospy.loginfo(odom_quart)
odom_broadcaster.sendTransform((self.x_pos, self.y_pos, 0),
odom_quart, rospy.Time.now(),
self.fixed_frame, self.odom_frame)
odom_msg.header.stamp = self.current_time
odom_msg.header.frame_id = self.odom_frame
odom_msg.pose.pose.position.x = self.x_pos
odom_msg.pose.pose.position.y = self.y_pos
odom_msg.pose.pose.position.z = 0.0
odom_msg.pose.pose.orientation.x = odom_quart[0]
odom_msg.pose.pose.orientation.y = odom_quart[1]
odom_msg.pose.pose.orientation.z = odom_quart[2]
odom_msg.pose.pose.orientation.w = odom_quart[3]
odom_msg.pose.covariance[0] = 0.00001
odom_msg.pose.covariance[7] = 0.00001
odom_msg.pose.covariance[14] = 1000000000000.0
odom_msg.pose.covariance[21] = 1000000000000.0
odom_msg.pose.covariance[28] = 1000000000000.0
odom_msg.pose.covariance[35] = 0.001
odom_msg.child_frame_id = self.fixed_frame
odom_msg.twist.twist.linear.x = self.v_x
odom_msg.twist.twist.linear.y = 0.0
odom_msg.twist.twist.linear.z = 0.0
odom_msg.twist.twist.angular.x = 0.0
odom_msg.twist.twist.angular.y = 0.0
odom_msg.twist.twist.angular.z = self.imu_z
odom_msg.twist.covariance[0] = 0.00001
odom_msg.twist.covariance[7] = 0.00001
odom_msg.twist.covariance[14] = 1000000000000.0
odom_msg.twist.covariance[21] = 1000000000000.0
odom_msg.twist.covariance[28] = 1000000000000.0
odom_msg.twist.covariance[35] = 0.001
depth_msg.radiation_type = Range.ULTRASOUND
depth_msg.header.stamp = self.current_time
depth_msg.header.frame_id = self.odom_frame
depth_msg.field_of_view = 3 * 2 / math.pi
depth_msg.min_range = 0.2
depth_msg.max_range = 80
depth_msg.range = self.depth
temperature_msg.header.stamp = self.current_time
temperature_msg.header.frame_id = self.odom_frame
temperature_msg.temperature = self.temperature
temperature_msg.variance = 0.0
odom_pub.publish(odom_msg)
depth_pub.publish(depth_msg)
temperature_pub.publish(temperature_msg)
self.last_time = self.current_time
r.sleep()
w = 0.0
# velocities - where to get?
vx = 0.0
vy = 0.0
vth = 0.0
# header frame for odometry message
seq = 0
# define temperature message
temp_msg = Temperature()
# ignore the covariance data, it is optional
temp_msg.variance = 0
# define IMU message
imu_msg = Imu()
# ignore the covariance data, it is optional
imu_msg.orientation_covariance[0] = -1
imu_msg.angular_velocity_covariance[0] = -1
imu_msg.linear_acceleration_covariance[0] = -1
# run some parts only on the real robot
if hostname == 'minibot' or hostname == 'minibottest':
# load library
from Adafruit_BNO055 import BNO055
# Create and configure the BNO sensor connection.
altitude = (1.0 - math.pow(pressure / 1013.25, 0.190295)) * 44330
#print(' Altitude = %2.2f m\n' % altitude)
# Set Pressure variables
pressMsg = FluidPressure()
pressMsg.header.stamp = rospy.Time.now()
pressMsg.header.frame_id = "imu_link"
pressMsg.fluid_pressure = pressure
pressMsg.variance = 0
# Set Temperature variables
tempMsg = Temperature()
tempMsg.header.stamp = rospy.Time.now()
tempMsg.header.frame_id = "imu_link"
tempMsg.temperature = temperature
tempMsg.variance = 0
# Set Altitude variables
altMsg = altitude
# publish message
temp_pub.publish(tempMsg)
pressure_pub.publish(pressMsg)
alt_pub.publish(altMsg)
if em7180.gotMagnetometer():
mx, my, mz = em7180.readMagnetometer()
# Magnetic field vector
magneticVector = MagneticField()
