def main(): global NUM_SENSORS, BAUD_RATE, DEVICE_ADDRESS rospy.loginfo("Launching arduino analog reader") rospy.init_node("arduinoReader") ser = serial.Serial(DEVICE_ADDRESS) ser.baudrate = BAUD_RATE pubs = [] for index in range(NUM_SENSORS): pubs.append(rospy.Publisher(("/sonar/" + str(index)), Range)) while not rospy.is_shutdown(): data = ser.readline().split("\t") data.pop() # last element is garbage rospy.loginfo("Read line: " + str(data)) for reading in data: #construct message msg = Range() try: msg.range = float(reading) except ValueError: msg.range = -1.0 pubIndex = data.index(reading) if pubIndex < NUM_SENSORS : pubs[pubIndex].publish(msg)
def __init__(self): #初始化节点 rospy.init_node("talkerUltraSound", anonymous = True) #超声波数据发布节点 ultrasound_pub = rospy.Publisher("UltraSoundPublisher", Range, queue_size = 20) ran_broadcaster = tf.TransformBroadcaster() rate=rospy.Rate(1) while not rospy.is_shutdown(): ran_quat = Quaternion() ran_quat = quaternion_from_euler(0, 0, 0) #发布TF关系 ran_broadcaster.sendTransform((0.2,0.0,0.2),ran_quat,rospy.Time.now(),"ultrasound","base_link") #定义一个超声波消息 ran = Range() ran.header.stamp = rospy.Time.now() ran.header.frame_id = "/ultrasound" ran.field_of_view = 1; ran.min_range = 0; ran.max_range = 5; ran.range = 0.5; ultrasound_pub.publish(ran) rate.sleep()
def Publish(self): #初始化节点 rospy.init_node("talkerUltraSound", anonymous = True) #超声波数据发布节点 self.ultrasound_pub = rospy.Publisher("UltraSoundPublisher", Range, queue_size = 20) ran_broadcaster = tf.TransformBroadcaster() rate=rospy.Rate(10) #print "Input distance('+' is the end of the number):" while not rospy.is_shutdown(): #ch=self.getKey() #self.addChar(ch) ran_quat = Quaternion() ran_quat = quaternion_from_euler(0, 0, 0) #发布TF关系 ran_broadcaster.sendTransform((0.2,0.0,0.2),ran_quat,rospy.Time.now(),"ultrasound","base_link") #定义一个超声波消息 ran = Range() ran.header.stamp = rospy.Time.now() ran.header.frame_id = "/ultrasound" ran.field_of_view = 1; ran.min_range = 0; ran.max_range = 5; ran.range = self.dist; #ultrasound_pub.publish(ran) rate.sleep()
def prepareArray(self): for i in range(0, len(self.sensors)): r = Range() r.header.frame_id = "ir{}_link".format(i + 1) r.min_range = self.sensors[i].min_range r.max_range = self.sensors[i].max_range r.radiation_type = r.INFRARED self.msg.array += [r]
def 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 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 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 random_sonar(delay=1): print('Starting random sonar...') pub = Publisher('/sensors/range', Range) msg = Range() while not rospy.is_shutdown(): msg.header = rospy.Header(frame_id='right_sonar_frame') msg.range = random.random() * 20 pub.publish(msg) sleep(delay) msg.header = rospy.Header(frame_id='left_sonar_frame') msg.range = random.random() * 20 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 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 test_range(self): sensor = Sensors.Range(min_safe=10) msg = Range() msg.max_range = 3000 msg.min_range = 5 msg.range = 2500 sensor.measure(msg) self.assertEquals(sensor.max_range, msg.max_range) self.assertEquals(sensor.min_range, msg.min_range) self.assertEquals(sensor.range , msg.range) self.assertFalse(sensor.isFar()) self.assertFalse(sensor.isNear()) msg.range = 4001 sensor.measure(msg) self.assertTrue(sensor.isFar()) self.assertFalse(sensor.isNear()) msg.range = 3 sensor.measure(msg) self.assertFalse(sensor.isFar()) self.assertTrue(sensor.isNear()) msg.range = 8 sensor.measure(msg) self.assertFalse(sensor.isFar()) self.assertTrue(sensor.isNear())
def range_test(): from sensor_msgs.msg import Range sensor = RangeSensor() print sensor m = Range() m.range = 2000.0 m.max_range = 3000.0 m.min_range = 2.01 sensor.min_safe = 500 sensor.measure(m) print sensor print sensor.isNear() print sensor.isFar() m.range = 4000 sensor.measure(m) print sensor print sensor.isNear() print sensor.isFar() m.range = -2000 sensor.measure(m) print sensor print sensor.isNear() print sensor.isFar() m.range = 300 sensor.measure(m) print sensor print sensor.isNear() print sensor.isFar()
def talker(): pub = rospy.Publisher('lidar_data', Range, queue_size=10) rospy.init_node('lidar_node') rate = rospy.Rate(1000) # 10hz ser=serial.Serial('/dev/ttyUSB1',115200) msg=Range() while not rospy.is_shutdown(): p=ser.readline() if p!='\n' and p!='': try: msg.range=int(p) msg.header.stamp = rospy.Time.now() print msg.range pub.publish(msg) except: pass 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 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 callback(event): value = ADC.read("P9_39") outvoltage = 3.3 * value #minimum range if outvoltage > 2.75: rangevalue = 15 elif outvoltage < .4: rangevalue = 150 else: #exponential fit function, derived from data sheet rangevalue = 61.7 * math.pow(outvoltage, -1.2) #assign values and publish pub = rospy.Publisher('/range', Range, queue_size=10) rangeout = Range() rangeout.min_range = 15 rangeout.max_range = 150 rangeout.range = rangevalue rospy.loginfo(rangeout) pub.publish(rangeout)
def 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 talker(): signal.signal(signal.SIGINT, signal_handler) time.sleep(1) setup_servo() pub = rospy.Publisher('servo_ulta', Range, queue_size=10) rospy.init_node('servo_ultra', anonymous=True, disable_signals=False ) rate = rospy.Rate(10) # 10hz ultrasonic_number = 1 pause_time = 0.1 delay_ultrasonic = 0.11 STEP_ANGLE = 10 CURR_ANGLE = (90.0 * math.pi) / 180.0 mindt = 0.65 maxdt = 2.48 dt = [mindt, maxdt] extremes = [int(x * 1024.0 / 20.0) for x in dt] dt_range = extremes[1] - extremes[0] dt_step = int((dt_range / 180.0) * STEP_ANGLE) br = tf.TransformBroadcaster() while not rospy.is_shutdown(): #broadcast_transforms() # Move servo to counter-clockwise extreme. wiringpi.pwmWrite(18, extremes[1]) sleep(0.2) CURR_ANGLE = (90.0 * math.pi) / 180.0 for i in range(extremes[1],extremes[0],-dt_step): # 1025 because it stops at 1024 wiringpi.pwmWrite(18,i) sleep(pause_time) br.sendTransform((0.15, 0.0, 0.0),tf.transformations.quaternion_about_axis(CURR_ANGLE, (0, 0, 1)), rospy.Time.now(), "1", "base_link") data = Range() data.header.frame_id = "1" data.header.stamp = rospy.Time.now() data.radiation_type = 0 data.min_range = 0.05 data.max_range = 2.0 data.field_of_view = 0.164 try : data.range = float(get_us_data_from(ultrasonic_number)) /100.0 except IOError: subprocess.call(['i2cdetect', '-y', '1']) data.range = 0.0 pub.publish(data) CURR_ANGLE -= (STEP_ANGLE * math.pi) / 180.0
def default(self, ci='unused'): msg = Range() msg.header = self.get_ros_header() msg.radiation_type = Range.INFRARED msg.field_of_view = 20 msg.min_range = 0 msg.max_range = self.component_instance.bge_object['laser_range'] tmp = msg.max_range for ray in self.data['range_list']: if tmp > ray: tmp = ray msg.range = tmp self.publish(msg)
def 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 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 default(self, ci='unused'): """ Publish the data of the infrared sensor as a ROS Range message """ msg = Range() msg.radiation_type = Range.INFRARED msg.field_of_view = 20 msg.min_range = 0 msg.max_range = self.component_instance.bge_object['laser_range'] tmp = msg.max_range for ray in self.data['range_list']: if tmp > ray: tmp = ray msg.range = tmp self.publish(msg)
def inputCallback(self, msg): ######################################################################### # rospy.loginfo("-D- range_filter inputCallback") cur_val = msg.value if cur_val <= self.max_valid and cur_val >= self.min_valid: self.prev.append(cur_val) del self.prev[0] p = array(self.prev) self.rolling_ave = p.mean() self.rolling_std = p.std() self.rolling_meters = ((self.b * self.rolling_ave) ** self.m) / 100 self.filtered_pub.publish( self.rolling_meters ) self.std_pub.publish( self.rolling_std ) rng = Range() rng.radiation_type = 1 rng.min_range = self.min_range rng.max_range = self.max_range rng.range = self.rolling_meters rng.header.frame_id = self.frame rng.field_of_view = 0.1 rng.header.stamp = rospy.Time.now() self.range_pub.publish( rng ) ranges = [] intensities = [] angle_start = 0.0 - self.field_of_view angle_stop = self.field_of_view for angle in linspace( angle_start, angle_stop, 10): ranges.append( self.rolling_meters ) intensities.append( 1.0 ) scan = LaserScan() scan.ranges = ranges scan.header.frame_id = self.frame scan.time_increment = 0; scan.range_min = self.min_range scan.range_max = self.max_range scan.angle_min = angle_start scan.angle_max = angle_stop scan.angle_increment = (angle_stop - angle_start) / 10 scan.intensities = intensities scan.scan_time = self.scan_time scan.header.stamp = rospy.Time.now() self.scan_pub.publish( scan )
def 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 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 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 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
import math def enable_front_sensors_cb(msg): global enable_front_sensors enable_front_sensors = msg.data rospy.loginfo("enable_front_sensors: {}".format(enable_front_sensors)) SONAR_COUNT = 10 sonar_hist = [[] for _ in range(SONAR_COUNT)] # [[],] * SONAR_COUNT BUGFIX temp_correction_coef = 1.0 # Updated in imu_calib_callback() enable_front_sensors = True # If false shortens front sonars range and disables from bump sensors range_sonar_msg = Range() range_bump_msg = Range() # 0 1 2 3 4 | 5 6 7 8 9 # sonar_max_ranges = [0.2, 0.5, 0.55, 0.7, 0.5, 0.5, 0.7, 0.55, 0.5, 0.2] # NOTE: tuning filter. Found experimentally sonar_max_ranges = [0.07, 0.4, 0.45, 0.55, 0.4, 0.4, 0.55, 0.45, 0.4, 0.07] # NOTE: tuning filter. Found experimentally sonar_max_ranges_disabled = [0.03, 0.06, -1, -1, -1, -1, -1, -1, 0.06, 0.03] # NOTE: tuning filter. Found experimentally # sonar_max_ranges = [0.4] + [0.5] * 8 + [0.4] # NOTE: tuning filter. Found experimentally # Publishers sonar_pub = rospy.Publisher("range_sonar", Range, queue_size=10) # sonar_pub_all = rospy.Publisher("range_sonar_all", Range, queue_size=5) # NOTE: debug bump_pub1 = rospy.Publisher("range_bump1", Range, queue_size=10) bump_pub2 = rospy.Publisher("range_bump2", Range, queue_size=10)
def init_sensor_variables(self): #Primer mensaje #STD self.data_STD = Range() self.data_STD.radiation_type = self.data_STD.ULTRASOUND #Ultrasound self.data_STD.field_of_view = self.field_of_view #rad self.data_STD.min_range = self.sensor_min_range self.data_STD.max_range = self.sensor_max_range self.data_STD.range = 4.0 self.data_STD.header.stamp = rospy.Time.now() self.data_STD.header.frame_id = "STD_frame" #SDI self.data_SDI = Range() self.data_SDI.radiation_type = self.data_SDI.ULTRASOUND #Ultrasound self.data_SDI.field_of_view = self.field_of_view #rad self.data_SDI.min_range = self.sensor_min_range self.data_SDI.max_range = self.sensor_max_range self.data_SDI.range = 1.4 self.data_SDI.header.stamp = rospy.Time.now() self.data_SDI.header.frame_id = "SDI_frame" #SLIT self.data_SLIT = Range() self.data_SLIT.radiation_type = self.data_SLIT.ULTRASOUND #Ultrasound self.data_SLIT.field_of_view = self.field_of_view #rad self.data_SLIT.min_range = self.sensor_min_range self.data_SLIT.max_range = self.sensor_max_range self.data_SLIT.range = 3.0 self.data_SLIT.header.stamp = rospy.Time.now() self.data_SLIT.header.frame_id = "SLIT_frame" #SLDD self.data_SLDD = Range() self.data_SLDD.radiation_type = self.data_SLDD.ULTRASOUND #Ultrasound self.data_SLDD.field_of_view = self.field_of_view #rad self.data_SLDD.min_range = self.sensor_min_range self.data_SLDD.max_range = self.sensor_max_range self.data_SLDD.range = 3.0 self.data_SLDD.header.stamp = rospy.Time.now() self.data_SLDD.header.frame_id = "SLDD_frame" #Segundo mensaje #SLDT self.data_SLDT = Range() self.data_SLDT.radiation_type = self.data_SLDT.ULTRASOUND #Ultrasound self.data_SLDT.field_of_view = self.field_of_view #rad self.data_SLDT.min_range = self.sensor_min_range self.data_SLDT.max_range = self.sensor_max_range self.data_SLDT.range = 3.0 self.data_SLDT.header.stamp = rospy.Time.now() self.data_SLDT.header.frame_id = "SLDT_frame" #STI self.data_STI = Range() self.data_STI.radiation_type = self.data_STI.ULTRASOUND #Ultrasound self.data_STI.field_of_view = self.field_of_view #rad self.data_STI.min_range = self.sensor_min_range self.data_STI.max_range = self.sensor_max_range self.data_STI.range = 3.0 self.data_STI.header.stamp = rospy.Time.now() self.data_STI.header.frame_id = "STI_frame" #SLID self.data_SLID = Range() self.data_SLID.radiation_type = self.data_SLID.ULTRASOUND #Ultrasound self.data_SLID.field_of_view = self.field_of_view #rad self.data_SLID.min_range = self.sensor_min_range self.data_SLID.max_range = self.sensor_max_range self.data_SLID.range = 3.0 self.data_SLID.header.stamp = rospy.Time.now() self.data_SLID.header.frame_id = "SLID_frame" #SDD self.data_SDD = Range() self.data_SDD.radiation_type = self.data_SDD.ULTRASOUND #Ultrasound self.data_SDD.field_of_view = self.field_of_view #rad self.data_SDD.min_range = self.sensor_min_range self.data_SDD.max_range = self.sensor_max_range self.data_SDD.range = 2.2 self.data_SDD.header.stamp = rospy.Time.now() self.data_SDD.header.frame_id = "SDD_frame" self.max_array_ultrasonic = 3
def __init__(self): global place_areax global place_areay global placetimes rospy.loginfo("=============Ready to move the robot================") #Initialize moveit_commander self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm") self.left_arm_group = moveit_commander.MoveGroupCommander("left_arm") #Display Trajectory in RVIZ self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.moving_publisher = rospy.Publisher('amimoving', String, queue_size=10) rospy.sleep(3.0) self.planning_frame = self.right_arm_group.get_planning_frame() rospy.loginfo("============ Reference frame: %s" % self.planning_frame) self.eef_link = self.right_arm_group.get_end_effector_link() print "============ End effector: %s" % self.eef_link self.group_names = self.robot.get_group_names() print "============ Robot Groups:", self.robot.get_group_names() #set planning and execution parameters self.right_arm_group.set_goal_position_tolerance(0.01) self.right_arm_group.set_goal_orientation_tolerance(0.01) self.right_arm_group.set_planning_time(5.0) self.right_arm_group.allow_replanning(False) self.right_arm_group.set_max_velocity_scaling_factor(1) self.right_arm_group.set_max_acceleration_scaling_factor(1) self.left_arm_group.set_goal_position_tolerance(0.01) self.left_arm_group.set_goal_orientation_tolerance(0.01) self.left_arm_group.set_planning_time(5.0) self.left_arm_group.allow_replanning(False) self.left_arm_group.set_max_velocity_scaling_factor(0.5) self.left_arm_group.set_max_acceleration_scaling_factor(0.5) #specify which gripper and limb are taking command self.right_gripper = Gripper('right', CHECK_VERSION) self.right_gripper.reboot() self.right_gripper.calibrate() # self.left_gripper = Gripper('left', CHECK_VERSION) # self.left_gripper.reboot() # self.left_gripper.calibrate() #Standoff hight above the block self.standoff = 0.2 #Screw axis for Right arm of Baxter self.Blist = np.array([[-1, 0, 0, 0, 1.17594, 0], [0, 1, 0, 1.10694, 0, -0.079], [0, 0, 1, 0, 0.079, 0], [0, 1, 0, 0.74259, 0, -0.01], [0, 0, 1, 0, 0.01, 0], [0, 1, 0, 0.3683, 0, 0], [0, 0, 1, 0, 0, 0]]).T self.M = np.array( [[0, 1 / sqrt(2), 1 / sqrt(2), 0.064 + (1.17594 / sqrt(2))], [0, 1 / sqrt(2), -1 / sqrt(2), -0.278 - (1.17594 / sqrt(2))], [-1, 0, 0, 0.19135], [0, 0, 0, 1]]) #subscribe to range sensor topic self.__right_sensor = rospy.Subscriber( '/robot/range/right_hand_range/state', Range, self.rangecallback) self.rangestatetemp = Range() #subscribe to target location topic self.grabcamera() self.go_startpose() self.location_data = rospy.Subscriber('square_location', String, self.compute_cartesian)
def __init__(self, rate): # linear and angular velocity self.velocity = Twist() # joints' states self.joint_states = JointState() # Sensors self.imu = Imu() self.imu_yaw = 0.0 # (-pi, pi] self.sonar_F = Range() self.sonar_FL = Range() self.sonar_FR = Range() self.sonar_L = Range() self.sonar_R = Range() # ROS SETUP # initialize subscribers for reading encoders and publishers for performing position control in the joint-space # Robot self.velocity_pub = rospy.Publisher('/mymobibot/cmd_vel', Twist, queue_size=1) self.joint_states_sub = rospy.Subscriber('/mymobibot/joint_states', JointState, self.joint_states_callback, queue_size=1) # Sensors self.imu_sub = rospy.Subscriber('/imu', Imu, self.imu_callback, queue_size=1) self.sonar_front_sub = rospy.Subscriber('/sensor/sonar_F', Range, self.sonar_front_callback, queue_size=1) self.sonar_frontleft_sub = rospy.Subscriber( '/sensor/sonar_FL', Range, self.sonar_frontleft_callback, queue_size=1) self.sonar_frontright_sub = rospy.Subscriber( '/sensor/sonar_FR', Range, self.sonar_frontright_callback, queue_size=1) self.sonar_left_sub = rospy.Subscriber('/sensor/sonar_L', Range, self.sonar_left_callback, queue_size=1) self.sonar_right_sub = rospy.Subscriber('/sensor/sonar_R', Range, self.sonar_right_callback, queue_size=1) #Create new publishers for plotting: self.pub1 = rospy.Publisher("/R_sonar_error", Float64, queue_size=100) self.pub2 = rospy.Publisher("/FR_sonar_error", Float64, queue_size=100) self.pub3 = rospy.Publisher("/linear_x_velocity", Float64, queue_size=100) #Publishing rate self.period = 1.0 / rate self.pub_rate = rospy.Rate(rate) self.publish()
def init(): global area_pub, pc2_area_pub, tf2_buffer, map_sub, pc2_sub, fov_msg, max_time, ml, cl, default_colour, rgb_channeli, cl_mapping, va_func rospy.init_node("ros_rvv") # Parameters # Range rframe = rospy.get_param("/ros_rvv/frame", "camera_frame") fov_topic = rospy.get_param("/ros_rvv/fov_pub_topic", "/ros_rvv/fov") rt = rospy.get_param("/ros_rvv/radiation_type", 1) # 0 for ultrasound, 1 for IR fov = rospy.get_param("/ros_rvv/field_of_view", math.pi) # radians r = rospy.get_param("/ros_rvv/range", 1) # meters rate = rospy.get_param("/ros_rvv/range_rate", 5) # Hz # TODO currently not supporting the occupancy grid expr = rospy.get_param("/ros_rvv/viewed_area_calculation_expression", "dt") # expr = rospy.get_param("/ros_rvv/viewed_area_calculation_expression", "dt/r*r") va_func = lambda dt, r=0: eval(expr) # Calling the function to force quit the program if it contains an illegal expression va_func(0, 0) # OccupancyGrid pa = rospy.get_param("/ros_rvv/publish_area", False) mst = rospy.get_param("/ros_rvv/map_sub_topic", "projected_map") mpt = rospy.get_param("/ros_rvv/map_pub_topic", "/ros_rvv/viewed_area") # Map topic does not change, so subscribe only once ml = rospy.get_param("/ros_rvv/latched_map", True) # PointCloud2 pap = rospy.get_param("/ros_rvv/publish_area_as_pc2", True) cst = rospy.get_param("/ros_rvv/pc2_sub_topic", "octomap_point_cloud_centers") cpt = rospy.get_param("/ros_rvv/pc2_pub_topic", "/ros_rvv/viewed_area_pc2") cl_mapping = rospy.get_param("/ros_rvv/cloud_mapping", True) # Map topic does not change, so subscribe only once cl = rospy.get_param("/ros_rvv/latched_cloud", True) default_colour = rospy.get_param("/ros_rvv/default_cloud_colour", (100, 100, 100)) rgb_channel = rospy.get_param("/ros_rvv/cloud_viewed_channel", "g") rgb_channeli = 1 # Disable latching if we have enabled mapping cl = cl if not cl_mapping else False if rgb_channel == "r": rgb_channeli = 0 elif rgb_channel == "b": rgb_channeli = 2 default_colour = struct.unpack( "f", struct.pack("i", int("%02x%02x%02x" % default_colour, 16)))[0] max_time = rospy.get_param("/ros_rvv/max_time", 10) # in secs, <=0 to disable it fov_pub = rospy.Publisher(fov_topic, Range, queue_size=1) # Constant range msg fov_msg = Range() fov_msg.header.frame_id = rframe fov_msg.radiation_type = rt fov_msg.field_of_view = fov fov_msg.min_range = r fov_msg.max_range = r fov_msg.range = r # Initialize only if we need the viewed area if pa or pap: tf2_buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tf2_buffer) rospy.Service("/ros_rvv/clear_viewed_area", Empty, clearCallback) if pa: area_pub = rospy.Publisher(mpt, OccupancyGrid, queue_size=1) map_sub = rospy.Subscriber(mst, OccupancyGrid, ogCallback) if pap: pc2_area_pub = rospy.Publisher(cpt, PointCloud2, queue_size=1) pc2_sub = rospy.Subscriber(cst, PointCloud2, pc2Callback) while not rospy.is_shutdown(): fov_msg.header.stamp = rospy.Time.now() fov_pub.publish(fov_msg) rospy.sleep(1 / rate) if ml and lmap is not None: handleOccupancyGrid(lmap) if cl and lcloud is not None: handlePointCloud2(lcloud)
#from sensor_msgs.msg import LaserScan from sensor_msgs.msg import Range Fs = 8000 f = 500 rospy.init_node('laser_scan_publisher') scan_pub_s0 = rospy.Publisher('scan_s0', Range, queue_size=50) scan_pub_s1 = rospy.Publisher('scan_s1', Range, queue_size=50) num_readings = 100 laser_frequency = 40 a = [0] * num_readings count = 0 r = rospy.Rate(1.0) scan = Range() scan_s0 = Range() scan_s1 = Range() scan.header.frame_id = "/vl_sensor" scan.radiation_type = 0 scan.field_of_view = 0.1 scan.min_range = 1 #min_range scan.max_range = 20 #max_range scan_s0 = scan scan_s1 = scan while not rospy.is_shutdown(): # scan = LaserScan() scan_s0.header.stamp = rospy.Time.now() scan_s1.header.stamp = rospy.Time.now()
import rospy import time from sensor_msgs.msg import Range last_reading = time.time() curr_height = 10.0 curr_z_vel = 0 velocity_alpha = 0.1 height_alpha = 0.1 sensor_angle_alpha = 0.5 sensor_safe_zone = (3, 30) zpub = None rng = Range() def sensor_callback(data): global curr_height, curr_z_vel, last_reading global velocity_alpha, height_alpha, sensor_safe_zone, sensor_angle_alpha global zpub, rng measured_height = data.range # convert units as needed here measured_angle = 0 # we'll start populating this eventually dt = time.time() - last_reading # find elapsed time last_reading = time.time() predicted_height = curr_height + curr_z_vel * dt # predict the height based on previous velocity # we can add in control here as well when we want to angle_conf = 1.0 / measured_angle if measured_angle > 0 else 1.0
#!/usr/bin/env python # coding: utf-8 #======= Import ================ import rospy from std_msgs.msg import Float64MultiArray from std_msgs.msg import Int64MultiArray from sensor_msgs.msg import Range from sensor_msgs.msg import BatteryState from tquad.msg import LineSensor lineSensor = LineSensor() ultrasound = Range() battery = BatteryState() encoders = Int64MultiArray() def publishRange(value): """ Fonction pour publier la valeur renvoyée par le capteur ultrason """ ultrasound.header.stamp = rospy.Time.now() ultrasound.header.frame_id = "/ultrasound" ultrasound.radiation_type = 0 ultrasound.field_of_view = 0.1 ultrasound.min_range = 0 ultrasound.max_range = 2 ultrasound.range = value ultrasound_pub.publish(ultrasound)
def read_data(self): # def read_data(self): # print("manuel driver reading started") # while(self.reading): #self.dataready=False dataf = np.zeros(12) locationStr = "" #self.distances = np.zeros(4) #self.distances_id = np.zeros(4) locationStr = self.manhid.read_device(64) if (locationStr == ""): self.dataready = False pass #self.errorCode = 16 else: datas = locationStr.split(',') dataf = map(int, datas[0:12]) self.distances[0] = dataf[0] self.distances[1] = dataf[2] self.distances[2] = dataf[4] self.distances[3] = dataf[6] #print(self.distances) self.distances_id[0] = dataf[1] self.distances_id[1] = dataf[3] self.distances_id[2] = dataf[5] self.distances_id[3] = dataf[7] self.adc = dataf[8:9] self.manuelcontrol = dataf[10] self.emergency = dataf[11] self.dataready = True self.stamp = time.time() if (self.dataready): self.dataready = False distancesready = False ultraSonicRange1 = Range() ultraSonicRange2 = Range() ultraSonicRange3 = Range() ultraSonicRange4 = Range() self.distanceses = np.zeros(4) # if(self.desiredspeedleft<0 or self.desiredspeedright<0): # self.man.write_data("T3") # else: self.write_data("T1") # counter = 0 # for i in self.man.distances_id: # self.distanceses[int(i)] = self.man.distances[counter] # counter = counter+1 # distancesready = True distancesready = True self.distanceses = self.distances #print(2.4*self.distanceses/9212.0) if (distancesready): ultraSonicRange1.header.stamp = rospy.Time.now() ultraSonicRange1.header.frame_id = "ultrasonic1" ultraSonicRange1.radiation_type = 0 ultraSonicRange1.field_of_view = 0.52 ultraSonicRange1.min_range = 0.2 ultraSonicRange1.max_range = 2.4 self.distanceses1[0] = 24 * self.distanceses[0] / 9212.0 ultraSonicRange1.range = self.distanceses1[0] ultraSonicRange2.header.stamp = rospy.Time.now() ultraSonicRange2.header.frame_id = "ultrasonic2" ultraSonicRange2.radiation_type = 0 ultraSonicRange2.field_of_view = 0.52 ultraSonicRange2.min_range = 0.2 ultraSonicRange2.max_range = 2.4 self.distanceses1[1] = 24 * self.distanceses[1] / 9212.0 ultraSonicRange2.range = self.distanceses1[1] ultraSonicRange3.header.stamp = rospy.Time.now() ultraSonicRange3.header.frame_id = "ultrasonic3" ultraSonicRange3.radiation_type = 0 ultraSonicRange3.field_of_view = 0.52 ultraSonicRange3.min_range = 0.2 ultraSonicRange3.max_range = 2.4 self.distanceses1[2] = 24 * self.distanceses[2] / 9212.0 ultraSonicRange3.range = self.distanceses1[2] ultraSonicRange4.header.stamp = rospy.Time.now() ultraSonicRange4.header.frame_id = "ultrasonic4" ultraSonicRange4.radiation_type = 0 ultraSonicRange4.field_of_view = 0.52 ultraSonicRange4.min_range = 0.2 ultraSonicRange4.max_range = 2.4 self.distanceses1[3] = 24 * self.distanceses[3] / 9212.0 ultraSonicRange4.range = self.distanceses1[3] if (self.distanceses1[0] > 2.4): self.distanceses1[0] = 2.4 ultraSonicRange1.range = self.distanceses1[0] if (self.distanceses1[1] > 2.4): self.distanceses1[1] = 2.4 ultraSonicRange2.range = self.distanceses1[1] if (self.distanceses1[2] > 2.4): self.distanceses1[2] = 2.4 ultraSonicRange3.range = self.distanceses1[2] if (self.distanceses1[3] > 2.4): self.distanceses1[3] = 2.4 ultraSonicRange4.range = self.distanceses1[3] #print(self.distanceses1) self.sonic_range_pub1.publish(ultraSonicRange1) self.sonic_range_pub2.publish(ultraSonicRange2) self.sonic_range_pub3.publish(ultraSonicRange3) self.sonic_range_pub4.publish(ultraSonicRange4)
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 prepare_range_msg(self): ''' Fill range message Please, do it your self for practice ''' range_msg = Range()
def poll(self): now = rospy.Time.now() if now > self.t_next: #try: # left_pidin, right_pidin = self.arduino.get_pidin() #except: # rospy.logerr("getpidout exception count: ") # return #self.lEncoderPub.publish(left_pidin) #self.rEncoderPub.publish(right_pidin) #try: # left_pidout, right_pidout = self.arduino.get_pidout() #except: # rospy.logerr("getpidout exception count: ") # return #self.lPidoutPub.publish(left_pidout) #self.rPidoutPub.publish(right_pidout) # Read the encoders try: left_enc, right_enc = self.arduino.get_encoder_counts() #rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc)) except: self.bad_encoder_count += 1 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) return try: sonar_value1 = self.arduino.get_sonar1_value() except: rospy.logerr("sonar exception ") try: sonar_value2 = self.arduino.get_sonar2_value() except: rospy.logerr("sonar exception ") try: sonar_value3 = self.arduino.get_sonar3_value() except: rospy.logerr("sonar exception ") try: sonar_value4 = self.arduino.get_sonar4_value() except: rospy.logerr("sonar exception ") try: sonar_value5 = self.arduino.get_sonar5_value() except: rospy.logerr("sonar exception ") try: sonar_value6 = self.arduino.get_sonar6_value() except: rospy.logerr("sonar exception ") dt = now - self.then self.then = now dt = dt.to_sec() # Calculate odometry if self.enc_left == None: dright = 0 dleft = 0 else: if (left_enc < self.encoder_low_wrap and self.enc_left > self.encoder_high_wrap): self.l_wheel_mult = self.l_wheel_mult + 1 elif (left_enc > self.encoder_high_wrap and self.enc_left < self.encoder_low_wrap): self.l_wheel_mult = self.l_wheel_mult - 1 else: self.l_wheel_mult = 0 if (right_enc < self.encoder_low_wrap and self.enc_right > self.encoder_high_wrap): self.r_wheel_mult = self.r_wheel_mult + 1 elif (right_enc > self.encoder_high_wrap and self.enc_right < self.encoder_low_wrap): self.r_wheel_mult = self.r_wheel_mult - 1 else: self.r_wheel_mult = 0 #dright = (right_enc - self.enc_right) / self.ticks_per_meter #dleft = (left_enc - self.enc_left) / self.ticks_per_meter dleft = 1.0 * (left_enc + self.l_wheel_mult * (self.encoder_max - self.encoder_min) - self.enc_left) / self.ticks_per_meter dright = 1.0 * (right_enc + self.r_wheel_mult * (self.encoder_max - self.encoder_min) - self.enc_right) / self.ticks_per_meter self.enc_right = right_enc self.enc_left = left_enc dxy_ave = (dright + dleft) / 2.0 dth = (dright - dleft) / self.wheel_track vxy = dxy_ave / dt vth = dth / dt if (dxy_ave != 0): dx = cos(dth) * dxy_ave dy = -sin(dth) * dxy_ave self.x += (cos(self.th) * dx - sin(self.th) * dy) self.y += (sin(self.th) * dx + cos(self.th) * dy) if (dth != 0): self.th += dth quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) # Create the odometry transform frame broadcaster. self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), self.base_frame, "odom") odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = self.base_frame odom.header.stamp = now odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = vxy odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth # todo sensor_state.distance == 0 #if self.v_des_left == 0 and self.v_des_right == 0: # odom.pose.covariance = ODOM_POSE_COVARIANCE2 # odom.twist.covariance = ODOM_TWIST_COVARIANCE2 #else: # odom.pose.covariance = ODOM_POSE_COVARIANCE # odom.twist.covariance = ODOM_TWIST_COVARIANCE self.odomPub.publish(odom) dia = xtyh() rang = Ranges() rang1 = Range() rang2 = Range() rang3 = Range() rang4 = Range() rang5 = Range() rang6 = Range() rang1.range = sonar_value1 rang2.range = sonar_value2 rang3.range = sonar_value3 rang4.range = sonar_value4 rang5.range = sonar_value5 rang6.range = sonar_value6 rang.ranges.append(rang1) rang.ranges.append(rang2) rang.ranges.append(rang3) rang.ranges.append(rang4) rang.ranges.append(rang5) rang.ranges.append(rang6) dia.huf.append(sonar_value1) dia.huf.append(sonar_value2) dia.huf.append(sonar_value3) dia.huf.append(sonar_value4) dia.huf.append(sonar_value5) dia.huf.append(sonar_value6) self.sonarPub.publish(rang) self.sonarnPub.publish(dia) self.sonar1Pub.publish(sonar_value1) self.sonar2Pub.publish(sonar_value2) self.sonar3Pub.publish(sonar_value3) self.sonar4Pub.publish(sonar_value4) self.sonar5Pub.publish(sonar_value5) self.sonar6Pub.publish(sonar_value6) if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): self.v_des_left = 0 self.v_des_right = 0 if self.v_left < self.v_des_left: self.v_left += self.max_accel if self.v_left > self.v_des_left: self.v_left = self.v_des_left else: self.v_left -= self.max_accel if self.v_left < self.v_des_left: self.v_left = self.v_des_left if self.v_right < self.v_des_right: self.v_right += self.max_accel if self.v_right > self.v_des_right: self.v_right = self.v_des_right else: self.v_right -= self.max_accel if self.v_right < self.v_des_right: self.v_right = self.v_des_right '''
def publish_dist(self, time): if len(self.measurement_queue) > 0: msg = Range() msg.range = np.asarray(self.measurement_queue).mean() self.measurement_queue = list() self.pub.publish(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)
import random import string import tf2_ros import numpy as np import tf2_geometry_msgs from std_srvs.srv import Empty from nav_msgs.msg import OccupancyGrid import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import Range, PointCloud2, PointField from geometry_msgs.msg import TransformStamped, PoseStamped area_pub = None map_sub = None pc2_area_pub = None pc2_sub = None fov_msg = Range() tf2_buffer = None grid_timestamps = np.array([]) cloud_timestamps = np.array([]) cloud_timestampsd = dict() max_time = 0 last_t = None last_tc = None lmap = None lcloud = None ml = False cl = True default_colour = (0, 0, 0) rgb_channeli = 1 # green cl_mapping = True va_func = None
#!/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()
def _hw_infrareds_cb(self, msg): leftMsg = Range() leftMsg.header.stamp = rospy.Time.now() leftMsg.header.frame_id = "left_infrared_link" leftMsg.radiation_type = Range.INFRARED 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_infra_pub.publish(leftMsg) rightMsg = Range() rightMsg.header.stamp = rospy.Time.now() rightMsg.header.frame_id = "right_infrared_link" rightMsg.radiation_type = Range.INFRARED 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_infra_pub.publish(rightMsg)
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()
import csv import rospy import rospkg import message_filters from ysi_exo.msg import Sonde from sensor_msgs.msg import NavSatFix, Range, Temperature from mavros_msgs.msg import VFR_HUD from nav_msgs.msg import Odometry from geometry_msgs.msg import TwistStamped #from ping_nodelet.msg import Ping from std_msgs.msg import Float64 CSV_FILE_PATH = "" cur_compass = Range() def append_file(data_array): with open(CSV_FILE_PATH, 'a') as csv_file: writer = csv.writer(csv_file) writer.writerow(data_array) # bluerov2 # def gps_sonde_callback(sonde_msg, gps_msg, depth_msg, velocity_msg): # data_array = [gps_msg.header.stamp.to_sec(), gps_msg.latitude, gps_msg.longitude, # depth_msg.header.stamp.to_sec(), depth_msg.altitude, # # temp_msg.header.stamp.to_sec(), temp_msg.temperature, # # echosounder_msg.header.stamp.to_sec(), echosounder_msg.distance, echosounder_msg.confidence, # velocity_msg.header.stamp.to_sec(),
def timerSonarCB(self, event): output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x19) + chr( 0x00) + chr(0xff) #0xff is CRC-8 value while (self.serialIDLE_flag): time.sleep(0.01) self.serialIDLE_flag = 3 try: while self.serial.out_waiting: pass self.serial.write(output) except: rospy.logerr("Sonar Command Send Faild") self.serialIDLE_flag = 0 msg = Range() msg.header.stamp = self.current_time msg.field_of_view = 0.26 #about 15 degree msg.max_range = 2.5 msg.min_range = 0.01 # Sonar 1 msg.header.frame_id = 'Sonar_1' if self.Sonar[0] == 0xff: msg.range = float('inf') else: msg.range = self.Sonar[0] / 100.0 self.range_pub1.publish(msg) # TF value calculate from mechanical structure if ('NanoRobot' in base_type): self.tf_broadcaster.sendTransform( (0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), self.current_time, 'Sonar_1', self.baseId) elif ('NanoCar' in base_type): self.tf_broadcaster.sendTransform( (0.18, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), self.current_time, 'Sonar_1', self.baseId) elif ('4WD' in base_type): self.tf_broadcaster.sendTransform( (0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), self.current_time, 'Sonar_1', self.baseId) else: pass # Sonar 2 if sonar_num > 1: if self.Sonar[1] == 0xff: msg.range = float('inf') else: msg.range = self.Sonar[1] / 100.0 msg.header.frame_id = 'Sonar_2' self.range_pub2.publish(msg) if ('NanoRobot' in base_type): self.tf_broadcaster.sendTransform( (0.0, 0.0, 0.0), (0.0, 0.0, -1.0, 0), self.current_time, 'Sonar_2', self.baseId) elif ('NanoCar' in base_type): self.tf_broadcaster.sendTransform( (-0.035, 0.0, 0.0), (0.0, 0.0, -1.0, 0), self.current_time, 'Sonar_2', self.baseId) elif ('4WD' in base_type): self.tf_broadcaster.sendTransform( (0.0, 0.0, 0.0), (0.0, 0.0, -1.0, 0), self.current_time, 'Sonar_2', self.baseId) else: pass if sonar_num > 2: # Sonar 3 msg.header.frame_id = 'Sonar_3' if self.Sonar[2] == 0xff: msg.range = float('inf') else: msg.range = self.Sonar[2] / 100.0 self.range_pub3.publish(msg) self.tf_broadcaster.sendTransform( (0.0, 0.0, 0.0), (0.0, 0.0, 0.707, 0.707), self.current_time, 'Sonar_3', self.baseId) if sonar_num > 3: # Sonar 4 if self.Sonar[3] == 0xff: msg.range = float('inf') else: msg.range = self.Sonar[3] / 100.0 msg.header.frame_id = 'Sonar_4' self.range_pub4.publish(msg) self.tf_broadcaster.sendTransform( (0.0, 0.0, 0.0), (0.0, 0.0, -0.707, 0.707), self.current_time, 'Sonar_4', self.baseId)
#!/usr/bin/env python import rospy import mavros_msgs #from mavros_msgs import srv from mavros_msgs.msg import State from sensor_msgs.msg import BatteryState, Temperature, Range drone_state = State() drone_battery = BatteryState() rangefinder = Range() def drone_validate(): imu_temperature = 25 # imu_temperature e a temperatura interna da pixhawk max_temperature = 60 rate = rospy.Rate(20) rate.sleep() def state_callback(state_data): global drone_state drone_state = state_data def battery_callback(battery_data): global drone_battery drone_battery.voltage = battery_data.voltage drone_battery.current = battery_data.current drone_battery.percentage = battery_data.percentage
def __init__(self): ''' This class is used to transmit data from morse node to a transparent node used in ros airship you will have to modify it. ''' # Get the ~private namespace parameters from command line or launch file. init_message = rospy.get_param('~message', 'hello') self.rate = float(rospy.get_param('~rate', '30.0')) self.rate_before_sleep = self.rate self.topic_root = rospy.get_param('~topic', 'trejectory_planner') self.topic_hardcom = rospy.get_param('~topic_hardcom', 'hardcom') self.topic_camcom = rospy.get_param('~topic_camcom', 'cameracom') self.topic_command = rospy.get_param('~topic_command', 'command') self.topic_odometry = rospy.get_param('~topic_odometry', 'odometry') rospy.loginfo("airship/" + self.topic_root + "/rate = " + str(self.rate) + "Hz") #Defining published messages ''' RANGE FIELDS: Header header uint8 ULTRASOUND=0 uint8 INFRARED=1 uint8 radiation_type float32 field_of_view float32 min_range float32 max_range float32 range ''' self.range = Range() ''' IMU FIELDS: Header header geometry_msgs/Quaternion orientation float64[9] orientation_covariance # Row major about x, y, z axes geometry_msgs/Vector3 angular_velocity float64[9] angular_velocity_covariance # Row major about x, y, z axes geometry_msgs/Vector3 linear_acceleration float64[9] linear_acceleration_covariance # Row major x, y z ''' self.imu = Imu() ''' ODOMETRY FIELDS std_msgs/Header header uint32 seq time stamp string frame_id string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[36] covariance geometry_msgs/TwistWithCovariance twist geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z float64[36] covariance ''' self.estimated_pose = Odometry() self.image = Image() self.viz_img = Image() ''' POSE FIELDS: # A representation of pose in free space, composed of position and orientation. Point position POINT FIELDS: # This contains the position of a point in free space float64 x float64 y float64 z Quaternion orientation QUATERNION FIELDS float64 x float64 y float64 z float64 w ''' #self.wish_pose = Pose() ''' Vector3 FIELDS: float64 x float64 y float64 z ''' self.wish_command = Vector3() # x : speed command ; y : elevation command ; z : rudder command self.is_line_tracking = False self.is_trajectory_tracking = False self.is_sleep_state = False self.bridge = CvBridge() #Defining the subscriber rospy.Subscriber(self.topic_hardcom+"/LidarRange", Range, self.call_range) rospy.Subscriber(self.topic_hardcom+"/InertialData", Imu, self.call_imu) rospy.Subscriber(self.topic_command+"/isSleep", Bool, self.call_sleep) rospy.Subscriber("cameracom/CameraImg", Image, self.call_image) rospy.Subscriber("pose2odom/EstimatedPose", Odometry, self.call_estimated_pose) #Defining all the publisher self.wish_command_pub = rospy.Publisher(self.topic_root+"/WishCommand", Vector3, queue_size=10) self.vizualisation_pub = rospy.Publisher("planner/linetrack_viz", Image, queue_size=10) #Defining service to set tracking mode self.proceed_line_tracking = rospy.Service(self.topic_root+"/proceed_line_tracking", Trigger, self.line_track) self.proceed_point_tracking = rospy.Service(self.topic_root+"/proceed_point_tracking", TrigPointTracking, self.point_track) #Defining service to set publish rate self.set_rate_service = rospy.Service(self.topic_root+"/set_rate", SetRate, self.set_rate) self.linetracker = lt.LineTracking() while not rospy.is_shutdown(): if not self.is_sleep_state: #Publish the wishpose if self.is_line_tracking: (r, p, y) = tf.transformations.euler_from_quaternion([self.estimated_pose.pose.pose.orientation.x, self.estimated_pose.pose.pose.orientation.y, self.estimated_pose.pose.pose.orientation.z, self.estimated_pose.pose.pose.orientation.w]) img = self.bridge.imgmsg_to_cv2(self.image, "bgr8") unit_dir, unit_line_dir, sum_vect, frame, edge = self.linetracker.update(img, [r,p,y], self.estimated_pose.pose.pose.position.z) self.viz_img = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8") unit = sum_vect yaw = (np.arctan(unit[1]/unit[0])) + y self.wish_command.x = 0.5 self.wish_command.y = 0. self.wish_command.z = yaw self.vizualisation_pub.publish(self.viz_img) self.wish_command_pub.publish(self.wish_command) #this is the rate of the publishment. if self.rate: rospy.sleep(1/self.rate) else: rospy.sleep(1.0) self.publish_onShutdown()
class Ulink: msg = dict() CONT = dict() imuMsg = Imu() poseMsg = PoseWithCovarianceStamped() navvelMsg = TwistWithCovarianceStamped() pose_navMsg = Odometry() altMsg = Odometry() compassMsg = MagneticField() remoteMsg = Joy() baroMsg = FluidPressure() tempMsg = Temperature() gpsrawMsg = NavSatFix() desire_navMSG = Odometry() ackMsg = Ack() sonarMsg = Range() NavMsg = Navdata() #rospy.init_node('imu_node2', anonymous=True) time_start = rospy.Time.now() imuMsg.orientation_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1] imuMsg.angular_velocity_covariance = [ 1.2184696791468346e-7, 0, 0, 0, 1.2184696791468346e-7, 0, 0, 0, 1.2184696791468346e-7 ] imuMsg.linear_acceleration_covariance = [ 8.99999999999e-8, 0, 0, 0, 8.99999999999e-8, 0, 0, 0, 8.99999999999e-8 ] #u = serial.Serial() def __init__(self, ser): try: self.s = ser print "Conecting.." except ValueError: print "Cannot connect the Port" if self.s.isOpen(): print "Conected" time.sleep(2) ## self.u.write('X') ## print 'wait start..' ## while (ord(self.u.read(1)) != 0xa1) : ## time.sleep(0.1) ## 'wait start cmd' ## time.sleep(0.2) #print "sending data request" #self.send_data_request(self.s,DATA_ATT) #self.send_data_request(self.s,DATA_GPS) print "Starting message handler thread" self.msg['STATUS'] = 'alive' thread.start_new_thread(self.MsgHandler, (self.s, )) #thread.start_new_thread(self.cmd_send,()) #self.send_cmd_takeoff(5) ## for i in range (0,10) : ## print self.u.write(chr(0xaa)) def MsgHandler(self, s): while s.isOpen(): if s.inWaiting() > 0: ch = s.read(1) #print ord(ch) if ord(ch) == 0xb5: hdr = ord(ch) lenp = ord(s.read(1)) idp = ord(s.read(1)) pay = s.read(lenp) chk = struct.unpack('h', s.read(2))[0] summ = 0 for c in pay: summ = summ + ord(c) if chk == hdr + lenp + idp + summ: #print 'chk ok' #print idp,lenp self.packet_decode(idp, pay, lenp) def wait_alive(self, s): 'nothing' def send_data_request(self, s, d): #hdr =0xb5 len =1 id = 6 chk = 181 + 1 + DATA_REQUEST + d data = struct.pack('cccch', chr(181), chr(1), chr(DATA_REQUEST), chr(d), chk) s.write(data) def send_cmd_takeoff(self, h): chk = 181 + 1 + CMD_TAKEOFF + h return self.s.write( struct.pack('cccch', chr(181), chr(1), chr(CMD_TAKEOFF), chr(h), chk)) def send_position_desire(self, px, py, pz, yaw): dlen = 16 head = struct.pack('ccc', chr(181), chr(dlen), chr(POSE_DEMAND)) data = struct.pack('ffff', px, py, pz, yaw) summ = 181 + dlen + POSE_DEMAND for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_flow(self, fx, fy, track): dlen = 10 head = struct.pack('ccc', chr(181), chr(dlen), chr(FLOW_DATA)) data = struct.pack('ffH', fx, fy, track) summ = 181 + dlen + FLOW_DATA for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_motor_desire(self, M1, M2, M3, M4): dlen = 16 head = struct.pack('ccc', chr(181), chr(dlen), chr(MOTOR_OUT)) data = struct.pack('ffff', M1, M2, M3, M4) summ = 181 + dlen + MOTOR_OUT for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def packet_decode(self, msgid, payload, lenp): #print 'get ',msgid t_now = rospy.Time.now() if msgid == ATTITUDE: #print 'id : ' ,idp d = struct.unpack('hhh', payload) #d=struct.unpack('iii',payload) self.msg.update({'ATTITUDE': d}) self.NavMsg.tm = 1000000 * (t_now.secs - self.time_start.secs) + ( t_now.nsecs - self.time_start.nsecs) / 1000 self.NavMsg.rotX = d[0] * 0.0572957 self.NavMsg.rotY = d[1] * 0.0572957 self.NavMsg.rotZ = d[2] * 0.0572957 q = tf.transformations.quaternion_from_euler( d[0] * 0.001, d[1] * 0.001, d[2] * 0.001) #roll pitch yaw (addition for use in rviz +p/2) self.imuMsg.header.stamp = t_now #-self.time_start self.imuMsg.orientation.x = q[0] self.imuMsg.orientation.y = q[1] self.imuMsg.orientation.z = q[2] self.imuMsg.orientation.w = q[3] self.poseMsg.pose.pose.orientation.x = self.imuMsg.orientation.x self.poseMsg.pose.pose.orientation.y = self.imuMsg.orientation.y self.poseMsg.pose.pose.orientation.z = self.imuMsg.orientation.z self.poseMsg.pose.pose.orientation.w = self.imuMsg.orientation.w #self.imuMsg.linear_acceleration.x = d[0] #self.imuMsg.linear_acceleration.y = d[1] #self.imuMsg.linear_acceleration.z = d[2] #print 'ATT ',d[0],d[1],d[2] # print 'ATTITUDE roll', str(d[0]*180/3.14)[:5] ,' pitch :' , str(d[1]*180/3.14)[:5] ,' yaw:' , str(d[2]*180/3.14)[:6] elif msgid == RAW_IMU: d = struct.unpack('hhhhhh', payload) self.msg.update({'RAW_IMU': d}) self.imuMsg.header.stamp = t_now self.imuMsg.linear_acceleration.x = d[0] * 9.80655 / 1000.0 self.imuMsg.linear_acceleration.y = d[1] * 9.80655 / 1000.0 self.imuMsg.linear_acceleration.z = d[2] * 9.80655 / 1000.0 self.imuMsg.angular_velocity.x = d[3] / 1000.0 self.imuMsg.angular_velocity.y = d[4] / 1000.0 self.imuMsg.angular_velocity.z = d[5] / 1000.0 self.compassMsg.header.stamp = t_now #-self.time_start # self.compassMsg.magnetic_field.x = d[6] # self.compassMsg.magnetic_field.y = d[7] # self.compassMsg.magnetic_field.z = d[8] self.NavMsg.ax = d[0] / 1000.00 self.NavMsg.ay = d[1] / 1000.00 self.NavMsg.az = d[2] / 1000.00 elif msgid == V_VAL: d = struct.unpack('hh', payload) #v_est_x v_est_y self.msg.update({'V_VAL': d}) self.pose_navMsg.twist.twist.linear.x = d[0] * 0.01 self.pose_navMsg.twist.twist.linear.y = d[1] * 0.01 #print 'r1:', str(d[0])[:4] ,'r2:' , str(d[1])[:4] elif msgid == RAW_BARO: d = struct.unpack('fff', payload) #alt_bybaro , pressure , temperature global baro_old global temp_old self.msg.update({'RAW_BARO': d}) # if baro_old == 0 or temp_old ==0: # baro_old = d[1] # temp_old = d[2] # else : # if abs(d[1]-baro_old)>2 or abs(d[2]-temp_old)>1: # self.poseMsg.pose.pose.position.x=1+self.poseMsg.pose.pose.position.x # else : # self.baroMsg.fluid_pressure = d[1] # self.tempMsg.temperature = d[2] # baro_old = d[1] # temp_old = d[2] #print 'BARODATA : ', str(d[1])[:7] ,' temp :' , str(d[2])[:7] ,' alt :' , str(d[0])[:7] elif msgid == RAW_RADIO: d = struct.unpack('HHHHHH', payload) self.msg.update({'RAW_RADIO': d}) self.remoteMsg.header.stamp = t_now #-self.time_start # self.remoteMsg.pose.position.x = d[0] # self.remoteMsg.pose.position.y = d[1] # self.remoteMsg.pose.position.z = d[2] # self.remoteMsg.pose.orientation.x = d[3] # self.remoteMsg.pose.orientation.y = d[4] # self.remoteMsg.pose.orientation.z = d[5] self.remoteMsg.axes = d[0:5] #self.send_motor_desire(d[2]/8,d[2]/8,d[2]/8,d[2]/8) #print 'r1:', str(d[0])[:4] ,'r2:' , str(d[1])[:4] ,'r3:' , str(d[2])[:4] ,'r4:' , str(d[3])[:4] ,'r5:' , str(d[4])[:4] ,'r6:' , str(d[5])[:4] elif msgid == MOTOR_OUT: d = struct.unpack('hhhh', payload) self.msg.update({'MOTOR_OUT': d}) #print str(d[0]), '\t', str(d[1]) ,'\t', str(d[2]),'\t', str(d[3]) elif msgid == CNT_VAL: d = struct.unpack('ffff', payload) self.msg.update({'CNT_VAL': d}) #print 'CT: ', str(d[0])[:4] ,'CR : ', str(d[1])[:4] ,' CP :' , str(d[2])[:4] ,' CY :' , str(d[3])[:4] # elif msgid == SONAR_DATA2_O : # d=struct.unpack('f',payload) # self.msg.update({'SONAR_DATA2_O': d}) # self.sonarMsg.header.stamp = t_now # self.sonarMsg.range = d[0] # print 'SONAR : ', str(d[0])[:4] elif msgid == STATUS_CHK: d = struct.unpack('hhhh', payload) self.msg.update({'STATUS_CHK': d}) print 'NUM SEN : ', str(d[0]), '--ARM : ', bool( d[1]), '--MODE : ', modes[d[2]](), '--RC : ', d[3] elif msgid == UNKNOWN_TYPE: print struct.unpack('%ds' % lenp, payload) #self.msg.update({'UNKOWN_TYPE': s}) elif msgid == UNKNOWN_FLOAT: d = struct.unpack('%df' % (lenp / 4), payload) ## a=(d) ## for i in range((lenp/4)) : ## a[i] = (round(a[i],5)) ## print a ## #self.msg.update({'UNKOWN_TYPE': s}) #print round(d[0]*100,5),'\t',round(d[1]*100,5),'\t',round(d[2]*100,5),'\t',round(d[3]*100,5) elif msgid == GPS: # north(cm) , east (cm) , v_est(cm) , v_north(cm) #print 'get gps' gps = struct.unpack('ffhh', payload) #self.msg.update({'GPS': (gps[0]/(1e7),gps[1]/(1e7))}) self.msg.update({'GPS': (gps[0], gps[1])}) #print 'GPS lat', gps[0] ,' lon :' , gps[1] self.poseMsg.header.stamp = t_now #-self.time_start self.poseMsg.pose.pose.position.x = gps[1] / 100.00 #to m self.poseMsg.pose.pose.position.y = gps[0] / 100.00 #ros UTM y is northing , x is easting self.navvelMsg.header.stamp = t_now #-self.time_start self.navvelMsg.twist.twist.linear.x = gps[2] / 100.00 self.navvelMsg.twist.twist.linear.y = gps[3] / 100.00 elif msgid == GPS_RAW_FIX: #lat,lng , numsat, hacc ,alt,fix_type,vacc #print 'get gps' gps_raw = struct.unpack('iiHHiHHH', payload) #self.msg.update({'GPS': (gps[0]/(1e7),gps[1]/(1e7))}) self.msg.update({ 'GPS_RAW_FIX': (gps_raw[0], gps_raw[1], gps_raw[2], gps_raw[3], gps_raw[4], gps_raw[5], gps_raw[6], gps_raw[7]) }) #lat lon numsat hacc alt fix vacc #print 'GPS lat', gps[0] ,' lon :' , gps[1] self.gpsrawMsg.header.stamp = t_now #-self.time_start self.gpsrawMsg.latitude = gps_raw[0] / 10000000.00000000 #deg self.gpsrawMsg.longitude = gps_raw[1] / 10000000.00000000 self.gpsrawMsg.status.status = -1 if gps_raw[5] == 0 else 1 self.gpsrawMsg.position_covariance = [ gps_raw[3] * 0.01, 0, 0, 0, gps_raw[3] * 0.01, 0, 0, 0, gps_raw[6] * 0.01 ] self.poseMsg.pose.covariance = [ gps_raw[3] * 0.01, 0, 0, 0, 0, 0, 0, gps_raw[3] * 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 ] #chamge covariance term z position and z_vel to covariance of baro # [gps_raw[3], 0, 0, 0, 0, 0, # 0, gps_raw[3],0, 0, 0, 0, # 0, 0, gps_raw[6],0, 0, 0, # 0, 0, 0, 0.01, 0, 0, # 0, 0, 0, 0, 0.01, 0, # 0, 0, 0, 0, 0, 0.1] self.navvelMsg.twist.covariance = [ gps_raw[7] * 0.01, 0, 0, 0, 0, 0, 0, gps_raw[7] * 0.01, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.gpsrawMsg.status.service = gps_raw[2] self.gpsrawMsg.altitude = gps_raw[4] / 1000.00000000 self.gpsrawMsg.position_covariance_type = 2 elif msgid == NAV_DATA: d = struct.unpack('ff', payload) self.msg.update({'NAV_DATA': d}) self.pose_navMsg.header.stamp = t_now self.pose_navMsg.pose.pose.position.x = d[0] #to m self.pose_navMsg.pose.pose.position.y = d[1] #print 'nav x:', d[0] ,'nav y:' , d[1] elif msgid == ALTITUDE: d = struct.unpack('fh', payload) self.msg.update({'ALTITUDE': d}) # self.poseMsg.pose.pose.position.z = d[0] self.pose_navMsg.pose.pose.position.z = d[0] # self.poseMsg.header.stamp = t_now#-self.time_start # self.poseMsg.pose.pose.position.z = d[0] self.altMsg.header.stamp = t_now #-self.time_start self.altMsg.pose.pose.position.z = d[0] self.altMsg.twist.twist.linear.z = d[1] / 100.000 #to m/s self.altMsg.pose.covariance = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.altMsg.twist.covariance = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.navvelMsg.twist.twist.linear.z = d[1] / 100.000 #to m/s # print d[0]*1000 self.NavMsg.altd = int(d[0] * 1000) #report in mm # print 'ALT rel_ALT : ', str(d[0]*100)[:4] ,' vel_z :' , str(d[1]*10)[:4] elif msgid == CONT_COMPASS: compass = struct.unpack('hhhhhh', payload) self.CONT.update({'COMPASS': compass}) print 'Update Compass constance..' elif msgid == CONT_PID: pid = struct.unpack('HHHHHHHHHHHH', payload) self.CONT.update({'PID': pid}) print 'Update PID constance..' elif msgid == ACK: d = struct.unpack('H', payload)[0] ack = [] for i in range(16): ack.append(bool(d & (0x0001 << i))) print ack self.ackMsg.ACK_PENDING = ack[0] self.ackMsg.ACK_GCONFIG = ack[1] self.ackMsg.ACK_PID_RPY1 = ack[2] self.ackMsg.ACK_PID_RPY2 = ack[3] self.ackMsg.ACK_PID_ALT = ack[4] self.ackMsg.ACK_PID_NAV = ack[5] self.ackMsg.ACK_SPD = ack[6] self.ackMsg.ACK_NAV_INER = ack[7] self.ackMsg.ACK_MODE = ack[8] #del self.ack[:] print "....." elif msgid == DESIRE_NAV: d = struct.unpack('hhhhhhh', payload) self.msg.update({'DESIRE_NAV': d}) self.desire_navMSG.header.stamp = t_now #-self.time_start self.desire_navMSG.pose.pose.position.x = d[0] * 0.01 self.desire_navMSG.pose.pose.position.y = d[2] * 0.01 self.desire_navMSG.pose.pose.position.z = d[4] * 0.01 self.desire_navMSG.twist.twist.linear.x = d[1] * 0.01 self.desire_navMSG.twist.twist.linear.y = d[3] * 0.01 self.desire_navMSG.twist.twist.linear.z = d[5] * 0.01 q = tf.transformations.quaternion_from_euler(0, 0, d[6] * 0.01) self.desire_navMSG.pose.pose.orientation.x = q[0] self.desire_navMSG.pose.pose.orientation.y = q[1] self.desire_navMSG.pose.pose.orientation.z = q[2] self.desire_navMSG.pose.pose.orientation.w = q[3] def takeoff(self, h): return self.send_cmd_takeoff(h) def msg_print(self): print '---------------------------------------------' print self.msg print '---------------------------------------------' def send_pid1_param(self, data): #(6 float) dlen = 24 head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_RPY1)) data = struct.pack('ffffff', data[0], data[1], data[2], data[3], data[4], data[5]) summ = 181 + dlen + PID_RPY1 for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_pid2_param(self, data): #(6 float) dlen = 24 head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_RPY2)) data = struct.pack('ffffff', data[0], data[1], data[2], data[3], data[4], data[5]) summ = 181 + dlen + PID_RPY2 for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_pid_alt_param(self, data): #(5 float) dlen = 20 head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_ALT)) data = struct.pack('fffff', data[0], data[1], data[2], data[3], data[4]) summ = 181 + dlen + PID_ALT for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_pid_nav_param(self, data): #(5 float) dlen = 20 head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_NAV)) data = struct.pack('fffff', data[0], data[1], data[2], data[3], data[4]) summ = 181 + dlen + PID_NAV for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_spd_param(self, data): #(2 float) dlen = 8 head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_SPD)) data = struct.pack('ff', data[0], data[1]) summ = 181 + dlen + PID_SPD for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_nav_iner_param(self, data): #(5 float) dlen = 20 head = struct.pack('ccc', chr(181), chr(dlen), chr(NAV_INER)) data = struct.pack('fffff', data[0], data[1], data[2], data[3], data[4]) summ = 181 + dlen + NAV_INER for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_mode_param(self, data): #(6 float) dlen = 24 head = struct.pack('ccc', chr(181), chr(dlen), chr(MODE_CFG)) data = struct.pack('ffffff', data[0], data[1], data[2], data[3], data[4], data[5]) summ = 181 + dlen + MODE_CFG for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) return tatal def send_vision_data(self, datav): #(3 float 1 uint) dlen = 14 head = struct.pack('ccc', chr(181), chr(dlen), chr(VISION_DATA)) data = struct.pack('fffH', datav[0], datav[1], datav[2], datav[3]) summ = 181 + dlen + VISION_DATA for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) print 'p x:', datav[0], 'p y:', datav[1] return tatal def send_msf_data(self, datam): #(3 float 1 uint) dlen = 24 head = struct.pack('ccc', chr(181), chr(dlen), chr(MSF_DATA)) data = struct.pack('ffffff', datam[0], datam[1], datam[2], datam[3], datam[4], datam[5]) summ = 181 + dlen + MSF_DATA for d in str(data): summ += ord(d) chk = struct.pack('h', summ) tatal = len(head) + len(data) + len(chk) self.s.write(head) self.s.write(data) self.s.write(chk) # print 'p x:', datam[0] ,'p y:' , datam[1] return tatal
#!/usr/bin/env python import rospy import signal import os from sensor_msgs.msg import Range from geometry_msgs.msg import Twist rospy.init_node('obs_avoid', anonymous=True) p = rospy.Publisher('ddrobot/cmd_vel', Twist, queue_size=10) r_data = Range() l_data = Range() def right(data): global r_data r_data = data def left(data): global l_data l_data = data def do(): global r_data, l_data command = Twist() if r_data.range < 3 and l_data.range > 3: command.angular.z = 1 elif r_data.range > 3 and l_data.range < 3: command.angular.z = -1
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()
import rospy import tf from sensor_msgs.msg import Range # <2> if __name__ == '__main__': sensor = GroveUltrasonicRanger() # <4> rospy.init_node('ranger') pub = rospy.Publisher('distance', Range, queue_size=10) # create default message of type sensor_msg/Range with default values # http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Range.html msg = Range() msg.header.frame_id = "ranger_distance" # http://wiki.seeedstudio.com/Grove-Ultrasonic_Ranger/#specification msg.field_of_view = 0.261799 # rad (15 deg) msg.min_range = 0.02 # m msg.max_range = 3.5 # m rate = rospy.Rate(10.0) # <5> while not rospy.is_shutdown(): # <6> range = sensor.measureDistance() # update time stamp of default message msg.header.stamp = rospy.Time.now() # update measured distance to obstacle msg.range = range
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)
rlist, _, _ = select.select([ ser ], [], [], 1.0) if not rlist: continue new = ser.read(ser.inWaiting()) buf += new if brk in new: msg, buf = buf.split(brk)[-2:] yield msg if __name__ == '__main__': #global ser rospy.init_node('sonarmite') range_pub = rospy.Publisher('depth', Range) quality_pub = rospy.Publisher('quality', Float32) range_msg = Range(radiation_type=Range.ULTRASOUND, field_of_view=0.26, min_range=0.2, max_range=100.0) range_msg.header.frame_id = "sonar" port = rospy.get_param('~port', '/dev/ttyUSB0') baud = rospy.get_param('~baud', 9600) quality_threshold = rospy.get_param('~quality_threshold', 0.54) rospy.on_shutdown(_shutdown) try: ser = serial.Serial(port=port, baudrate=baud, timeout=.5) lines = serial_lines(ser) while not rospy.is_shutdown(): data = lines.next()
#!/usr/bin/env python import rospy from sensor_msgs.msg import Range from geometry_msgs.msg import Twist from math import atan, pi, sin threshold = 0.5 ob1 = Twist() psd1L = Range() psd1R = Range() psd2L = Range() psd2R = Range() X1_slope = 0.00 slope_angle = 0.00 X2_o = 0.79 X2_T = 0.02 X1_o = 0.95 X1_T = 0.02 alpha = 45 L = 0.1326 def callback1(msg): #X1 global psd1L, X1_slope psd1L = msg.range psd1L = round(psd1L, 2) X1_slope = psd1L - X1_o - X1_T + X2_o - X2_T
def __init__(self): ''' This class is used to transmit data from morse node to a transparent node used in ros airship you will have to modify it. ''' # Get the ~private namespace parameters from command line or launch file. init_message = rospy.get_param('~message', 'hello') self.rate = float(rospy.get_param('~rate', '30.0')) self.rate_before_sleep = self.rate self.topic_root = rospy.get_param('~topic', 'odometry') self.topic_imu = rospy.get_param('~topic_imu', 'imu') self.topic_hardcom = rospy.get_param('~topic_hardcom', 'hardcom') self.topic_camcom = rospy.get_param('~topic_camcom', 'cameracom') self.topic_command = rospy.get_param('~topic_command', 'command') rospy.loginfo("airship/" + self.topic_root + "/rate = " + str(self.rate) + "Hz") #Defining published messages ''' RANGE FIELDS: Header header uint8 ULTRASOUND=0 uint8 INFRARED=1 uint8 radiation_type float32 field_of_view float32 min_range float32 max_range float32 range ''' self.range = Range() ''' IMU FIELDS: Header header geometry_msgs/Quaternion orientation float64[9] orientation_covariance # Row major about x, y, z axes geometry_msgs/Vector3 angular_velocity float64[9] angular_velocity_covariance # Row major about x, y, z axes geometry_msgs/Vector3 linear_acceleration float64[9] linear_acceleration_covariance # Row major x, y z ''' self.imu = Imu() ''' IMAGE FIELDS Header header uint32 height # image height, that is, number of rows uint32 width # image width, that is, number of columns string encoding # Encoding of pixels -- channel meaning, ordering, size # taken from the list of strings in include/sensor_msgs/image_encodings.h uint8 is_bigendian # is this data bigendian? uint32 step # Full row length in bytes uint8[] data # actual matrix data, size is (step * rows) ''' self.image = Image() self.camera_info = CameraInfo() ''' ODOMETRY FIELDS: std_msgs/Header header uint32 seq time stamp string frame_id string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[36] covariance geometry_msgs/TwistWithCovariance twist geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z float64[36] covariance ''' self.estimated_pose = Odometry() ''' Header header uint32 seq time stamp string frame_id geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w ''' self.real_pose = PoseStamped() self.is_sleep_state = False #Defining the subscriber rospy.Subscriber(self.topic_hardcom+"/LidarRange", Range, self.call_range) rospy.Subscriber(self.topic_imu+"/InertialData", Imu, self.call_imu) rospy.Subscriber(self.topic_camcom+"/CameraImg", Image, self.call_image) rospy.Subscriber(self.topic_camcom+"/CameraInfo", CameraInfo, self.call_camera_info) rospy.Subscriber(self.topic_command+"/isSleep", Bool, self.call_sleep) rospy.Subscriber("raw/pose", PoseStamped, self.call_pose) #Defining all the publisher self.estimated_pose_pub = rospy.Publisher(self.topic_root+"/EstimatedPose", Odometry, queue_size=10) while not rospy.is_shutdown(): if not self.is_sleep_state: ####################################### # This is where we can compute the estimated # pose ####################################### self.estimated_pose_pub.publish(self.estimated_pose) #this is the rate of the publishment. if self.rate: rospy.sleep(1/self.rate) else: rospy.sleep(1.0) self.publish_onShutdown()