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