def _hw_ultrasonics_cb(self, msg): leftMsg = Range() leftMsg.header.stamp = rospy.Time.now() leftMsg.header.frame_id = "left_ultrasonic_link" leftMsg.radiation_type = Range.ULTRASOUND leftMsg.field_of_view = msg.field_of_view leftMsg.min_range = msg.min_range leftMsg.max_range = msg.max_range leftMsg.range = msg.left_range self._left_ultra_pub.publish(leftMsg) centerMsg = Range() centerMsg.header.stamp = rospy.Time.now() centerMsg.header.frame_id = "center_ultrasonic_link" centerMsg.radiation_type = Range.ULTRASOUND centerMsg.field_of_view = msg.field_of_view centerMsg.min_range = msg.min_range centerMsg.max_range = msg.max_range centerMsg.range = msg.center_range self._center_ultra_pub.publish(centerMsg) rightMsg = Range() rightMsg.header.stamp = rospy.Time.now() rightMsg.header.frame_id = "right_ultrasonic_link" rightMsg.radiation_type = Range.ULTRASOUND rightMsg.field_of_view = msg.field_of_view rightMsg.min_range = msg.min_range rightMsg.max_range = msg.max_range rightMsg.range = msg.right_range self._right_ultra_pub.publish(rightMsg)
def lazer_msg_constructor(self, topic_name, lazer_frame, distance, angle): r = Range() r.header.stamp = rospy.Time.now() r.header.frame_id = lazer_frame r.radiation_type = 0 r.field_of_view = 0.8 r.min_range = 0 r.max_range = 10000 r.radiation_type = 1 r.range = distance scan_pub = rospy.Publisher(topic_name, Range, queue_size=10) scan_pub.publish(r) time.sleep(0.05)
def main(): GPG = gopigo3.GoPiGo3() rospy.init_node("ultra_sonic") port_id = rospy.get_param("~port", 1) if port_id == 1: port = GPG.GROVE_1 elif port_id == 2: port = GPG.GROVE_2 else: rospy.logerr("unknown port %i", port_id) return GPG.set_grove_type(port, GPG.GROVE_TYPE.US) pub_distance = rospy.Publisher("~distance", Range, queue_size=10) msg_range = Range() msg_range.header.frame_id = "ultra_sonic" msg_range.radiation_type = Range.ULTRASOUND msg_range.min_range = 0.02 msg_range.max_range = 4.3 rate = rospy.Rate(rospy.get_param('~hz', 30)) while not rospy.is_shutdown(): # read distance in meter msg_range.range = GPG.get_grove_value(port) / 1000.0 msg_range.header.stamp = rospy.Time.now() pub_distance.publish(msg_range) rate.sleep()
def __init__( self, num_sensor=5, #Number of active infrared sensors range_min=1.0, #Minimum valid range of sensor [m] range_max=4.0 #Maximum valid range of sensor [m] ): super().__init__("ir_data") self.ir_array = [] self.pub_array = [] self.distance = [0, 0, 0, 0, 0] self.num_sensor = num_sensor timer_period = 0.05 self.timer = self.create_timer(timer_period, self.scan) for i in range(num_sensor): ir_num = i + 1 ir = IR() ir.angle = math.radians(5) self.ir_array.append(ir) topic_name = "/agv/ir_%d" % ir_num topic_name = str(topic_name) pub = self.create_publisher(Range, topic_name, 5) self.pub_array.append(pub) #Default message message = Range() message.radiation_type = 0 message.min_range = range_min message.max_range = range_max self.msg = message
def __init__( self, num_sensor=5, #Number of active ultrasonic sensors range_min=0.04, #Minimum valid range of sensor [m] range_max=3.5 #Maximum valid range of sensor [m] ): super().__init__("sonar_data") self.sonar_array = [] self.pub_array = [] self.distance = [0, 0, 0, 0, 0] self.num_sensor = num_sensor timer_period = 0.05 self.timer = self.create_timer(timer_period, self.scan) for i in range(num_sensor): sonar_num = i + 1 sonar = Sonar() sonar.angle = math.radians(15) self.sonar_array.append(sonar) topic_name = "/agv/sonar_%d" % sonar_num topic_name = str(topic_name) pub = self.create_publisher(Range, topic_name, 5) self.pub_array.append(pub) sonar_num = 0 #Default message message = Range() message.radiation_type = 0 message.min_range = range_min message.max_range = range_max self.msg = message
def main(): rospy.init_node('range_faker', anonymous=True) rate = rospy.Rate(1) # 1Hz frames_in = rospy.get_param('~frames_in') topic_out = rospy.get_param('~topic_out') pub = rospy.Publisher(topic_out, Range, queue_size=10) while not rospy.is_shutdown(): t = rospy.Time.now() for frame in frames_in: msg = Range() # defaults msg.radiation_type = Range.INFRARED msg.field_of_view = 1 msg.min_range = 0.1 msg.max_range = 0.8 msg.header.stamp = t # custom msg.header.frame_id = frame msg.range = 0.7 pub.publish(msg) rate.sleep() pub.unregister()
def __init__(self): rospy.init_node('sonar', anonymous=True) topic_name_d = '/sonarTP_D' self.distance_publisher_down = rospy.Publisher(topic_name_d, Range, queue_size=5) self.r = rospy.Rate(40) self.serialPort0 = "/dev/ttyUSB0" self.serialPort1 = "/dev/ttyUSB0" self.serialPort2 = "/dev/ttyUSB0" self.serialPort3 = "/dev/ttyUSB0" self.serialPort4 = "/dev/ttyUSB0" self.maxRange = 2000 # change for 5m vs 10m sensor self.sleepTime = 0.1 self.minMM = 0.3 self.maxMM = 3 self.mm = float r = Range() r.header.stamp = rospy.Time.now() r.header.frame_id = "/sonarD_link" r.radiation_type = 0 r.field_of_view = 0.8 self.min_range = self.minMM self.max_range = self.maxRange r.min_range = self.min_range r.max_range = self.max_range self._range = r
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_laser(self): """ TODO Griffin: Add a transform from base_link to base_laser TODO: Modify the is valid condition if significant slope traversal object detection required. Publish filtered laser distance as Range message. Will publish NaN distance if scanner is blocked or robot in orientation not suited to horizontal navigation. See API for sensor range, fov etc https://developer.anki.com/vector/docs/generated/anki_vector.proximity.html """ if self._laser_pub.get_num_connections() == 0: return now = rospy.Time.now() laser = Range() laser.header.frame_id = self._base_frame laser.header.stamp = now laser.radiation_type = 1 # IR laser laser.field_of_view = 0.436332 # 25 deg laser.min_range = 0.03 # 30mm laser.max_range = 1.5 # 300mm laser_reading = self._vector.proximity.last_sensor_reading if(laser_reading.is_valid): laser.range = self._vector.proximity.last_sensor_reading.distance.distance_mm/1000 else: laser.range = float('nan') self._laser_pub.publish(laser)
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 main(): # instantiate the distance object #sensor = DistanceSensor() my_sensor = EasyDistanceSensor() #sensor.start_continuous() rospy.init_node("distance_sensor") pub_distance = rospy.Publisher("~distance", Range, queue_size=10) msg_range = Range() msg_range.header.frame_id = "distance" msg_range.radiation_type = Range.INFRARED msg_range.min_range = 0.02 msg_range.max_range = 3.0 rate = rospy.Rate(rospy.get_param('~hz', 1)) while not rospy.is_shutdown(): # read distance in meters read_distance = my_sensor.read() / 100.0 msg_range.range = read_distance # Add timestamp msg_range.header.stamp = rospy.Time.now() # Print current distance print msg_range.range * 1000, " mm" # Publish distance message pub_distance.publish(msg_range) rate.sleep()
def rangeCB(self, data): range_msg = Range() range_msg.header.stamp = rospy.Time.now() range_msg.radiation_type = Range.ULTRASOUND range_msg.range = float(data[1]) range_msg.field_of_view = float(data[0]) self.range_pub.publish(range_msg)
def distance_thread(): global b # ROS topic publisher for sonar distance readings pub = rospy.Publisher('nxt/sonar', Range, queue_size=10) # Sonar driver ultrasonic = Ultrasonic(b, PORT_4) # Setup the ROS range message with sensor properties range_msg = Range() range_msg.radiation_type = Range.ULTRASOUND range_msg.field_of_view = 10.0 * pi / 180.0 range_msg.min_range = 0.01 range_msg.max_range = 2.55 # Rate limiter for process loop rate = rospy.Rate(4) # Main process loop while not rospy.is_shutdown(): # Perform and read US range measurement try: d = ultrasonic.get_sample() except: rospy.logfatal("Connection with NXT robot interrupted!") rospy.signal_shutdown("Connexion error") # Fill and publish the ROS range message range_msg.header.stamp = rospy.Time.now() range_msg.range = 0.01 * d pub.publish(range_msg) # Wait until next loop rate.sleep()
def callback(self, msg): ranges = msg.ranges terarangers_msg = Range() terarangers_msg.header.frame_id = "base_range" terarangers_msg.radiation_type = 1 terarangers_msg.field_of_view = 0.0593 terarangers_msg.min_range = 200 terarangers_msg.max_range = 14000 # 14 metres v0 = ranges[45] * 1000 #45 degree and convert m to mm v1 = ranges[135] * 1000 #135 degree and convert m to mm v2 = ranges[225] * 1000 #225 degree and convert m to mm v3 = ranges[315] * 1000 #315 degree and convert m to mm terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.range = v0 self.trone0_pub.publish(terarangers_msg) terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.range = v1 self.trone1_pub.publish(terarangers_msg) terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.range = v2 self.trone2_pub.publish(terarangers_msg) terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.range = v3 self.trone3_pub.publish(terarangers_msg)
def main(): range_data = Range() rospy.init_node('srf_08_rangefinder', anonymous=True) device_address = rospy.get_param('~device_addr') sample_rate = rospy.get_param('~sample_rate') range_data.radiation_type = rospy.get_param('~radiation_type') range_data.min_range = rospy.get_param('~min_range') range_data.max_range = rospy.get_param('~max_range') range_data.field_of_view = rospy.get_param('~fov') pub = rospy.Publisher('rangefinder_data', Range, queue_size=1) rate = rospy.Rate(sample_rate) rangefinder = srf_08(device_address) while not rospy.is_shutdown(): range_data.header.stamp = rospy.Time.now() range_data.range = (rangefinder.distance() / 100) pub.publish(range_data) rate.sleep()
def distance_callback(self, stamp): distance_from_center = 0.035 for key in self.sensors: msg = Range() msg.field_of_view = self.sensors[key].getAperture() msg.min_range = intensity_to_distance( self.sensors[key].getMaxValue() - 8.2) + distance_from_center msg.max_range = intensity_to_distance( self.sensors[key].getMinValue() + 3.3) + distance_from_center msg.range = intensity_to_distance(self.sensors[key].getValue()) msg.radiation_type = Range.INFRARED self.sensor_publishers[key].publish(msg) # Max range of ToF sensor is 2m so we put it as maximum laser range. # Therefore, for all invalid ranges we put 0 so it get deleted by rviz msg = LaserScan() msg.header.frame_id = 'laser_scanner' msg.header.stamp = stamp msg.angle_min = 0.0 msg.angle_max = 2 * pi msg.angle_increment = 15 * pi / 180.0 msg.scan_time = self.timestep.value / 1000 msg.range_min = intensity_to_distance( self.sensors['ps0'].getMaxValue() - 20) + distance_from_center msg.range_max = 1.0 + distance_from_center msg.ranges = [ self.sensors['tof'].getValue(), # 0 intensity_to_distance(self.sensors['ps7'].getValue()), # 15 0.0, # 30 intensity_to_distance(self.sensors['ps6'].getValue()), # 45 0.0, # 60 0.0, # 75 intensity_to_distance(self.sensors['ps5'].getValue()), # 90 0.0, # 105 0.0, # 120 0.0, # 135 intensity_to_distance(self.sensors['ps4'].getValue()), # 150 0.0, # 165 0.0, # 180 0.0, # 195 intensity_to_distance(self.sensors['ps3'].getValue()), # 210 0.0, # 225 0.0, # 240 0.0, # 255 intensity_to_distance(self.sensors['ps2'].getValue()), # 270 0.0, # 285 0.0, # 300 intensity_to_distance(self.sensors['ps1'].getValue()), # 315 0.0, # 330 intensity_to_distance(self.sensors['ps0'].getValue()), # 345 self.sensors['tof'].getValue(), # 0 ] for i in range(len(msg.ranges)): if msg.ranges[i] != 0: msg.ranges[i] += distance_from_center self.laser_publisher.publish(msg)
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 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 pub_depth(self): msg = Range() msg.radiation_type = 0 msg.min_range = 0.5 msg.max_range = 30.0 msg.range = random.uniform(1.0, 5.0) msg.field_of_view = 100.0 msg.header.stamp = self.get_clock().now().to_msg() self.p_depth.publish(msg)
def init(): global msg, device_range, range_pub msg = Range() msg.radiation_type = 2 msg.field_of_view = 3.1415 msg.min_range = 0.2 msg.max_range = 10.0 # depends on the configuration, 300m on datasheet, 10m reported by users device_range = pypozyx.DeviceRange() rospy.init_node('pozyx_pose_node')
def parse_sensor_data(line): global scan_pub, range_pub # Handle debug text from the arduino if "," not in line: rospy.loginfo("RangeNode: %s" % line) return # Parse the range string into a float array ranges = [float(x) / 1000.0 for x in line.split(",")[::-1]] if len(ranges) != 8: rospy.logwarn("Received other than 8 scan ranges", ranges) return br = tf2_ros.TransformBroadcaster() # msg = LaserScan() # msg.header.frame_id = "base_link" # msg.header.stamp = rospy.Time.now() # msg.angle_increment = 26.0/180.0*3.141592 # msg.angle_min = msg.angle_increment*-3.5 # msg.angle_max = msg.angle_increment*3.5 # msg.range_min = 0.1 # msg.range_max = 4.0 # msg.ranges = ranges # reverse! # scan_pub.publish(msg) for i in range(0, 8): # Emit the range data for this range rmsg = Range() rmsg.header.frame_id = "base_link" rmsg.header.stamp = rospy.Time.now() rmsg.header.frame_id = "rangefinder_%d" % i rmsg.min_range = 0.1 rmsg.max_range = 4.0 rmsg.field_of_view = 26.0 / 180.0 * 3.141592 rmsg.radiation_type = rmsg.INFRARED rmsg.range = ranges[i] range_pub[i].publish(rmsg) # output the TF2 for this range t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "base_footprint" t.child_frame_id = rmsg.header.frame_id t.transform.translation.x = range_pos[i][0] t.transform.translation.y = range_pos[i][1] - 0.2 t.transform.translation.z = 0.2 q = tf_conversions.transformations.quaternion_from_euler( 0, -range_pos[i][3] / 180.0 * 3.1415, range_pos[i][2] / 180.0 * 3.1415 - 3.1415 / 2) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] br.sendTransform(t)
def define_cone(frame_id): cone = Range() cone.header.frame_id = frame_id cone.field_of_view = pi * friction_angle*2 / 180 cone.radiation_type = 1 cone.min_range = 0.05 cone.max_range = 0.1 cone.range = 0.05 return cone
def pub_laser(self, dist): laser_msg = Range() laser_msg.header.stamp = rospy.Time.now() laser_msg.range = dist laser_msg.min_range = 0.5 laser_msg.max_range = 12. laser_msg.field_of_view = 0.0349 laser_msg.radiation_type = 1 self._laser_pub.publish(laser_msg)
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 start(self): self.enable = True self.ultrasonicPub = rospy.Publisher(self.robot_host + '/ultrasonic/front/distance', Range, queue_size=10) self.ultrasonicVelocityPub = rospy.Publisher(self.robot_host + '/ultrasonic/rear/relative_velocity', RelativeVelocity, queue_size=10) ultrasonic = UltrasonicParallax(27) #ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')] min_range = 3 max_range = 300 while not rospy.is_shutdown(): distance = ultrasonic.distance() relative_velocity = ultrasonic.speed() # status = ultrasonic.less_than(threshold) # if distance != -1: # print('distance', distance, 'cm') # time.sleep(0.2) # else: # print(False) # if status == 1: # print("Less than %d" % threshold) # elif status == 0: # print("Over %d" % threshold) # else: # print("Read distance error.") message_str = "frontUltrasonic 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.34906585039887 # 20 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.34906585039887 # 20 degrees rv.relative_velocity = relative_velocity self.ultrasonicPub.publish(r) self.ultrasonicVelocityPub.publish(rv) self.rate.sleep()
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 publish_range(index): range = Range() range.header.frame_id = 'world' range.header.stamp = rospy.get_rostime() range.radiation_type = 0 range.field_of_view = (math.pi / 3) range.min_range = 0.1 range.max_range = 2 range.range = 1.5 sensor_publisher[index].publish(range)
def CreateRangeMessage(rng, img_time, sequence): msg_range = Range() msg_range.header.frame_id = "world" msg_range.header.seq = sequence msg_range.header.stamp = img_time msg_range.radiation_type = 1 msg_range.field_of_view = 0 msg_range.min_range = 0 msg_range.max_range = 1000000 msg_range.range = rng return msg_range
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 _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 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 __init__(self): rospy.init_node('sonar', anonymous=True) topic_name_d = '/sonarTP_D' self.distance_publisher_down = rospy.Publisher(topic_name_d, Range, queue_size=5) #self.r = rospy.Rate(2) topic_name_f = '/sonarTP_F' self.distance_publisher_front = rospy.Publisher(topic_name_f, Range, queue_size=5) #self.r = rospy.Rate(2) topic_name_r = '/sonarTP_R' self.distance_publisher_right = rospy.Publisher(topic_name_r, Range, queue_size=5) #self.r = rospy.Rate(2) topic_name_b = '/sonarTP_B' self.distance_publisher_back = rospy.Publisher(topic_name_b, Range, queue_size=5) #self.r = rospy.Rate(2) topic_name_l = '/sonarTP_L' self.distance_publisher_left = rospy.Publisher(topic_name_l, Range, queue_size=5) self.r = rospy.Rate(2) self.serialPort0 = "/dev/ttyUSB0" self.serialPort1 = "/dev/ttyUSB1" self.serialPort2 = "/dev/ttyUSB2" self.serialPort3 = "/dev/ttyUSB3" self.serialPort4 = "/dev/ttyUSB4" self.maxRange = 2000 # change for 5m vs 10m sensor self.sleepTime = 0.5 self.minMM = 0.3 self.maxMM = 3 self.mm = float self.frames = [ '/sonarD_link', '/sonarF_link', '/sonarL_link', '/sonarB_link', '/sonarR_link' ] r = Range() r.header.stamp = rospy.Time.now() #r.header.frame_id = "/sonarD_link" r.radiation_type = 0 r.field_of_view = 0.8 self.min_range = self.minMM self.max_range = self.maxRange r.min_range = self.min_range r.max_range = self.max_range self._range = r
def publish_ground_sensor_data(self, stamp): for idx in self.ground_sensors.keys(): msg = Range() msg.header.stamp = stamp msg.header.frame_id = idx msg.field_of_view = self.ground_sensors[idx].getAperture() msg.min_range = GROUND_MIN_RANGE msg.max_range = GROUND_MAX_RANGE msg.range = interpolate_table( self.ground_sensors[idx].getValue(), GROUND_TABLE) msg.radiation_type = Range.INFRARED self.ground_sensor_publishers[idx].publish(msg)
def composeRangeMessage(distance): sensor_range = Range() # compose the message 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 return sensor_range
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 publish_ultrasonic_messages(data_publishers, measured_distances): range_msg = Range() range_msg.radiation_type = 0 # ULTRASOUND range_msg.field_of_view = math.pi / 6.0 # 30 degrees range_msg.min_range = 0.01 range_msg.max_range = 2.5 for index, measured_distance in enumerate(measured_distances): range_msg.range = measured_distance range_msg.header.frame_id = range_sensors_frame_ids[index] range_msg.header.stamp = rospy.Time.now() data_publishers[index].publish(range_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'): """ 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 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 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 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 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 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 node(): rospy.init_node('sonar_node', anonymous=True) range_msg = Range() range_msg.radiation_type = range_msg.ULTRASOUND pub = rospy.Publisher("/msg_from_sonar", Range, queue_size=10) r = rospy.Rate(20) rospy.loginfo("reading sonar range") while not rospy.is_shutdown(): #trigger sonar to measure cmd = chr(0x22)+chr(0x00)+chr(0x00)+chr(0x22) ser.write(cmd) data = ser.read(4) if len(data)==4 and (ord(data[0])+ord(data[1])+ord(data[2]))&255 == ord(data[3]): sonar_range = ord(data[1])*256+ord(data[2]) range_msg.range = sonar_range pub.publish(range_msg) rospy.loginfo("distance : %d", sonar_range) else: rospy.logwarn("sonar error!") r.sleep()
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 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 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 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
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/env python #vim /etc/puppet/manifests/site.pp import tf import rospy from sensor_msgs.msg import Range if __name__ == '__main__': rospy.init_node('rangePub') pub = rospy.Publisher('/vision_range', Range) r = rospy.Rate(50) print "gonna start publishin'" while not rospy.is_shutdown(): vision_range = Range() vision_range.header.frame_id = "/high_def_frame" # vision_range.header.stamp = rospy.Time.now() vision_range.radiation_type = 0 vision_range.field_of_view = 0.5 vision_range.min_range = 0.0 vision_range.max_range = 100.0 vision_range.range = 1.0 pub.publish(vision_range) # tf_sub = rospy.Subscriber("/tf", tf.msg.tfMessage, tf_callback) rospy.spin()
print l 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
import rospy 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
#!/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:
if sonar_range < min_ranges[sonar_type]: print "lower than min range of", min_ranges[sonar_type] continue if sonar_range > max_ranges[sonar_type]: print "higher than max range of", max_ranges[sonar_type] continue marker_trans = scipy.matrix([[0., 0., 1., sonar_range], [1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]]) marker_mat = sonar_mats[sonar_id] * marker_trans range_msg = Range() range_msg.radiation_type = range_msg.ULTRASOUND range_msg.field_of_view = angle_spreads[sonar_type] range_msg.min_range = min_ranges[sonar_type] range_msg.max_range = max_ranges[sonar_type] range_msg.range = sonar_range range_msg.header.stamp = rospy.Time.now() range_msg.header.frame_id = "sonar" + str(sonar_id) sonar_pub.publish(range_msg) sonar_pos, sonar_quat = mat_to_pos_and_quat(sonar_mats[sonar_id]) tf_broadcaster.sendTransform(sonar_frame_pos, sonar_frame_quat, rospy.Time.now(), "sonar_frame", "base_footprint" ) tf_broadcaster.sendTransform(sonar_pos, sonar_quat, rospy.Time.now(), "sonar" + str(sonar_id), "sonar_frame" ) # draw_funcs.draw_rviz_cylinder(marker_mat, sonar_range * math.tan(angle_spreads[sonar_type]/2.),
def sendRange(pub, radiation_type, r): msg = Range() msg.radiation_type = radiation_type msg.range = r pub.publish(msg)
def run(self): L = Range() R = Range() L.min_range = 0.1 L.max_range = 1.0 L.radiation_type = Range.INFRARED L.field_of_view = 0.4 # ~22degrees L.header.frame_id = 'L' R.min_range = L.min_range R.max_range = L.max_range R.radiation_type = L.radiation_type R.field_of_view = L.field_of_view R.header.frame_id = 'R' C = Range() C.min_range = 0.1 C.max_range = 3.0 C.radiation_type = Range.ULTRASOUND C.field_of_view = 0.4 # ~22degrees C.header.frame_id = 'C' O = Odometry() O.header.frame_id = 'odom' O.child_frame_id = 'base_link' NaN = float('nan') r = rospy.Rate(self.rate) lastSensorCommandSent = None lastTfCommandSent = None while not rospy.is_shutdown(): if lastSensorCommandSent is None or rospy.get_rostime().to_sec()-lastSensorCommandSent > 5: self.serial.addCommand('R L R C O\n') lastSensorCommandSent = rospy.get_rostime().to_sec() if lastTfCommandSent is None or rospy.get_rostime().to_sec()-lastTfCommandSent > 1: self.pub_tf.sendTransform((0.09, 0.00, 0.10), tf.transformations.quaternion_about_axis(0.0, (0, 0, 1)), rospy.get_rostime(), 'C', 'base_link') self.pub_tf.sendTransform((0.07, 0.05, 0.05), tf.transformations.quaternion_about_axis(0.6, (0, 0, 1)), rospy.get_rostime(), 'L', 'base_link') self.pub_tf.sendTransform((0.07, -0.05, 0.05), tf.transformations.quaternion_about_axis(-0.6, (0, 0, 1)), rospy.get_rostime(), 'R', 'base_link') lastTfCommandSent = rospy.get_rostime().to_sec() ''' #code to dynamically enable/disable sensors depending on number of subscribers, disabled for now if lastSensorCommandSent==None or rospy.get_rostime().to_sec()-lastSensorCommandSent>5: cmdOn = 'R ' cmdOff = 'r ' if self.pub_C.get_num_connections()>0: cmdOn+=' C' else: cmdOff+=' C' if self.pub_L.get_num_connections()>0: cmdOn+=' L' else: cmdOff+=' L' if self.pub_R.get_num_connections()>0: cmdOn+=' R' else: cmdOff+=' R' if self.pub_O.get_num_connections()>0: cmdOn+=' O' else: cmdOff+=' O' if len(cmdOff)>2: self.serial.addCommand(cmdOff) if len(cmdOn)>2: self.serial.addCommand(cmdOn) lastSensorCommandSent = rospy.get_rostime().to_sec() ''' data = self.serial.getData() if data is not None: self.pub_serial_out.publish(String(data)) if data.startswith('R '): datas = string.split(data, ' ') for i, reading in enumerate(datas): reading_arr = string.split(reading, ':') if i > 0 and len(reading_arr) == 2: rType = reading_arr[0] rVal = reading_arr[1] if rType == 'L': L.header.stamp = rospy.get_rostime() L.header.seq += 1 L.range = int(rVal)/1000.0 if int(rVal) > 0 else NaN self.pub_L.publish(L) elif rType == 'R': R.header.stamp = rospy.get_rostime() R.header.seq += 1 R.range = int(rVal)/1000.0 if int(rVal) > 0 else NaN self.pub_R.publish(R) pass elif rType == 'C': C.header.stamp = rospy.get_rostime() C.header.seq += 1 C.range = int(rVal)/1000.0 if int(rVal) > 0 else NaN self.pub_C.publish(C) elif rType == 'O': rVal_arr = string.split(rVal, ',') if len(rVal_arr) == 5: O.header.stamp = rospy.get_rostime() O.pose.pose.position.x = int(rVal_arr[0])/1000.0 O.pose.pose.position.y = int(rVal_arr[1])/1000.0 q = tf.transformations.quaternion_about_axis(int(rVal_arr[2])/1000.0, (0, 0, 1)) O.pose.pose.orientation.z = q[2] O.pose.pose.orientation.w = q[3] O.twist.twist.linear.x = int(rVal_arr[3])/1000.0 O.twist.twist.angular.z = int(rVal_arr[4])/1000.0 self.pub_O.publish(O) self.pub_tf.sendTransform( (O.pose.pose.position.x, O.pose.pose.position.y, O.pose.pose.position.z), (O.pose.pose.orientation.x, O.pose.pose.orientation.y, O.pose.pose.orientation.z, O.pose.pose.orientation.w), rospy.get_rostime(), 'base_link', 'odom') r.sleep()
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)
time.sleep(0.2) import rospy from sensor_msgs.msg import Range try: rospy.init_node('sonar') pub = rospy.Publisher('sonar',Range,queue_size=10) ser = serial.Serial('/dev/cu.usbmodem1411',115200) ser.flushInput() t1 = threading.Thread(target=serial_reader) t2 = threading.Thread(target=cmd_reader) t1.start() t2.start() r_msg = Range() r_msg.radiation_type = r_msg.ULTRASOUND r_msg.field_of_view = 0.1 r_msg.min_range = 0.1 r_msg.max_range = 2.0 r_msg.header.frame_id = 'sonar_link' r= rospy.Rate(20) while not rospy.is_shutdown(): yaw = (90-servo_cmd)/180.0*3.14 br = tf.TransformBroadcaster() br.sendTransform((0,0,0), tf.transformations.quaternion_from_euler(0,0,yaw), rospy.Time.now(),"world","sonar_link") r_msg.range = d_cm/100.0 r_msg.header.stamp = rospy.Time.now()
import roslib; roslib.load_manifest('rviz') from sensor_msgs.msg import Range import rospy topic = 'test_range' publisher = rospy.Publisher(topic, Range) rospy.init_node('range_test') dist = 3 while not rospy.is_shutdown(): r = Range() r.header.frame_id = "/moon" r.header.stamp = rospy.Time.now() r.radiation_type = 0 r.field_of_view = 2.0/dist r.min_range = .4 r.max_range = 10 r.range = dist publisher.publish( r ) rospy.sleep(0.1) dist += .3 if dist > 10: dist = 3