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