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 talker(): top_pub = rospy.Publisher('/IR_top', Range, queue_size=1) left_pub = rospy.Publisher('/IR_left', Range, queue_size=1) right_pub = rospy.Publisher('/IR_right', Range, queue_size=1) rospy.init_node('IR_node', anonymous=True) rate = rospy.Rate(6) # 10hz #top_data top_range = Range() top_range.header.frame_id = '/IR_top' top_range.field_of_view = 0.4 top_range.max_range = 3 top_range.min_range = 0.003 print('TOF start') #left left_range = Range() left_range.header.frame_id = '/IR_left' left_range.field_of_view = 0.4 left_range.max_range = 3 left_range.min_range = 0.005 #right right_range = Range() right_range.header.frame_id = '/IR_right' right_range.field_of_view = 0.4 right_range.max_range = 3 right_range.min_range = 0.003 time.sleep(3) while not rospy.is_shutdown(): receiced_distance = ir_pub() #top top_range.header.stamp = rospy.Time.now() top_range.range = receiced_distance[0] #print('top is %f \n' % top_range.range) top_pub.publish(top_range) #left left_range.header.stamp = rospy.Time.now() left_range.range = receiced_distance[12] # print('left is %f \n' %left_range.range) left_pub.publish(left_range) #right right_range.header.stamp = rospy.Time.now() right_range.range = receiced_distance[4] #print('right is %f \n' % right_range.range) right_pub.publish(right_range) rate.sleep()
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 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 __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 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 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 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 _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 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 timerCallback(self, event): data = self.pinger.get_distance() if data: range_msg = Range() range_msg.header.stamp = rospy.Time.now() range_msg.header.frame_id = 'pinger' range_msg.header.seq = self.count range_msg.field_of_view = self.field_of_view range_msg.min_range = 0.5 range_msg.max_range = 30.0 range_msg.range = float(data["distance"]) / 1000.0 self.range_pub.publish(range_msg) ping_msg = PingRange() ping_msg.header = range_msg.header ping_msg.field_of_view = range_msg.field_of_view ping_msg.min_range = range_msg.min_range ping_msg.max_range = range_msg.max_range ping_msg.range = range_msg.range ping_msg.confidence = data['confidence'] / 100.0 self.ping_pub.publish(ping_msg) else: oar_msg = Bool() oar_msg.data = True self.out_of_range_pub.publish(oar_msg) rospy.loginfo("Failed to get distance data") self.count += 1
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 test_map_received(self): poseMsg = PoseStamped() poseMsg.header = Header() poseMsg.pose.position.x = 0 poseMsg.pose.position.y = 0 poseMsg.pose.position.z = 0 poseMsg.pose.orientation.x = 0 poseMsg.pose.orientation.y = 0 poseMsg.pose.orientation.z = 0 poseMsg.pose.orientation.w = 1 self.pose_pub.publish(poseMsg) rangeMsg = RangeArray() rangeMsg.header = Header() ranges = [0, 0.2, -1, 1, 1.5, 8, 3, 1, 8, 4, 2.3, 0.2, 0.1, 2, 3, 1, 9] for rangerange in ranges: range = Range() range.header = Header() range.range = rangerange range.field_of_view = 1 range.min_range = 0.1 range.max_range = 2.1 rangeMsg.ranges.append(range) rangeMsg.ranges = rangeMsg.ranges[:self.NUM_RANGE_SENSORS] self.range_pub.publish(rangeMsg) rospy.sleep(0.2) self.assertEqual(self.rx.receivedMsg, True) self.assertGreater(len(self.rx.map.points), 0)
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 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 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 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 altitude_pub(self, alt): rng = Range() rng.field_of_view = math.pi * 0.1 rng.max_range = 300 rng.header.frame_id = "sonar_link" rng.header.stamp = rospy.Time.now() rng.range = alt return rng
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 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 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 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 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 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 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 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 talker(): pub = rospy.Publisher('/IR_top', Range, queue_size=20) left_pub = rospy.Publisher('/IR_left', Range, queue_size=20) right_pub = rospy.Publisher('/IR_right', Range, queue_size=20) rospy.init_node('IR_node', anonymous=True) rate = rospy.Rate(10) # 5hz r = Range() r.header.frame_id = '/IR_top' #r.radiation_type = Range.INFRARED r.field_of_view = 0.21 r.max_range = 0.80 r.min_range = 0.10 left = Range() left.header.frame_id = '/IR_left' #left.radiation_type = Range.INFRARED left.field_of_view = 0.21 left.max_range = 0.80 left.min_range = 0.10 right = Range() right.header.frame_id = '/IR_right' #right.radiation_type = Range.INFRARED right.field_of_view = 0.21 right.max_range = 0.80 right.min_range = 0.1 print('IR start') while not rospy.is_shutdown(): r.header.stamp = rospy.Time.now() r.range = top_IR() pub.publish(r) left.header.stamp = rospy.Time.now() left.range = left_IR() left_pub.publish(left) right.header.stamp = rospy.Time.now() right.range = right_IR() right_pub.publish(right) rate.sleep()
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 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 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 pub_range(self): rang_msg = Range() for i in range(self.num_cam): rang_msg.header.stamp = rospy.Time.now() frame_id = "robot"+str(i+1)+"/camera_link" rang_msg.header.frame_id = frame_id rang_msg.field_of_view = self.field_of_view # 0-ULTRASOUND, 1-INFRARED rang_msg.min_range = 0.1 # min_range of sonar, useless here rang_msg.max_range = 201 # max_range of sonar, useless here rang_msg.range = self.radius self.pubs[i].publish(rang_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 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'): """ 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 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 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 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 publish(self): ts = rospy.Time.now() self.tf_broadcaster.sendTransform(self.position, self.orientation, ts, self.frame_id, 'odom') msg = Range() msg.header.stamp = ts msg.header.frame_id = self.frame_id msg.field_of_view = self.fov msg.max_range = self.max_range if self.caching and self.cache: msg.range = self.cache else: try: distance = self.nearest_point().distances[0] if distance > self.max_range: msg.range = self.max_range else: msg.range = distance except rospy.ServiceException: return self.cache = msg.range self.range_pub.publish(msg)
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)
#!/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()
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
#!/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.), # 0.025, frame = '/camera_link',
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() pub.publish(r_msg)
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)
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(): #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)
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