def publish(self, data): msg = Range() msg.header.stamp = rospy.get_rostime() msg.header.frame_id = self._frameId if self._urfType == URF_HRLV: msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar else: msg.min_range = MIN_RANGE_URF_LV_MaxSonar msg.max_range = MAX_RANGE_URF_LV_MaxSonar msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar msg.radiation_type = Range.ULTRASOUND msg.range = data self._pub.publish(msg)
def __init__(self): #初始化节点 rospy.init_node("talkerUltraSound", anonymous = True) #超声波数据发布节点 ultrasound_pub = rospy.Publisher("UltraSoundPublisher", Range, queue_size = 20) ran_broadcaster = tf.TransformBroadcaster() rate=rospy.Rate(1) while not rospy.is_shutdown(): ran_quat = Quaternion() ran_quat = quaternion_from_euler(0, 0, 0) #发布TF关系 ran_broadcaster.sendTransform((0.2,0.0,0.2),ran_quat,rospy.Time.now(),"ultrasound","base_link") #定义一个超声波消息 ran = Range() ran.header.stamp = rospy.Time.now() ran.header.frame_id = "/ultrasound" ran.field_of_view = 1; ran.min_range = 0; ran.max_range = 5; ran.range = 0.5; ultrasound_pub.publish(ran) rate.sleep()
def range_test(): from sensor_msgs.msg import Range sensor = RangeSensor() print sensor m = Range() m.range = 2000.0 m.max_range = 3000.0 m.min_range = 2.01 sensor.min_safe = 500 sensor.measure(m) print sensor print sensor.isNear() print sensor.isFar() m.range = 4000 sensor.measure(m) print sensor print sensor.isNear() print sensor.isFar() m.range = -2000 sensor.measure(m) print sensor print sensor.isNear() print sensor.isFar() m.range = 300 sensor.measure(m) print sensor print sensor.isNear() print sensor.isFar()
def test_range(self): sensor = Sensors.Range(min_safe=10) msg = Range() msg.max_range = 3000 msg.min_range = 5 msg.range = 2500 sensor.measure(msg) self.assertEquals(sensor.max_range, msg.max_range) self.assertEquals(sensor.min_range, msg.min_range) self.assertEquals(sensor.range , msg.range) self.assertFalse(sensor.isFar()) self.assertFalse(sensor.isNear()) msg.range = 4001 sensor.measure(msg) self.assertTrue(sensor.isFar()) self.assertFalse(sensor.isNear()) msg.range = 3 sensor.measure(msg) self.assertFalse(sensor.isFar()) self.assertTrue(sensor.isNear()) msg.range = 8 sensor.measure(msg) self.assertFalse(sensor.isFar()) self.assertTrue(sensor.isNear())
def callDist(data): #reads voltage value voltage = ADC.read("P9_40") #converts voltage values into distance(meters) distance = (voltage**-.8271732796) distance = distance*.1679936709 #checks and discards data outside of accurate range if distance>2: distance = 2 elif distance<.15: distance = .15 #Writes data to Range message msg = Range() header = Header() #creates header msg.header.stamp.secs = rospy.get_time() msg.header.frame_id = "/base_link" msg.radiation_type = 1 msg.field_of_view = .15 #about 8.5 degrees msg.min_range = .15 msg.max_range = 2 msg.range = distance pub.publish(msg)
def Publish(self): #初始化节点 rospy.init_node("talkerUltraSound", anonymous = True) #超声波数据发布节点 self.ultrasound_pub = rospy.Publisher("UltraSoundPublisher", Range, queue_size = 20) ran_broadcaster = tf.TransformBroadcaster() rate=rospy.Rate(10) #print "Input distance('+' is the end of the number):" while not rospy.is_shutdown(): #ch=self.getKey() #self.addChar(ch) ran_quat = Quaternion() ran_quat = quaternion_from_euler(0, 0, 0) #发布TF关系 ran_broadcaster.sendTransform((0.2,0.0,0.2),ran_quat,rospy.Time.now(),"ultrasound","base_link") #定义一个超声波消息 ran = Range() ran.header.stamp = rospy.Time.now() ran.header.frame_id = "/ultrasound" ran.field_of_view = 1; ran.min_range = 0; ran.max_range = 5; ran.range = self.dist; #ultrasound_pub.publish(ran) rate.sleep()
def prepareArray(self): for i in range(0, len(self.sensors)): r = Range() r.header.frame_id = "ir{}_link".format(i + 1) r.min_range = self.sensors[i].min_range r.max_range = self.sensors[i].max_range r.radiation_type = r.INFRARED self.msg.array += [r]
def trigger(self): ds = Range() ds.header.frame_id = '/ultrasonic_link' ds.header.stamp = rospy.Time.now() ds.range = self.ultrasonic.get_sample()/100.0 ds.field_of_view = self.spread ds.min_range = self.min_range ds.max_range = self.max_range self.pub.publish(ds)
def _msg(self, frame_id, distance): msg = Range() msg.header.frame_id = frame_id msg.header.stamp = rospy.Time.now() msg.radiation_type = self._type msg.field_of_view = self._fov msg.min_range = self._min msg.max_range = self._max msg.range = min(max(distance, msg.min_range), msg.max_range) return msg
def sendMessage(sonarId, distance): sonarDictionary = {'1':"frontLeftSonar", '2':"diagLeftSonar", '3':"sideLeftSonar", '4':"frontRightSonar", '5':"diagRightSonar", '6':"sideRightSonar"} sonarName = sonarDictionary[sonarId] sonar = Range() sonar.header.stamp = rospy.Time.now() sonar.header.frame_id = sonarName sonar.range = float(distance) sonar.field_of_view = .3489 sonar.max_range = 6.45 sonar.min_range = .1524 return sonar
def send_range(self, msg, current_time): # ultra sonic range finder scan = Range() scan.header.stamp = current_time scan.header.frame_id = "forward_sensor" scan.radiation_type = 0 scan.field_of_view = 60*pi/180 scan.min_range = 0.0 scan.max_range = 4.0 scan.range = msg.d1/100.0 self.pub_sonar.publish(scan)
def sonar_publisher(): # NAO connect # Connect to ALSonar module. sonarProxy = ALProxy("ALSonar", IP, PORT) # Subscribe to sonars, this will launch sonars (at hardware level) and start data acquisition. sonarProxy.subscribe("ROS_sonar_publisher") #Now you can retrieve sonar data from ALMemory. memoryProxy = ALProxy("ALMemory", IP, PORT) #ROS pub_right = rospy.Publisher(TOPIC_NAME+'right', Range, queue_size=10) pub_left = rospy.Publisher(TOPIC_NAME+'left', Range, queue_size=10) rospy.init_node('nao_sonar_publisher') r = rospy.Rate(PUBLISH_RATE) while not rospy.is_shutdown(): # Get sonar left first echo (distance in meters to the first obstacle). left_range = memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value") # Same thing for right. right_range = memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value") left = Range() left.header.stamp = rospy.Time.now() left.header.frame_id = '/torso' left.radiation_type = Range.ULTRASOUND left.field_of_view = 60 left.min_range = 0.25 # m left.max_range = 2.55 # m left.range = left_range right = Range() right.header.stamp = rospy.Time.now() right.header.frame_id = '/torso' right.radiation_type = Range.ULTRASOUND right.field_of_view = 60 right.min_range = 0.25 # m right.max_range = 2.55 # m right.range = right_range pub_right.publish(right) pub_left.publish(left) r.sleep()
def default(self, ci="unused"): msg = Range() msg.radiation_type = Range.INFRARED msg.field_of_view = 20 msg.min_range = 0 msg.max_range = self.component_instance.bge_object["laser_range"] tmp = msg.max_range for ray in self.data["range_list"]: if tmp > ray: tmp = ray msg.range = tmp self.publish(msg)
def talker(): signal.signal(signal.SIGINT, signal_handler) time.sleep(1) setup_servo() pub = rospy.Publisher('servo_ulta', Range, queue_size=10) rospy.init_node('servo_ultra', anonymous=True, disable_signals=False ) rate = rospy.Rate(10) # 10hz ultrasonic_number = 1 pause_time = 0.1 delay_ultrasonic = 0.11 STEP_ANGLE = 10 CURR_ANGLE = (90.0 * math.pi) / 180.0 mindt = 0.65 maxdt = 2.48 dt = [mindt, maxdt] extremes = [int(x * 1024.0 / 20.0) for x in dt] dt_range = extremes[1] - extremes[0] dt_step = int((dt_range / 180.0) * STEP_ANGLE) br = tf.TransformBroadcaster() while not rospy.is_shutdown(): #broadcast_transforms() # Move servo to counter-clockwise extreme. wiringpi.pwmWrite(18, extremes[1]) sleep(0.2) CURR_ANGLE = (90.0 * math.pi) / 180.0 for i in range(extremes[1],extremes[0],-dt_step): # 1025 because it stops at 1024 wiringpi.pwmWrite(18,i) sleep(pause_time) br.sendTransform((0.15, 0.0, 0.0),tf.transformations.quaternion_about_axis(CURR_ANGLE, (0, 0, 1)), rospy.Time.now(), "1", "base_link") data = Range() data.header.frame_id = "1" data.header.stamp = rospy.Time.now() data.radiation_type = 0 data.min_range = 0.05 data.max_range = 2.0 data.field_of_view = 0.164 try : data.range = float(get_us_data_from(ultrasonic_number)) /100.0 except IOError: subprocess.call(['i2cdetect', '-y', '1']) data.range = 0.0 pub.publish(data) CURR_ANGLE -= (STEP_ANGLE * math.pi) / 180.0
def default(self, ci='unused'): msg = Range() msg.header = self.get_ros_header() msg.radiation_type = Range.INFRARED msg.field_of_view = 20 msg.min_range = 0 msg.max_range = self.component_instance.bge_object['laser_range'] tmp = msg.max_range for ray in self.data['range_list']: if tmp > ray: tmp = ray msg.range = tmp self.publish(msg)
def default(self, ci='unused'): """ Publish the data of the infrared sensor as a ROS Range message """ msg = Range() msg.radiation_type = Range.INFRARED msg.field_of_view = 20 msg.min_range = 0 msg.max_range = self.component_instance.bge_object['laser_range'] tmp = msg.max_range for ray in self.data['range_list']: if tmp > ray: tmp = ray msg.range = tmp self.publish(msg)
def inputCallback(self, msg): ######################################################################### # rospy.loginfo("-D- range_filter inputCallback") cur_val = msg.value if cur_val <= self.max_valid and cur_val >= self.min_valid: self.prev.append(cur_val) del self.prev[0] p = array(self.prev) self.rolling_ave = p.mean() self.rolling_std = p.std() self.rolling_meters = ((self.b * self.rolling_ave) ** self.m) / 100 self.filtered_pub.publish( self.rolling_meters ) self.std_pub.publish( self.rolling_std ) rng = Range() rng.radiation_type = 1 rng.min_range = self.min_range rng.max_range = self.max_range rng.range = self.rolling_meters rng.header.frame_id = self.frame rng.field_of_view = 0.1 rng.header.stamp = rospy.Time.now() self.range_pub.publish( rng ) ranges = [] intensities = [] angle_start = 0.0 - self.field_of_view angle_stop = self.field_of_view for angle in linspace( angle_start, angle_stop, 10): ranges.append( self.rolling_meters ) intensities.append( 1.0 ) scan = LaserScan() scan.ranges = ranges scan.header.frame_id = self.frame scan.time_increment = 0; scan.range_min = self.min_range scan.range_max = self.max_range scan.angle_min = angle_start scan.angle_max = angle_stop scan.angle_increment = (angle_stop - angle_start) / 10 scan.intensities = intensities scan.scan_time = self.scan_time scan.header.stamp = rospy.Time.now() self.scan_pub.publish( scan )
def getDistance(self): while True: self.dist=input("plase input dis:") try: print "you have input:"+str(self.dist) except: print "you have input nothing" ran = Range() ran.header.stamp = rospy.Time.now() ran.header.frame_id = "/ultrasound" ran.field_of_view = 1; ran.min_range = 0; ran.max_range = 5; ran.range = self.dist; self.ultrasound_pub.publish(ran)
def get_height(self): voltage = self.adc.get_last_result() if voltage <= 0: voltage = 1 rospy.logerr("ERROR: BAD VOLTAGE!!!") return #rospy.loginfo("voltage = " + str(voltage)) self.distance = pow(voltage, self.p) * self.m #rospy.loginfo("distance = " + str(self.distance)) msg = Range() msg.max_range = 0.8 msg.min_range = 0 msg.range = self.distance msg.header.stamp = rospy.Time.now() self.pub_height.publish(msg)
def sample(self): data = self.ping.get_distance() if data: msg = Range() msg.radiation_type = 0 msg.min_range = 0.5 msg.max_range = 30.0 msg.range = float(data["distance"]) / 1000.0 msg.field_of_view = float( data["confidence"] ) / 100.0 # Abusing this field to store the confidence value msg.header.stamp = self.get_clock().now().to_msg() self.pub.publish(msg) else: print("Failed to get distance data")
def create_ranged_message(distance): ''' Create a ranged message for publishing sonar values The field of view takes the dual sonar implementation in account ''' message = Range() message.header.stamp = rospy.Time.now() message.header.frame_id = 'base_footprint' message.radiation_type = 0 message.field_of_view = math.pi / 25 # [rad] - 7.2 deg message.min_range = 0.05 # [m] closest 1 cm message.max_range = 3.50 # [m] furtheset 100 cm / 1m message.range = distance - 0.05 return message
def main(): rospy.init_node("infrared_pub") pub = rospy.Publisher('/pidrone/infrared', Range, queue_size=1) rnge = Range() rnge.max_range = 0.8 rnge.min_range = 0 rnge.header.frame_id = "base" r = rospy.Rate(100) print "publishing IR" while not rospy.is_shutdown(): rnge.header.stamp = rospy.get_rostime() rnge.header.frame_id = "ir_link" rnge.range = get_range() pub.publish(rnge) r.sleep()
def talker(): pub = rospy.Publisher("sensor1", Range, queue_size=50) rospy.init_node("robot", anonymous=True) rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): msg = Range() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "/sensor1" msg.min_range = 0 msg.max_range = 2 msg.radiation_type = Range.ULTRASOUND rospy.loginfo(msg) pub.publish(msg) rate.sleep()
def distance_sonic(): pub = rospy.Publisher('IR_top', Range, queue_size=20) rospy.init_node('qqqqq', anonymous=True) rate = rospy.Rate(10) r = Range() r.header.frame_id = '/IR_top' r.field_of_view = 0.26 r.max_range = 4.5 r.min_range = 0.05 print('IR start') while not rospy.is_shutdown(): r.header.stamp = rospy.Time.now() r.range = sensor_distance() pub.publish(r) rate.sleep()
def publish_range(): global distance, pub # compose the message sensor_range = Range() sensor_range.header.stamp = rospy.Time.now() sensor_range.radiation_type = Range.ULTRASOUND sensor_range.field_of_view = 0.26 sensor_range.min_range = 0 sensor_range.max_range = 2 sensor_range.range = distance/100 # distance in meters sensor_range.header.frame_id = "dextrobot_front_sonar" # publish the composed message pub.publish(sensor_range)
def publish_distance_data(self, time, data): range_msg = Range() range_msg.header.stamp = time range_msg.header.frame_id = "sonar" range_msg.header.seq = data["ping_number"] range_msg.radiation_type = Range.ULTRASOUND range_msg.min_range = data["scan_start"] / 1000.0 range_msg.max_range = (data["scan_start"] + data["scan_length"]) / 1000.0 range_msg.range = data["distance"] / 1000.0 self.distance_pub.publish(range_msg) pass
def timer_callback(self): # create the message containing the moving average for i in range(len(self.sensor)): # print "publishing" distance = self.sensor[i].readRangeData() if (distance < 14000 and distance > 200): terarangers_msg = Range() terarangers_msg.header.frame_id = "base_range" terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.radiation_type = 1 terarangers_msg.field_of_view = 0.0593 terarangers_msg.min_range = 200 terarangers_msg.max_range = 14000 # 14 metres terarangers_msg.range = distance # publish the moving average self.range_pub[i].publish(terarangers_msg)
def quad_sub(self, odom): for tag_name in self.tag_pos.keys(): r = Range() r.header.frame_id = tag_name dist = self.distance(self.tag_pos[tag_name], odom.pose.pose.position) dist = dist + random.gauss(0, 0.07) if dist < 0.1: dist = 0.1 r.range = dist r.field_of_view = math.pi * 0.1 r.min_range = 0 r.max_range = 300 self.tag_pub[tag_name].publish(r) self.alt_pub(odom) self.odom_pub(odom)
def publish(): while not rospy.is_shutdown(): pub = rospy.Publisher("distance", Range, queue_size=10) rospy.init_node("ultrasonic", anonymous=True) rate = rospy.Rate(4) #4Hz msg = Range(radiation_type=0) ## ULTRASOUND = ## IR = 1 msg.range = get_distance() msg.min_range = min_range msg.max_range = max_range if msg.range < max_range and msg.range > min_range: pub.publish(msg) rate.sleep() rospy.loginfo("%.2f" % (msg.range))
def to_msg(self, sensor_id, distance): msg = Range() msg.header = Header() msg.header.stamp = self.get_clock().now().to_msg() # need to define transforms for all sensors msg.header.frame_id = f"ultrasonic_sensor_{sensor_id}" msg.radiation_type = 0 msg.field_of_view = 0.0 msg.min_range = self.min_range msg.max_range = self.max_range msg.range = distance return msg
def sendLidar(): global lasers_raw lidar_publisher = rospy.Publisher('mavros/distance_sensor/lidar', Range, queue_size=1) rate = rospy.Rate(30) msg = Range() sendLidar_count = 0 msg.radiation_type = msg.INFRARED msg.field_of_view = 0.0523599 # 3° in radians msg.min_range = 0.05 msg.max_range = 20.0 while run: msg.header.stamp = rospy.Time.now() msg.header.seq=sendLidar_count msg.range = (lasers_raw.lasers[4] + lasers_raw.lasers[5])/200 lidar_publisher.publish(msg) sendLidar_count = sendLidar_count + 1 rate.sleep()
def talker(): pub = rospy.Publisher('range', Range, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(2) # 2hz msg = Range() seq = 0 while not rospy.is_shutdown(): seq += 1 msg.header.seq = seq msg.radiation_type = 0 msg.field_of_view = 30.0 msg.min_range = 0.2 msg.max_range = 2.0 msg.range = random.random() * 2.0 pub.publish(msg) rate.sleep()
def main(): trig_pin, echo_pin, units = setup() range_msg = Range() range_msg.header.frame_id = "ultrasonic_front" range_msg.radiation_type = 0 range_msg.field_of_view = 0.1 #Fake value range_msg.min_range = 0.78 range_msg.max_range = 157.48 rate = rospy.Rate(1) while not rospy.is_shutdown(): distance = get_sonar_readings(trig_pin, echo_pin, units) # print(distance) range_msg.header.stamp = rospy.Time.now() range_msg.range = distance pub.publish(range_msg) rate.sleep()
def pointcloud_cb(self, pc: PointCloud2): """Process new sensor information of depth camera.""" out_msg = Range() out_msg.field_of_view = self.param.tof.field_of_view out_msg.min_range = self.param.tof.min_range out_msg.max_range = self.param.tof.max_range out_msg.header.frame_id = self.param.frame_id vecs = (Vector(p) for p in sensor_msgs.point_cloud2.read_points( pc, field_names=("x", "y", "z"))) out_msg.range = min(v.norm for v in vecs) rospy.logdebug(f"Pointcloud received in {rospy.get_name()}:{vecs}") rospy.logdebug(f"Publishing range: {out_msg.range}") self.publisher.publish(out_msg)
def talker(): pub = rospy.Publisher('range', Range) rospy.init_node('talker') ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')] min_range = 2.0 max_range = 2.0 while not rospy.is_shutdown(): for rg in ranges: r = Range() r.header.stamp = rospy.Time.now() r.header.frame_id = "/base_link" r.radiation_type = 0 r.field_of_view = 0.1 r.min_range = min_range r.max_range = max_range r.range = rg pub.publish(r) rospy.sleep(1.0)
def sensor(): pub_sonar = [] for topic in sonar_topics: pub_sonar.append(rospy.Publisher(topic, Range, queue_size=10)) rospy.init_node("sensor", anonymous=True) rate = rospy.Rate(10) range_msg = Range() range_msg.field_of_view = 0.05 range_msg.min_range = 0.0 range_msg.max_range = 5.0 while not rospy.is_shutdown(): sonar_data = [200, 200, 200, 200, 1000, 1000, 1000, 1000] i = 0 print("\033c") for elem in sonar_data: range_msg.header.frame_id = sonar_topics[i] range_msg.range = elem / 1000.0 if elem == 0: range_msg.range = 5.0 pub_sonar[i].publish(range_msg) # rospy.loginfo(range_msg.range) bar = "" if int(elem) == 0: for k in range(1, 100): bar += "*" for j in range(1, int(elem) / 20): bar += "-" if j > 100: break rospy.loginfo(bar) i = i + 1 rate.sleep()
def step(self): stamp = super().step() if not stamp: return # Publish distance sensor data if self._publisher.get_subscription_count() > 0 or self._always_publish: self._wb_device.enable(self._timestep) msg = Range() msg.header.stamp = stamp msg.header.frame_id = self._frame_id msg.field_of_view = self._wb_device.getAperture() msg.min_range = self._min_range msg.max_range = self._max_range msg.range = interpolate_lookup_table(self._wb_device.getValue(), self._wb_device.getLookupTable()) msg.radiation_type = Range.INFRARED self._publisher.publish(msg) else: self._wb_device.disable()
def read_pose(self,pos): '''Callback when new pose topic has come. Updates distance and control signals''' distance=measure_distance_front((pos.x,pos.y,pos.theta)) range_message=Range() range_message.header.stamp = rospy.Time.now() range_message.field_of_view=0.1 range_message.min_range=0 range_message.max_range=10 range_message.range=distance self.distance_publisher.publish(range_message) self.latching_controller(distance)
def node_run(): rospy.init_node("UltraSonicSensor") pub = rospy.Publisher("UltraSonicRange", Range) #rate for measuring and publishing the distance 10 = 10Hz = 10 times per second rate = rospy.Rate(10) data = Range() while not rospy.is_shutdown(): #measure and publish data.radiation_type = data.ULTRASOUND data.field_of_view = 0.0 data.min_range = 0.01 data.max_range = 10.0 #devide by 100, Function returns cm, Message expects metres data.range = get_distance() / 100.0 pub.publish(data) rate.sleep()
def __init__( self, num_sonar, gpio_trigger_list, #-- list of all the trigger pins, starting from left gpio_echo_list, #-- list of all the echo pins, starting from left range_min, #- [m] range_max, #- [m] angle_min_deg, #- [deg] angle_max_deg): self.sonar_array = [] self.pub_array = [] self.num_sonar = num_sonar delta_angle_deg = (angle_max_deg - angle_min_deg) / float(num_sonar - 1) rospy.loginfo("Initializing the arrays") #--- Create an array and expand the object with its angle for i in range(num_sonar): sonar = Sonar(gpio_trigger_list[i], gpio_echo_list[i], range_min=range_min * 100, range_max=range_max * 100) angle_deg = angle_min_deg + delta_angle_deg * i sonar.angle = math.radians(angle_deg) self.sonar_array.append(sonar) rospy.loginfo("Sonar %d set" % i) #--- Publishers topic_name = "/dkcar/sonar/%d" % i pub = rospy.Publisher(topic_name, Range, queue_size=5) self.pub_array.append(pub) rospy.loginfo("Publisher %d set with topic %s" % (i, topic_name)) #--- Default message message = Range() # message.ULTRASOUND = 1 # message.INFRARED = 0 message.radiation_type = 0 message.min_range = range_min message.max_range = range_max self._message = message
def start(self): self.enable = True self.ultrasonicPub = rospy.Publisher(self.robot_host + '/ultrasonic/front/left/distance', Range, queue_size=10) self.ultrasonicVelocityPub = rospy.Publisher(self.robot_host + '/ultrasonic/front/left/relative_velocity', RelativeVelocity, queue_size=10) ultrasonicFrontLeft = UltrasonicHCSR04(23, 24) #ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')] min_range = 3 max_range = 400 while not rospy.is_shutdown(): distance = ultrasonicFrontLeft.distance() relative_velocity = ultrasonicFrontLeft.speed() message_str = "frontLeftUltrasonic Distance: %s cm and Speed: %s m/s" % (distance, relative_velocity) rospy.loginfo(message_str) #for distance in ranges: r = Range() rv = RelativeVelocity() r.header.stamp = rospy.Time.now() r.header.frame_id = "/base_link" r.radiation_type = Range.ULTRASOUND r.field_of_view = 0.26179938779915 # 15 degrees r.min_range = min_range r.max_range = max_range r.range = distance rv.header.stamp = rospy.Time.now() rv.header.frame_id = "/base_link" rv.radiation_type = Range.ULTRASOUND rv.field_of_view = 0.26179938779915 # 15 degrees rv.relative_velocity = relative_velocity self.ultrasonicPub.publish(r) self.ultrasonicVelocityPub.publish(rv) self.rate.sleep()
def start(self): self.enable = True self.infraredPub = rospy.Publisher(self.robot_host + '/infrared/rear/distance', Range, queue_size=10) self.infraredVelocityPub = rospy.Publisher(self.robot_host + '/infrared/rear/relative_velocity', RelativeVelocity, queue_size=10) infrared = IRGP2Y0A02YKOF(1) #ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')] min_range = 20 max_range = 150 while not rospy.is_shutdown(): distance = infrared.distance() relative_velocity = infrared.speed() message_str = "rearInfrared Distance: %s cm and Speed: %s m/s" % (distance, relative_velocity) rospy.loginfo(message_str) #for distance in ranges: r = Range() rv = RelativeVelocity() r.header.stamp = rospy.Time.now() r.header.frame_id = "/base_link" r.radiation_type = Range.INFRARED r.field_of_view = 0.087266462599716 # 5 degrees r.min_range = min_range r.max_range = max_range r.range = distance rv.header.stamp = rospy.Time.now() rv.header.frame_id = "/base_link" rv.radiation_type = Range.INFRARED rv.field_of_view = 0.087266462599716 # 5 degrees rv.relative_velocity = relative_velocity self.infraredPub.publish(r) self.infraredVelocityPub.publish(rv) self.rate.sleep()
def ultrasonic_range(): global ranger pub = rospy.Publisher('range', Range, queue_size=1) # 5Hz / Sensor could support 10Hz but the process # would use a lot of CPU in bad cases rate = rospy.Rate(10) range = Range() range.header.frame_id = "range_forward" range.radiation_type = 0 range.field_of_view = 20 range.min_range = 0.02 range.max_range = 4.00 while not rospy.is_shutdown(): range.range = ranger.get_distance_cm() / 100 pub.publish(range) rate.sleep() rospy.on_shutdown(shutdown_ranger)
def callback(event): value = ADC.read("P9_39") outvoltage = 3.3 * value #minimum range if outvoltage > 2.75: rangevalue = 15 elif outvoltage < .4: rangevalue = 150 else: #exponential fit function, derived from data sheet rangevalue = 61.7 * math.pow(outvoltage, -1.2) #assign values and publish pub = rospy.Publisher('/range', Range, queue_size=10) rangeout = Range() rangeout.min_range = 15 rangeout.max_range = 150 rangeout.range = rangevalue rospy.loginfo(rangeout) pub.publish(rangeout)
def run_lidar(self, event): if self.ser.in_waiting >= 9: if (b'Y' == self.ser.read()) and ( b'Y' == self.ser.read()): dist_l = self.ser.read() dist_h = self.ser.read() self.distance = (ord(dist_h) * 256) + (ord(dist_l)) strength_l = self.ser.read() strength_h = self.ser.read() self.strength = (ord(strength_h) * 256) + (ord(strength_l)) for i in range (0, 3): self.ser.read() distance_msg = Range() distance_msg.range = float(self.distance) / 100 distance_msg.min_range = 0.30 distance_msg.max_range = 7.0 self.distance_pub.publish(distance_msg) print (self.distance, self.strength) self.ser.flush()
def add_range_measurement(self, new_range_message, sensor): minimum_fov = 0.001 self.last_measurements[sensor] = [new_range_message.range ] + self.last_measurements[sensor] del self.last_measurements[sensor][10:] # Calculating the mean and variance self.avg_measurement[sensor] = sum( self.last_measurements[sensor]) / len( self.last_measurements[sensor]) sum_of_squared_error = 0 for measurement in self.last_measurements[sensor]: sum_of_squared_error += (measurement - self.avg_measurement[sensor])**2 self.variance[sensor] = sum_of_squared_error / len( self.last_measurements[sensor]) # Creates a range message range_and_variance_message = Range() range_and_variance_message.radiation_type = Range.ULTRASOUND range_and_variance_message.header.frame_id = "/ultrasound/" + sensor range_and_variance_message.min_range = 25.0 range_and_variance_message.max_range = 500.0 range_and_variance_message.range = self.avg_measurement[sensor] range_and_variance_message.field_of_view = np.clip( self.variance[sensor] / 1000, 0, 10) # Using FOV as variance with a max limit at ten # Publishing on the right topic if sensor == "left": self.left_measure_and_variance_pub.publish( range_and_variance_message) elif sensor == "center": self.center_measure_and_variance_pub.publish( range_and_variance_message) elif sensor == "right": self.right_measure_and_variance_pub.publish( range_and_variance_message)
def main(): pub = rospy.Publisher('ernest_sensors/imu', Imu, queue_size=10) eyes = rospy.Publisher('ernest_sensors/eyes', Range, queue_size=10) nunchuck = rospy.Publisher('ernest_sensors/joy', Joy, queue_size=10) nunchuck_giro = rospy.Publisher('ernest_sensors/joy_giro', Imu, queue_size=10) comm = ArduinoCommunicator() rospy.init_node('ernest_sensors') for line in comm.loop(): print line try: x, y, z, dis, joy_x, joy_y, giro_x, giro_y, giro_z, but_c, but_z = line.split(",") imu = Imu() imu.header.frame_id = "imu" imu.linear_acceleration.x = -float(x) imu.linear_acceleration.y = float(z) imu.linear_acceleration.z = float(y) r = Range() r.header.frame_id = "imu" r.radiation_type = 1 r.field_of_view = 0.5 r.min_range = 0.20 r.max_range = 3 r.range = float(dis)/100.0 j = Joy() j.axes = [ maprange(float(joy_x), 25, 226, -1, 1), maprange(float(joy_y), 32, 223, -1, 1) ] j.buttons = [int(but_c), int(but_z)] imu2 = Imu() imu2.header.frame_id = "imu" imu2.linear_acceleration.x = (float(giro_y) - 512) / 512.0 imu2.linear_acceleration.y = -(float(giro_x) - 512) / 512.0 imu2.linear_acceleration.z = -(float(giro_z) - 512) / 512.0 pub.publish(imu) eyes.publish(r) nunchuck.publish(j) nunchuck_giro.publish(imu2) except ValueError: continue
def post_range(self, component_instance): """ Publish the data on the rostopic http://www.ros.org/doc/api/sensor_msgs/html/msg/Range.html """ msg = Range() #msg.header.frame_id = 'infrared' msg.radiation_type = Range.INFRARED msg.field_of_view = 20 msg.min_range = 0 msg.max_range = component_instance.blender_obj['laser_range'] tmp = component_instance.blender_obj['laser_range'] for r in component_instance.local_data['range_list']: if tmp > r: tmp = r msg.range = tmp parent_name = component_instance.robot_parent.blender_obj.name for topic in self._topics: # publish the message on the correct topic if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name): topic.publish(msg)
def camera_callback(self): image = [] self.camera.enable(self.timestep) self.frontDistance_sensor.enable(self.timestep) self.camera.recognitionEnable(self.timestep) msg_camera = Range() if (len(self.camera.getRecognitionObjects()) > 0): object = self.camera.getRecognitionObjects()[0] position = (abs(object.get_position()[0])) color = object.get_colors() self.get_logger().info(str(color)) #id = object.get_id() pos_img = object.get_position_on_image() #msg_camera.radiation_type = id msg_camera.range = position if ((abs(position < 0.02))): if (self.object1 == 0.0 and self.object2 == 0.0 and self.object3 == 0.0): self.object1 = self.frontDistance_sensor.getValue() self.id1 = id if (self.object1 != 0.0 and self.object2 == 0.0 and self.object3 == 0.0 and self.id1 != id): self.object2 = self.frontDistance_sensor.getValue() self.id2 = id if (self.object1 != 0.0 and self.object2 != 0.0 and self.object3 == 0.0 and self.id2 != id): self.object3 = self.frontDistance_sensor.getValue() self.id3 = id if (self.object1 > 0.0 and self.object2 > 0.0 and self.object3 > 0.0 and self.count == 0): msg_camera.field_of_view = self.object1 msg_camera.min_range = self.object2 msg_camera.max_range = self.object3 self.camera_publisher.publish(msg_camera) self.get_logger().info(str(msg_camera)) self.count = 1
def lightware_ros(): rospy.init_node('lightware_ros') # ROS parameters hz = rospy.get_param('~rate', -1) port = rospy.get_param('~port', '/dev/ttyUSB0') t_out = rospy.get_param('~timeout', 0.05) baudrate = rospy.get_param('~baudrate', 115200) # init ROS stuff laser_pub = rospy.Publisher('range', Range, queue_size=10) if hz > 0: rate = rospy.Rate(hz) with serial.Serial(port, baudrate, timeout=t_out) as ser: while ser.isOpen() and not rospy.is_shutdown(): ser.reset_input_buffer() payload = ser.readline() range_string = payload.split("m")[0] try: distance = float(range_string) # populate message msg = Range() msg.header.stamp = rospy.Time.now() msg.radiation_type = 1 msg.field_of_view = 0.0035 # rad msg.min_range = 0.0 # m msg.max_range = 50.0 # m msg.range = distance laser_pub.publish(msg) except ValueError: rospy.logwarn('Serial Read Error: %s', payload) if hz > 0: rate.sleep()
def talker(): signal.signal(signal.SIGINT, signal_handler) time.sleep(1) pub = rospy.Publisher('ultrasonic_data', Range, queue_size=10) rospy.init_node('ultrasonic_node', anonymous=True, disable_signals=False ) rate = rospy.Rate(10) # 10hz ultrasonic_number = 1 while not rospy.is_shutdown(): data = Range() data.header.frame_id = ""+str(ultrasonic_number) data.header.stamp = rospy.Time.now() data.radiation_type = 0 # ULTRASONIC data.min_range = 5.0 data.max_range = 200.0 try : data.range = get_us_data_from(ultrasonic_number) except IOError: subprocess.call(['i2cdetect', '-y', '1']) data.range = 0 #rospy.loginfo() pub.publish(data) ultrasonic_number += 1 if ultrasonic_number == 6: ultrasonic_number = 1
from sensor_msgs.msg import Range from tf.broadcaster import TransformBroadcaster import ranger RATE = 40 #Hz BASE_FRAME = "base_link" range_left = Range() range_left.header.frame_id = "ir_left" range_left.radiation_type = Range.INFRARED range_left.field_of_view = 0.4 # ~20deg range_left.min_range = 0.025 # according to my measurments range_left.max_range = 0.30 # according to my measurments range_center = Range() range_center.header.frame_id = "ir_center" range_center.radiation_type = Range.INFRARED range_center.field_of_view = 0.4 # ~20 deg range_center.min_range = 0.35 # according to my measurments range_center.max_range = 2.0 # according to my measurments range_right = Range() range_right.header.frame_id = "ir_right" range_right.radiation_type = Range.INFRARED range_right.field_of_view = 0.4 # ~20deg range_right.min_range = 0.025 # according to my measurments range_right.max_range = 0.30 # according to my measurments
def talker(): #tcp config s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((TCP_IP, TCP_PORT)) pubMarker = {} # publisher dictionary pubText = {} # publisher dictionary pubUS1 = {} pubUS2 = {} pubUS3 = {} pubUS4 = {} rospy.init_node('RelocNode') #debug topics pubIRdata1=rospy.Publisher('IRdebug/point1', Point) pubIRdata2=rospy.Publisher('IRdebug/point2', Point) pubIRdata3=rospy.Publisher('IRdebug/point3', Point) pubIRdata4=rospy.Publisher('IRdebug/point4', Point) pubUSdata=rospy.Publisher('USdebug', Quaternion) #The published topics pubText=rospy.Publisher('relocNode/relocVizDataText', Marker) pubMarkerRange=rospy.Publisher('relocNode/relocVizDataRange', Marker) pubMarkerBear=rospy.Publisher('relocNode/relocVizDataBear', Marker) pubMarkerBear2=rospy.Publisher('relocNode/relocVizDataBear2', Marker) pubMarkerBear3=rospy.Publisher('relocNode/relocVizDataBear3', Marker) pubUSAoa = rospy.Publisher('relocNode/relocUsAoa', Range) pubUSAoa2 = rospy.Publisher('relocNode/relocUsAoa2', Range) #Us range sensors pubUS1=rospy.Publisher('relocNode/relocUS1', Range) pubUS2=rospy.Publisher('relocNode/relocUS2', Range) pubUS3=rospy.Publisher('relocNode/relocUS3', Range) pubUS4=rospy.Publisher('relocNode/relocUS4', Range) #parameter for tf prefix if rospy.has_param("~tf_prefix"): tf_prefix = rospy.get_param("~tf_prefix") else: tf_prefix= "" br = tf.TransformBroadcaster() robId="??" IRx1=0 IRy1=0 IRx2=0 IRy2=0 IRx3=0 IRy3=0 IRx4=0 IRy4=0 IRang=0 US1=0 US2=0 US3=0 US4=0 time_pre=0 # initialy flush the buffer response = s.recv(BUFFER_SIZE) while not rospy.is_shutdown(): response = s.recv(BUFFER_SIZE) # check if its a new source address and initialize publishers Rfdata=response #print response str2=Rfdata.split(',') #packet data extraction range_flag =0 bearing_flag =0 packetValid =0 new_packet =0 robId="??" try: for i in range(len(str2)): if str2[i][0:2] == "RL" or str2[i][0:2] == "rl": robID = str2[i][2:] localID = str2[i][2] targetID = str2[i][4] #print localID,targetID #transmit the previous set if str2[i] == "IR" or str2[i] == "ir": IRx1=int(str2[i+1]) IRy1=int(str2[i+2]) IRx2=int(str2[i+3]) IRy2=int(str2[i+4]) IRx3=int(str2[i+5]) IRy3=int(str2[i+6]) IRx4=int(str2[i+7]) IRy4=int(str2[i+8]) bearing_flag =1 packetValid=1 if str2[i] == "ANG" or str2[i] == "ang": #print str2[i+1] IRang=int(str2[i+1])-90 #print IRang/180.0*math.pi br.sendTransform((0.085, 0, -0.035),tf.transformations.quaternion_from_euler(0, 0, -IRang/180.0*math.pi),rospy.Time.now(),tf_prefix+"/IRcam",tf_prefix+"/base_reloc2") br.sendTransform((0.085, 0, -0.035),tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),tf_prefix+"/IRcamBase",tf_prefix+"/base_reloc2") #br.sendTransform((0, 0, 0.3),tf.transformations.quaternion_from_euler(0, 0, -IRang/200.0*2*math.pi),rospy.Time.now(),"/pioneer1/IRcam","/pioneer1/base_reloc1") if str2[i] == "US" or str2[i] == "us": US1=float(str2[i+1]) US2=float(str2[i+2]) US3=float(str2[i+3]) US4=float(str2[i+4]) range_flag =1 packetValid=1 except: print "Bad Packet\n" print str2,robID packetValid=0 #Measurement processing if packetValid==1: #print targetID,US1,US2,US3,US4,IRx1,IRang slog = "No Data" USmin=0 if range_flag==1 and bearing_flag==0: IRx=0 IRy=0 USmin=min(US1,US2,US3,US4)/1000.0 fc =1380 slog = "Range only" if USmin==10725: slog = "No Data" USmin=0 if range_flag==0 and bearing_flag==1: IRx=IRx1-1024/2 IRy=-IRy1+768/2 USmin=10 fc =1380 slog = "Bearing only" if range_flag==1 and bearing_flag==1: IRx=IRx1-1024/2 IRy=-IRy1+768/2 USmin=min(US1,US2,US3,US4)/1000.0 fc =1380 slog = "Range Bearing" #print slog #print USmin #print IRx1 if IRx1==1023 or bearing_flag==0: robx=0 roby=0 robz=0 else: rpx=math.sqrt(pow(IRx,2)+pow(IRy,2)+pow(fc,2)) robx=fc/rpx*USmin roby=IRx/rpx*USmin robz=IRy/rpx*USmin if USmin==0 or USmin>9: USmin=0 #Data publish text_marker = Marker() text_marker.header.frame_id=tf_prefix+"/base_reloc2" text_marker.type = Marker.TEXT_VIEW_FACING text_marker.text = "%s%s" % (localID,targetID) text_marker.pose.position.x= 0 text_marker.pose.position.y= 0 text_marker.pose.position.z= 0 text_marker.scale.z = 0.1 text_marker.color.r = 0.0 text_marker.color.g = 0.5 text_marker.color.b = 0.5 text_marker.color.a = 1 if slog == "Range only" or "Range Bearing": arrow_marker = Marker() arrow_marker.header.frame_id=tf_prefix+"/base_reloc2" arrow_marker.ns="%s%s" % (localID,targetID) arrow_marker.type = Marker.CYLINDER arrow_marker.scale.x = (USmin+0.05)*2 arrow_marker.scale.y = (USmin+0.05)*2 arrow_marker.scale.z = 0.1 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 0.5 if slog == "Bearing only" or "Range Bearing": arrow_marker2 = Marker() #arrow_marker2.header.frame_id=tf_prefix+"/base_reloc2" arrow_marker2.header.frame_id=tf_prefix+"/IRcam" arrow_marker2.ns="%s%s" % (localID,targetID) arrow_marker2.type = Marker.ARROW arrow_marker2.points = {Point(0,0,0),Point(robx,roby,robz)} arrow_marker2.scale.x = 0.1 arrow_marker2.scale.y = 0.1 arrow_marker2.scale.z = 0.2 arrow_marker2.color.r = 1.0 arrow_marker2.color.g = 0.5 arrow_marker2.color.b = 0.5 arrow_marker2.color.a = 0.5 arrow_marker3 = Marker() #arrow_marker2.header.frame_id=tf_prefix+"/base_reloc2" arrow_marker3.header.frame_id=tf_prefix+"/IRcamBase" arrow_marker3.ns="%s%s" % (localID,targetID) arrow_marker3.type = Marker.ARROW arrow_marker3.points = {Point(0,0,0),Point(robx,roby,robz)} arrow_marker3.scale.x = 0.1 arrow_marker3.scale.y = 0.1 arrow_marker3.scale.z = 0.2 arrow_marker3.color.r = 1.0 arrow_marker3.color.g = 0.5 arrow_marker3.color.b = 0.5 arrow_marker3.color.a = 0.5 arrow_marker4 = Marker() arrow_marker4.header.frame_id=tf_prefix+"/IRcamBase" arrow_marker4.ns="%s%s" % (localID,targetID) arrow_marker4.type = Marker.ARROW arrow_marker4.points = {Point(0,0,0),Point(robx,-roby,robz)} arrow_marker4.scale.x = 0.1 arrow_marker4.scale.y = 0.1 arrow_marker4.scale.z = 0.2 arrow_marker4.color.r = 1.0 arrow_marker4.color.g = 0.5 arrow_marker4.color.b = 0.5 arrow_marker4.color.a = 0.5 IRdebug1 = Point() IRdebug1.x = IRx1 IRdebug1.y = IRy1 IRdebug2 = Point() IRdebug2.x = IRx2 IRdebug2.y = IRy2 IRdebug3 = Point() IRdebug3.x = IRx3 IRdebug3.y = IRy3 IRdebug4 = Point() IRdebug4.x = IRx4 IRdebug4.y = IRy4 if slog == "Range only" or "Range Bearing": USang1=0.0 USarr=[US1/1000.0,US2/1000.0,US3/1000.0,US4/1000.0] #print USarr USIds=[0,1,2,3] USmin1=min(USarr) USminId=USarr.index(min(USarr)) USIds.remove(USminId) del USarr[USminId] if USminId==0: USang1=90.0 if USminId==1: USang1=-90.0 if USminId==2: USang1=0.0 if USminId==3: USang1=180.0 USminId=USarr.index(min(USarr)) USmin2=min(USarr) if USminId==0: USang2=90.0 if USminId==1: USang2=-90.0 if USminId==2: USang2=0.0 if USminId==3: USang2=180.0 corr=(USmin2-USmin1)*45/0.1 if corr>45:corr=45 if corr<-45:corr=-45 #print USmin2-USmin1 USang =(USang1+USang2)/2-corr #print (USang1+USang2)/2-corr #print USang1 #print USang2 #print "------------" #angular coorection br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, USang1*math.pi/180.0),rospy.Time.now(),tf_prefix+"/base_reloc2/USAoa",tf_prefix+"/base_reloc2") USAoa = Range() USAoa.header.frame_id=tf_prefix+"/base_reloc2/USAoa" USAoa.radiation_type=0 USAoa.field_of_view =math.pi/4.0 USAoa.min_range = 0.5 USAoa.max_range = 10.0 USAoa.range = USmin br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, -USang1*math.pi/180.0),rospy.Time.now(),tf_prefix+"/base_reloc2/USAoa2",tf_prefix+"/base_reloc2") USAoa2 = Range() USAoa2.header.frame_id=tf_prefix+"/base_reloc2/USAoa2" USAoa2.radiation_type=0 USAoa2.field_of_view =math.pi/4.0 USAoa2.min_range = 0.5 USAoa2.max_range = 10.0 USAoa2.range = USmin USdebug = Quaternion() USdebug.x = US1 USdebug.y = US2 USdebug.z = US3 USdebug.w = US4 USrange1 = Range() USrange1.header.frame_id=tf_prefix+"/base_reloc2/US1" USrange1.radiation_type=0 USrange1.field_of_view =math.pi/2.0 USrange1.min_range = 0.5 USrange1.max_range = 10.0 USrange1.range = US3/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),tf_prefix+"/base_reloc2/US1",tf_prefix+"/base_reloc2") USrange2 = Range() USrange2.header.frame_id=tf_prefix+"/base_reloc2/US2" USrange2.radiation_type=0 USrange2.field_of_view =math.pi/2.0 USrange2.min_range = 0.5 USrange2.max_range = 10.0 USrange2.range = US2/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, math.pi/2.0),rospy.Time.now(),tf_prefix+"/base_reloc2/US2",tf_prefix+"/base_reloc2/US1") USrange3 = Range() USrange3.header.frame_id=tf_prefix+"/base_reloc2/US3" USrange3.radiation_type=0 USrange3.field_of_view =math.pi/2.0 USrange3.min_range = 0.5 USrange3.max_range = 10.0 USrange3.range = US4/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, math.pi),rospy.Time.now(),tf_prefix+"/base_reloc2/US3",tf_prefix+"/base_reloc2/US1") USrange4 = Range() USrange4.header.frame_id=tf_prefix+"/base_reloc2/US4" USrange4.radiation_type=0 USrange4.field_of_view =math.pi/2.0 USrange4.min_range = 0.5 USrange4.max_range = 10.0 USrange4.range = US1/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, 3.0*math.pi/2.0),rospy.Time.now(),tf_prefix+"/base_reloc2/US4",tf_prefix+"/base_reloc2/US1") #print USdebug #print arrow_marker pubIRdata1.publish(IRdebug1) pubIRdata2.publish(IRdebug2) pubIRdata3.publish(IRdebug3) pubIRdata4.publish(IRdebug4) pubUSdata.publish(USdebug) pubText.publish(text_marker) pubMarkerRange.publish(arrow_marker) pubMarkerBear.publish(arrow_marker2) pubMarkerBear2.publish(arrow_marker3) pubMarkerBear3.publish(arrow_marker4) pubUS1.publish(USrange1) pubUS2.publish(USrange2) pubUS3.publish(USrange3) pubUS4.publish(USrange4) pubUSAoa.publish(USAoa) pubUSAoa2.publish(USAoa2)
#!/usr/bin/python import rospy from sensor_msgs.msg import Range import Adafruit_BBIO.ADC as ADC import math ADC.setup() if __name__ == '__main__': try: rospy.init_node('irnode') global pub IR = Range() IR.header.frame_id="inertal_link" IR.radiation_type = 1 IR.field_of_view = 3.14/5.0 IR.min_range = 0.20 IR.max_range = 1.5 IR.range = None pub = rospy.Publisher('/range',Range, queue_size=10) while not rospy.is_shutdown(): voltage = 0.0 voltage = ADC.read("P9_39") value = voltage * 5.0 distance = 59.23 * math.pow(value,-1.1597) distance = distance / 100.0 print( distance) IR.range = distance rospy.loginfo(IR) pub.publish(IR) rospy.spin() except rospy.ROSInterruptException:
def talker(): #XBee config ser = serial.Serial("/dev/ttyUSB0", 9600) ser.write('\n') ser.write('b\n') ser.close() ser = serial.Serial("/dev/ttyUSB0", 115200) sourceaddrlist = {} # node set dictionary pubMarker = {} # publisher dictionary pubText = {} # publisher dictionary pubUS1 = {} pubUS2 = {} pubUS3 = {} pubUS4 = {} xbradio1 = xbee.ZigBee(ser) # the DAQ radio rospy.init_node('RelocNode') pubIRdata1=rospy.Publisher('/IRdebug/point1', Point) pubIRdata2=rospy.Publisher('/IRdebug/point2', Point) pubIRdata3=rospy.Publisher('/IRdebug/point3', Point) pubIRdata4=rospy.Publisher('/IRdebug/point4', Point) pubUSdata=rospy.Publisher('/USdebug', Quaternion) br = tf.TransformBroadcaster() robId="??" IRx1=0 IRy1=0 IRx2=0 IRy2=0 IRx3=0 IRy3=0 IRx4=0 IRy4=0 IRang=0 US1=0 US2=0 US3=0 US4=0 time_pre=0 while not rospy.is_shutdown(): response = xbradio1.wait_read_frame() #response is a python dictionary #sorce address name=binascii.hexlify(response['source_addr_long']) sourceaddr=name.decode("utf-8") # check if its a new source address and initialize publishers nodeseen = 0 for j in range(len(sourceaddrlist)): if sourceaddrlist[j+1]==sourceaddr: nodeseen = 1 Rfdata=response['rf_data'].decode("utf-8") str2=Rfdata.split(',') #packet data extraction range_flag =0 bearing_flag =0 packetValid=0 try: for i in range(len(str2)): if str2[i][0:2] == "RL": robID = str2[i][2:] if str2[i] == "IR": IRx1=int(str2[i+1]) IRy1=int(str2[i+2]) IRx2=int(str2[i+3]) IRy2=int(str2[i+4]) IRx3=int(str2[i+5]) IRy3=int(str2[i+6]) IRx4=int(str2[i+7]) IRy4=int(str2[i+8]) bearing_flag =1 if str2[i] == "ANG": IRang=int(str2[i+1][0:len(str2[i+1])-2])-90 #print IRang/180.0*math.pi br.sendTransform((0, 0, 0.3),tf.transformations.quaternion_from_euler(0, 0, -IRang/180.0*math.pi),rospy.Time.now(),"/pioneer1/IRcam","/pioneer1/base_reloc1") #br.sendTransform((0, 0, 0.3),tf.transformations.quaternion_from_euler(0, 0, -IRang/200.0*2*math.pi),rospy.Time.now(),"/pioneer1/IRcam","/pioneer1/base_reloc1") if str2[i] == "US": US1=float(str2[i+1]) US2=float(str2[i+2]) US3=float(str2[i+3]) US4=float(str2[i+4]) range_flag =1 packetValid=1 except: #print "Bad Packet\n" packetValid=0 #Measurement processing if packetValid==1: slog = "No Data" USmin=0 if range_flag==1 and bearing_flag==0: IRx=0 IRy=0 USmin=min(US1,US2,US3,US4)/1000.0 fc =1380 slog = "Range only" if USmin==10725: slog = "No Data" USmin=0 if range_flag==0 and bearing_flag==1: IRx=IRx1-1024/2 IRy=-IRy1+768/2 USmin=10 fc =1380 slog = "Bearing only" if range_flag==1 and bearing_flag==1: IRx=IRx1-1024/2 IRy=-IRy1+768/2 USmin=min(US1,US2,US3,US4)/1000.0 fc =1380 slog = "Range Bearing" #print USmin #print IRx1 if IRx1==1023: robx=0 roby=0 robz=0 else: rpx=math.sqrt(pow(IRx,2)+pow(IRy,2)+pow(fc,2)) robx=fc/rpx*USmin roby=IRx/rpx*USmin robz=IRy/rpx*USmin if USmin==0 or USmin>9: USmin=0.1 #Data publish text_marker = Marker() text_marker.header.frame_id="/pioneer1/base_link" text_marker.type = Marker.TEXT_VIEW_FACING text_marker.text = "RobID : %s\nx : %f\ny : %f\nz : %f\ntheta :\n %s" % (robID,robx,roby,robz,slog) text_marker.pose.position.x= robx text_marker.pose.position.y= roby text_marker.pose.position.z= robz text_marker.scale.z = 0.1 text_marker.color.r = 0.0 text_marker.color.g = 0.5 text_marker.color.b = 0.5 text_marker.color.a = 1 if slog == "Range only": arrow_marker = Marker() arrow_marker.header.frame_id="/pioneer1/base_reloc1" arrow_marker.type = Marker.CYLINDER arrow_marker.scale.x = (USmin+0.05)*2 arrow_marker.scale.y = (USmin+0.05)*2 arrow_marker.scale.z = 0.1 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 0.5 else : arrow_marker = Marker() arrow_marker.header.frame_id="/pioneer1/base_reloc1" #arrow_marker.header.frame_id="/pioneer1/IRcam" arrow_marker.type = Marker.ARROW arrow_marker.points = {Point(0,0,0),Point(robx,roby,robz)} arrow_marker.scale.x = 0.1 arrow_marker.scale.y = 0.1 arrow_marker.scale.z = 0.2 arrow_marker.color.r = 1.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 0.5 IRdebug1 = Point() IRdebug1.x = IRx1 IRdebug1.y = IRy1 IRdebug2 = Point() IRdebug2.x = IRx2 IRdebug2.y = IRy2 IRdebug3 = Point() IRdebug3.x = IRx3 IRdebug3.y = IRy3 IRdebug4 = Point() IRdebug4.x = IRx4 IRdebug4.y = IRy4 USdebug = Quaternion() USdebug.x = US1 USdebug.y = US2 USdebug.z = US3 USdebug.w = US4 USrange1 = Range() USrange1.header.frame_id="/US1" USrange1.radiation_type=0 USrange1.field_of_view =math.pi/2.0 USrange1.min_range = 0.5 USrange1.max_range = 10.0 USrange1.range = US3/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/US1","/pioneer2/base_reloc2") USrange2 = Range() USrange2.header.frame_id="/US2" USrange2.radiation_type=0 USrange2.field_of_view =math.pi/2.0 USrange2.min_range = 0.5 USrange2.max_range = 10.0 USrange2.range = US2/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, math.pi/2.0),rospy.Time.now(),"/US2","/US1") USrange3 = Range() USrange3.header.frame_id="/US3" USrange3.radiation_type=0 USrange3.field_of_view =math.pi/2.0 USrange3.min_range = 0.5 USrange3.max_range = 10.0 USrange3.range = US4/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, math.pi),rospy.Time.now(),"/US3","/US1") USrange4 = Range() USrange4.header.frame_id="/US4" USrange4.radiation_type=0 USrange4.field_of_view =math.pi/2.0 USrange4.min_range = 0.5 USrange4.max_range = 10.0 USrange4.range = US1/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, 3.0*math.pi/2.0),rospy.Time.now(),"/US4","/US1") pubIRdata1.publish(IRdebug1) pubIRdata2.publish(IRdebug2) pubIRdata3.publish(IRdebug3) pubIRdata4.publish(IRdebug4) pubUSdata.publish(USdebug) pubText[j+1].publish(text_marker) pubMarker[j+1].publish(arrow_marker) pubUS1[j+1].publish(USrange1) pubUS2[j+1].publish(USrange2) pubUS3[j+1].publish(USrange3) pubUS4[j+1].publish(USrange4) if str2[0][0:5] == "RL001": #time_now=time.time() print str2 #time_pre=time_now #print str2 if nodeseen == 0: # New nodes handling sourceaddrlist[len(sourceaddrlist)+1]=sourceaddr pubText[len(pubText)+1]=rospy.Publisher('/relocNode'+str(len(pubText)+1)+'/relocVizDataText', Marker) pubMarker[len(pubMarker)+1]=rospy.Publisher('/relocNode'+str(len(pubMarker)+1)+'/relocVizData', Marker) pubUS1[len(pubMarker)]=rospy.Publisher('/relocNode'+str(len(pubMarker))+'/relocUS1', Range) pubUS2[len(pubMarker)]=rospy.Publisher('/relocNode'+str(len(pubMarker))+'/relocUS2', Range) pubUS3[len(pubMarker)]=rospy.Publisher('/relocNode'+str(len(pubMarker))+'/relocUS3', Range) pubUS4[len(pubMarker)]=rospy.Publisher('/relocNode'+str(len(pubMarker))+'/relocUS4', Range) print "New node registered:"+ sourceaddr #publish data of new node Rfdata=response['rf_data'].decode("utf-8") #print(sourceaddr+' :'+Rfdata) #Publish all topics rospy.sleep(0.01)
try: vals = [float(tok) for tok in l.strip().split()] except Exception as e: rospy.logerr('Failed to parse line from arduino board: %s' % l.strip()) rospy.logerr(e) continue if len(vals) != 4: rospy.logerr('Line from arduino board has wrong number of tokens: %s' % l.strip()) continue # Sharp 2D120X (4-30 cm range) ir1_range = Range() ir1_range.radiation_type = Range.INFRARED ir1_range.min_range = 0.04 ir1_range.max_range = 0.30 ir1_range.range = ir1_value_to_range(vals[0]) # Sharp 2Y0A21 (10-80 cm range) ir2_range = Range() ir2_range.radiation_type = Range.INFRARED ir2_range.min_range = 0.10 ir2_range.max_range = 0.80 ir2_range.range = ir2_value_to_range(vals[1]) # HRLV-MaxSonar-EZ MB1043 reports anything below 30 cm as 30 cm # inaccurate readings closer than 50 cm sonar1_range = Range() sonar1_range.radiation_type = Range.ULTRASOUND sonar1_range.min_range = 0