def sendRange(pub, radiation_type, r):
msg = Range()
msg.radiation_type = radiation_type
msg.range = r
pub.publish(msg)
#path_to_current_directory = str(pathlib.Path().parent.absolute())
#path_to_model = path_to_current_directory + "/models/"
#loaded_model = pickle.load(open(path_to_model + model, 'rb'))
loaded_model = pickle.load(open(path_to_model, 'rb'))
canopy_position = {0: 'center', 1: 'left', 2: 'none', 3: 'right'}
max_effective_spraying_distance = 0.8 # meters
min_effective_height_percentage = 0.3
max_effective_height_percentage = 0.6
effective_width_percentage = 0.3
max_depth_distance = 14.0 # meters
min_close_percentage = 0.3
fov_msg = Range()
fov_msg.header.frame_id = "spray_valve_link"
fov_msg.radiation_type = 1
fov_msg.field_of_view = 3.1415 / 3
fov_msg.min_range = 0
fov_msg.max_range = 1.0
valve_publisher = None
fov_pub = None
canopy_detected = False
on_the_move = False
valve = False
def usbCamCallback(msg):
#!/usr/bin/env python
from sensor_msgs.msg import Range
import rospy
topic = "test_range"
publisher = rospy.Publisher(topic, Range)
rospy.init_node("range_test")
dist = 3
while not rospy.is_shutdown():
r = Range()
r.header.frame_id = "base_link"
r.header.stamp = rospy.Time.now()
r.radiation_type = 0
r.field_of_view = 2.0 / dist
r.min_range = 0.4
r.max_range = 10
r.range = dist
publisher.publish(r)
rospy.sleep(0.1)
dist += 0.3
if dist > 10:
dist = 3
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 = self.sensor_max_range
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 = float('Inf')
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 = self.sensor_max_range
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 = self.sensor_max_range
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 = self.sensor_max_range
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 = self.sensor_max_range
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 = self.sensor_max_range
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 = self.sensor_max_range
self.data_SDD.header.stamp = rospy.Time.now()
self.data_SDD.header.frame_id = "SDD_frame"
#Tercer Mensaje
#SJO
self.data_SJO = Range()
self.data_SJO.radiation_type = self.data_SJO.ULTRASOUND #Ultrasound
self.data_SJO.field_of_view = self.field_of_view #rad
self.data_SJO.min_range = self.sensor_min_range
self.data_SJO.max_range = self.sensor_max_range
self.data_SJO.range = 1.0
self.data_SJO.header.stamp = rospy.Time.now()
self.data_SJO.header.frame_id = "SJO_frame"
#Arrays to calculate the median of the ultrasonic data
self.max_array_ultrasonic = 3
#First message
self.STD_array = [0] * self.max_array_ultrasonic
self.SDI_array = [0] * self.max_array_ultrasonic
self.SLIT_array = [0] * self.max_array_ultrasonic
self.SLDD_array = [0] * self.max_array_ultrasonic
self.i = 0
#Second message
self.SLDT_array = [0] * self.max_array_ultrasonic
self.STI_array = [0] * self.max_array_ultrasonic
self.SLID_array = [0] * self.max_array_ultrasonic
self.SDD_array = [0] * self.max_array_ultrasonic
self.j = 0
#Third message
self.SJO_array = [0] * self.max_array_ultrasonic
self.k = 0
def run(self):
# Connect to the ePuck
self._bridge.connect()
# Setup the necessary sensors.
self.setup_sensors()
# Disconnect when rospy is going to down
rospy.on_shutdown(self.disconnect)
self.greeting()
self._bridge.step()
# Subscribe to Commando Velocity Topic
rospy.Subscriber("mobile_base/cmd_vel", Twist, self.handler_velocity)
# Sensor Publishers
# rospy.Publisher("/%s/mobile_base/" % self._name, )
if self.enabled_sensors['camera']:
self.image_publisher = rospy.Publisher("camera", Image)
if self.enabled_sensors['proximity']:
for i in range(0, 8):
self.prox_publisher.append(
rospy.Publisher("proximity" + str(i), Range))
self.prox_msg.append(Range())
self.prox_msg[i].radiation_type = Range.INFRARED
self.prox_msg[
i].header.frame_id = self._name + "/base_prox" + str(i)
self.prox_msg[
i].field_of_view = 0.26 # About 15 degrees...to be checked!
self.prox_msg[i].min_range = 0.005 # 0.5 cm
self.prox_msg[i].max_range = 0.05 # 5 cm
if self.enabled_sensors['motor_position']:
self.odom_publisher = rospy.Publisher('odom', Odometry)
if self.enabled_sensors['accelerometer']:
self.accel_publisher = rospy.Publisher(
'accel', Imu) # Only "linear_acceleration" vector filled.
if self.enabled_sensors['selector']:
self.selector_publisher = rospy.Publisher('selector', Marker)
if self.enabled_sensors['light']:
self.light_publisher = rospy.Publisher('light', Marker)
if self.enabled_sensors['motor_speed']:
self.motor_speed_publisher = rospy.Publisher('motor_speed', Marker)
if self.enabled_sensors['microphone']:
self.microphone_publisher = rospy.Publisher('microphone', Marker)
if self.enabled_sensors['floor']:
self.floor_publisher = rospy.Publisher('floor', Marker)
# Spin almost forever
#rate = rospy.Rate(7) # 7 Hz. If you experience "timeout" problems with multiple robots try to reduce this value.
self.startTime = time.time()
while not rospy.is_shutdown():
self._bridge.step()
self.update_sensors()
#!/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
rospy.init_node('validation_node')
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
def __init__(self):
# Initialize variables and Take off
rospy.init_node('publish_test_data')
# sonarOrientation = rospy.Subscriber('/tf', Tran)
dataLeft = rospy.Publisher('/L_sensor', Range, queue_size=10)
dataRight = rospy.Publisher('/R_sensor', Range, queue_size=10)
dataFront = rospy.Publisher('/F_sensor', Range, queue_size=10)
scan_pub = rospy.Publisher('/fakeScan', LaserScan, queue_size=10)
self.sonarReadLeft = 20.0
self.sonarReadRight = 20.0
self.sonarReadFront = 20.0
rospy.Subscriber('/Left_sensor', Float32, self.setLeftSonarValue)
rospy.Subscriber('/Right_sensor', Float32, self.setRightSonarValue)
rospy.Subscriber('/Front_sensor', Float32, self.setFrontSonarValue)
self.pub_tf = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=1)
while not rospy.is_shutdown():
count = 0
# HRLV-MaxSonar-EZ MB1043 reports anything below 30 cm as 30 cm
# inaccurate readings closer than 50 cm
t = geometry_msgs.msg.TransformStamped()
t.header.frame_id = "base_frame"
t.header.stamp = rospy.Time.now()
t.child_frame_id = "sonar_left"
t.transform.translation.x = 0.0
t.transform.translation.y = 0.0
t.transform.translation.z = 0.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0
leftSonar = Range()
leftSonar.header.stamp = rospy.Time.now()
leftSonar.header.frame_id = "sonar_left"
leftSonar.radiation_type = Range.ULTRASOUND
leftSonar.min_range = 20.0
leftSonar.max_range = 500.0
leftSonar.field_of_view = 0.4
leftSonar.range = self.sonarReadLeft #This value should be the readings from the actual sonars
# print("publish data... ")
tfm = tf2_msgs.msg.TFMessage([t])
self.pub_tf.publish(tfm)
dataLeft.publish(leftSonar)
t.header.stamp = rospy.Time.now()
t.child_frame_id = "sonar_right"
t.transform.translation.x = 0.0
t.transform.translation.y = 0.0
t.transform.translation.z = 0.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 1.0
t.transform.rotation.w = 0.0
tfm = tf2_msgs.msg.TFMessage([t])
self.pub_tf.publish(tfm)
leftSonar.range = self.sonarReadRight
leftSonar.header.frame_id = "sonar_right"
dataRight.publish(leftSonar)
t.header.stamp = rospy.Time.now()
t.child_frame_id = "sonar_front"
t.transform.translation.x = 0.0
t.transform.translation.y = 0.0
t.transform.translation.z = 0.0
t.transform.rotation.x = -0.5
t.transform.rotation.y = 0.5
t.transform.rotation.z = -0.5
t.transform.rotation.w = 0.5
leftSonar.range = self.sonarReadFront
leftSonar.header.frame_id = "sonar_front"
dataFront.publish(leftSonar)
tfm = tf2_msgs.msg.TFMessage([t])
self.pub_tf.publish(tfm)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
try:
self.emergencybt_val = self.arduino.get_emergency_button()
self.emergencybt_pub.publish(self.emergencybt_val)
except:
self.emergencybt_val = -1
self.emergencybt_pub.publish(-1)
rospy.logerr("get_emergency_button except")
return
try:
self.voltage_val = self.arduino.get_voltage()
self.voltage_pub.publish(self.voltage_val)
self.voltage_percentage_pub.publish(
self.volTransPerentage(self.voltage_val))
#rospy.loginfo("voltage_Perentage:" + str(self.volTransPerentage(self.voltage_val)))
except:
self.voltage_pub.publish(-1)
#self.voltage_str_pub.publish("")
rospy.logerr("get voltage exception")
if (self.useSonar == True):
pcloud = PointCloud2()
try:
self.sonar_r0, self.sonar_r1 = self.arduino.ping_o()
#rospy.loginfo("r0: " + str(self.sonar_r0) + " r1: " + str(self.sonar_r1))
sonar0_range = Range()
sonar0_range.header.stamp = now
sonar0_range.header.frame_id = "/sonar0"
sonar0_range.radiation_type = Range.ULTRASOUND
sonar0_range.field_of_view = 0.3
sonar0_range.min_range = 0.04
sonar0_range.max_range = 0.8
sonar0_range.range = self.sonar_r0 / 100.0
if sonar0_range.range == 0.0: #sonar error or not exist flag
sonar0_range.range = 1.0
elif sonar0_range.range >= sonar0_range.max_range:
#sonar0_range.range = float('Inf')
sonar0_range.range = sonar0_range.max_range
self.sonar0_pub.publish(sonar0_range)
if sonar0_range.range >= 0.5 or sonar0_range.range == 0.0:
self.sonar_cloud[0][
0] = self.sonar0_offset_x + self.sonar_maxval * math.cos(
self.sonar0_offset_yaw)
self.sonar_cloud[0][
1] = self.sonar0_offset_y + self.sonar_maxval * math.sin(
self.sonar0_offset_yaw)
else:
self.sonar_cloud[0][
0] = self.sonar0_offset_x + sonar0_range.range * math.cos(
self.sonar0_offset_yaw)
self.sonar_cloud[0][
1] = self.sonar0_offset_y + sonar0_range.range * math.sin(
self.sonar0_offset_yaw)
#rospy.loginfo("r0: " + str(sonar0_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[0][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[0][1]))
sonar1_range = Range()
sonar1_range.header.stamp = now
sonar1_range.header.frame_id = "/sonar1"
sonar1_range.radiation_type = Range.ULTRASOUND
sonar1_range.field_of_view = 0.3
sonar1_range.min_range = 0.04
sonar1_range.max_range = 0.8
sonar1_range.range = self.sonar_r1 / 100.0
if sonar1_range.range == 0.0: #sonar error or not exist flag
sonar1_range.range = 1.0
elif sonar1_range.range >= sonar0_range.max_range:
sonar1_range.range = sonar1_range.max_range
self.sonar1_pub.publish(sonar1_range)
if sonar1_range.range >= 0.5 or sonar1_range.range == 0.0:
self.sonar_cloud[1][
0] = self.sonar1_offset_x + self.sonar_maxval * math.cos(
self.sonar1_offset_yaw)
self.sonar_cloud[1][
1] = self.sonar1_offset_y + self.sonar_maxval * math.sin(
self.sonar1_offset_yaw)
else:
self.sonar_cloud[1][
0] = self.sonar1_offset_x + sonar1_range.range * math.cos(
self.sonar1_offset_yaw)
self.sonar_cloud[1][
1] = self.sonar1_offset_y + sonar1_range.range * math.sin(
self.sonar1_offset_yaw)
#rospy.loginfo("r1: " + str(sonar1_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[1][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[1][1]))
except:
self.bad_encoder_count += 1
rospy.logerr("ping_o exception count: " +
str(self.bad_encoder_count))
return
try:
self.sonar_r2, self.sonar_r3 = self.arduino.ping_p()
#rospy.loginfo("r2: " + str(self.sonar_r2) + " r3: " + str(self.sonar_r3))
sonar2_range = Range()
sonar2_range.header.stamp = now
sonar2_range.header.frame_id = "/sonar2"
sonar2_range.radiation_type = Range.ULTRASOUND
sonar2_range.field_of_view = 0.3
sonar2_range.min_range = 0.04
sonar2_range.max_range = 0.8
sonar2_range.range = self.sonar_r2 / 100.0
if sonar2_range.range == 0.0: #sonar error or not exist flag
sonar2_range.range = 1.0
elif sonar2_range.range >= sonar2_range.max_range:
sonar2_range.range = sonar2_range.max_range
self.sonar2_pub.publish(sonar2_range)
if sonar2_range.range >= 0.5 or sonar2_range.range == 0.0:
self.sonar_cloud[2][
0] = self.sonar2_offset_x + self.sonar_maxval * math.cos(
self.sonar2_offset_yaw)
self.sonar_cloud[2][
1] = self.sonar2_offset_y + self.sonar_maxval * math.sin(
self.sonar2_offset_yaw)
else:
self.sonar_cloud[2][
0] = self.sonar2_offset_x + sonar2_range.range * math.cos(
self.sonar2_offset_yaw)
self.sonar_cloud[2][
1] = self.sonar2_offset_y + sonar2_range.range * math.sin(
self.sonar2_offset_yaw)
#rospy.loginfo("r2: " + str(sonar2_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[2][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[2][1]))
sonar3_range = Range()
sonar3_range.header.stamp = now
sonar3_range.header.frame_id = "/sonar3"
sonar3_range.radiation_type = Range.ULTRASOUND
sonar3_range.field_of_view = 0.3
sonar3_range.min_range = 0.04
sonar3_range.max_range = 0.8
sonar3_range.range = self.sonar_r3 / 100.0
if sonar3_range.range == 0.0: #sonar error or not exist flag
sonar3_range.range = 1.0
elif sonar3_range.range >= sonar3_range.max_range:
sonar3_range.range = sonar3_range.max_range
self.sonar3_pub.publish(sonar3_range)
if sonar3_range.range >= 0.5 or sonar3_range.range == 0.0:
self.sonar_cloud[3][
0] = self.sonar3_offset_x + self.sonar_maxval * math.cos(
self.sonar3_offset_yaw)
self.sonar_cloud[3][
1] = self.sonar3_offset_y + self.sonar_maxval * math.sin(
self.sonar3_offset_yaw)
else:
self.sonar_cloud[3][
0] = self.sonar3_offset_x + sonar3_range.range * math.cos(
self.sonar3_offset_yaw)
self.sonar_cloud[3][
1] = self.sonar3_offset_y + sonar3_range.range * math.sin(
self.sonar3_offset_yaw)
#rospy.loginfo("r3: " + str(sonar3_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[3][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[3][1]))
except:
self.bad_encoder_count += 1
rospy.logerr("ping_p exception count: " +
str(self.bad_encoder_count))
return
try:
self.sonar_r4 = self.arduino.ping_q()
#rospy.loginfo("r4: " + str(self.sonar_r4))
sonar4_range = Range()
sonar4_range.header.stamp = now
sonar4_range.header.frame_id = "/sonar4"
sonar4_range.radiation_type = Range.ULTRASOUND
sonar4_range.field_of_view = 0.3
sonar4_range.min_range = 0.04
sonar4_range.max_range = 0.8
sonar4_range.range = self.sonar_r4 / 100.0
if sonar4_range.range == 0.0: #sonar error or not exist flag
sonar4_range.range = 1.0
elif sonar4_range.range >= sonar4_range.max_range:
sonar4_range.range = sonar4_range.max_range
self.sonar4_pub.publish(sonar4_range)
if sonar4_range.range >= 0.5 or sonar4_range.range == 0.0:
self.sonar_cloud[4][
0] = self.sonar4_offset_x + self.sonar_maxval * math.cos(
self.sonar4_offset_yaw)
self.sonar_cloud[4][
1] = self.sonar4_offset_y + self.sonar_maxval * math.sin(
self.sonar4_offset_yaw)
else:
self.sonar_cloud[4][
0] = self.sonar4_offset_x + sonar4_range.range * math.cos(
self.sonar4_offset_yaw)
self.sonar_cloud[4][
1] = self.sonar4_offset_y + sonar4_range.range * math.sin(
self.sonar4_offset_yaw)
#rospy.loginfo("r4: " + str(sonar4_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[4][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[4][1]))
except:
self.bad_encoder_count += 1
rospy.logerr("ping_q exception count: " +
str(self.bad_encoder_count))
return
#rospy.loginfo("r0: " + str(self.sonar_r0) + " r1: " + str(self.sonar_r1) + " r2: "+str(self.sonar_r2)+ " r3: " + str(self.sonar_r3)+ " r4: " + str(self.sonar_r4))
pcloud.header.frame_id = "/base_footprint"
pcloud = pc2.create_cloud_xyz32(pcloud.header,
self.sonar_cloud)
self.sonar_pub_cloud.publish(pcloud)
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
#rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc))
self.lEncoderPub.publish(left_enc)
self.rEncoderPub.publish(right_enc)
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
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.
if (self.useImu == False):
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
odom.pose.covariance = ODOM_POSE_COVARIANCE
odom.twist.covariance = ODOM_TWIST_COVARIANCE
self.odomPub.publish(odom)
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
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
#!/usr/bin/env python
import rospy
import math
import tf
from sensor_msgs.msg import LaserScan, Range
from sensor_msgs.msg import Range
Scan_msg = LaserScan()
Range_msg = Range()
LaserFrequency = 5 # frequency of scanning by range sensors.
pub = rospy.Publisher('laser/laser_scan', LaserScan, queue_size=10)
Scan_msg.header.frame_id = 'RangeLaserFrame'
Scan_msg.angle_min = 0
Scan_msg.angle_max = 2*math.pi
Scan_msg.angle_increment = 2*math.pi/6
#Scan_msg.scan_time = 0.5
Scan_msg.range_min = 0.10000000149
Scan_msg.range_max = 30
Scan_msg.intensities = []
RangesTable = []
for lA in range(0,8):
def __init__(self):
rospy.init_node("range_array_converter", anonymous=True)
# Init of variables
self.tf_list = []
self.offsets = []
self.number_of_pos = 8
self.scan_init = False
self.pt_cld_init = False
self.number_of_sensors = 8
self.publishers = []
try:
ns = rospy.get_namespace()
if isinstance(ns, str):
sep = '/'
self.ns_prefix = sep.join(
[x for x in (ns).split(sep) if len(x) > 0])
else:
rospy.logwarn(
"Error when fetching namespace, pefix \"\" will be used instead"
)
except KeyError:
self.ns_prefix = ""
rospy.logwarn(
"Error when fetching namespace, pefix \"\" will be used instead"
)
# Getting mode param
try:
self.mode = rospy.get_param("~converter_mode")
except KeyError:
self.mode = "laser_scan"
rospy.logwarn("Private parameter 'converter_mode' is not set."
" Default value 'laser_scan' will be used instead.")
self.range_array_topic = "ranges"
# Getting sensor_mask
try:
self.sensor_mask = rospy.get_param("~sensor_mask")
except KeyError:
self.sensor_mask = [True] * self.number_of_sensors
rospy.logwarn("Private parameter 'sensor_mask' is not set."
" All sensors will be used for conversion")
# Getting conversion frame
try:
self.conversion_frame = rospy.get_param("~conversion_frame")
rospy.logwarn(
"By specifying the conversion frame you will override the automatic namespacing of the conversion_frame"
)
except KeyError:
if self.ns_prefix != "":
self.conversion_frame = "base_" + self.ns_prefix
else:
self.conversion_frame = "base_hub"
rospy.logwarn(
"Private parameter 'conversion_frame' is not set."
" Default value 'base + _<namespace>' will be used instead.")
# Getting refresh transform parameter
try:
self.force_tf_refresh = rospy.get_param("~force_tf_refresh")
if self.force_tf_refresh:
rospy.logwarn("WARNING: The transforms are going to be "
"refreshed for each conversion. This may impact "
"performance")
except KeyError:
self.force_tf_refresh = False
rospy.logwarn("Private parameter 'force_tf_refresh' is not set."
" Default value 'False' will be used instead.")
# Creating the right publisher
if self.mode == "laser_scan":
self.publisher = rospy.Publisher("scan", LaserScan, queue_size=1)
elif self.mode == "point_cloud":
self.publisher = rospy.Publisher("point_cloud",
PointCloud2,
queue_size=1)
elif self.mode == "sequential_ranges":
self.publisher = rospy.Publisher("sequential_ranges",
Range,
queue_size=8)
elif self.mode == "individual_ranges":
for i in range(self.number_of_sensors):
if self.sensor_mask[i]:
self.publishers.append(
rospy.Publisher("range_" + str(i), Range,
queue_size=1))
else:
self.publishers.append(None)
else:
rospy.logwarn("Incorrect value for the parameter 'converter_mode'."
" Default value 'laser_scan' will be used instead.")
self.mode = "laser_scan"
self.publisher = rospy.Publisher("scan", LaserScan, queue_size=1)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
# Wait for tf before launching conversion
rospy.loginfo('Waiting for tf to become available')
if not self.tf_buffer.can_transform('base_link', 'base_link',
rospy.Duration(0),
rospy.Duration(60)):
rospy.logerr('Tf timeout for transform between base_link and '
'base_link')
exit()
rospy.Rate(0.33).sleep()
rospy.loginfo('Tf successfully received')
# Initializing callback
self.hub_subscriber = rospy.Subscriber(self.range_array_topic,
RangeArray,
self.parser_callback)
self.data = RangeArray()
self.data.ranges = [Range()] * self.number_of_sensors
top_topic = "top_camera_range"
front_topic = "front_camera_range"
s_range = 4
fov = pi/4
top_pub = rospy.Publisher(top_topic, Range, queue_size = 10)
front_pub = rospy.Publisher(front_topic, Range, queue_size = 10)
rospy.init_node('range_publisher')
r = rospy.Rate(10)
while not rospy.is_shutdown():
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = top_frame
rg_top = Range()
rg_top.header = h
rg_top.field_of_view = fov
rg_top.max_range = s_range
rg_top.min_range = s_range
rg_top.range = s_range
rg_top.radiation_type = Range.INFRARED
rg_front = copy.deepcopy(rg_top)
rg_front.header.frame_id = front_frame
top_pub.publish(rg_top)
front_pub.publish(rg_front)
r.sleep()
#!/usr/bin/env python
import serial, time, rospy
from sensor_msgs.msg import Range
from std_msgs.msg import Header
rospy.init_node('lidar_range')
pub = [None, None, None, None]
# 4개의 토픽 발행
for i in range(4):
name = 'scan' + str(i)
pub[i] = rospy.Publisher(name, Range, queue_size=1)
msg = Range()
h = Header()
h.frame_id = "sensorXY"
msg.header = h
msg.radiation_type = Range().ULTRASOUND
msg.min_range = 0.02 # 2cm
msg.max_range = 2.0 # 2m
msg.field_of_view = (30.0 / 180.0) * 3.14
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
# msg.range에 장애물까지의 거리를 미터 단위로 넣고 토픽을 발행한다.
msg.range = 0.4
for i in range(4):
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 self.STD_array=[0]*self.max_array_ultrasonic #Max 15 elements self.SDI_array=[0]*self.max_array_ultrasonic self.SLIT_array=[0]*self.max_array_ultrasonic self.SLDD_array=[0]*self.max_array_ultrasonic self.i=0 self.SLDT_array=[0]*self.max_array_ultrasonic self.STI_array=[0]*self.max_array_ultrasonic self.SLID_array=[0]*self.max_array_ultrasonic self.SDD_array=[0]*self.max_array_ultrasonic self.j=0
import rospy
import traceback
import sys
import signal
from VL53L0X.python.VL53L0X import VL53L0X
from VL53L0X.python.VL53L0X import VL53L0X_BETTER_ACCURACY_MODE
from sensor_msgs.msg import Range
sys.path.append('.')
rospy.init_node('distance_sensors') # public display name of the publisher
rate = rospy.Rate(10) # 10hz
range1Publisher = rospy.Publisher('/holly/range/front_left',
Range,
queue_size=5)
range1Message = Range()
range5Publisher = rospy.Publisher('/holly/range/front_right',
Range,
queue_size=5)
range5Message = Range()
range2Publisher = rospy.Publisher('/holly/range/rear', Range, queue_size=5)
range2Message = Range()
seq1 = 1
seq2 = 1
seq5 = 1
sensorSetupNeeded = 1
18 Vert spd Double Bestvel 8 73
19 Time Uint Time stamps 4 81
20 Crc Uint Crc 4 85
'''
'''
Field# Field name Field type Data description Bin bytes Bin Offest
1 Sync Ucahr 0x86 1 0
2 Data_lenth uchar 0x0a 1 1
3 Encoder_left short Encoder 2 2
4 Encoder_right Short Encoder 2 4
5 Time Uint Time stamps 4 6
6 Check_sum Uchar Checksum 1 10
'''
sound_range = Range()
sound_seq = 0
def range_pub(range_msg):
global sound_seq
sound_range.max_range = 3.50
sound_range.min_range = 0.290
sound_range.field_of_view = 30 * math.pi / 180
sound_range.radiation_type = 0
sound_range.header.stamp = rospy.Time.now()
sound_range.header.seq = sound_seq
sound_range.header.frame_id = 'range1'
sound_range.range = range_msg[0] / 1000.0
pub_range_1.publish(sound_range)
sound_range.header.frame_id = 'range2'
def _receive_crazyflie_data(self, timestamp, data, logconf):
# Catch all exceptions because otherwise the crazyflie
# library catches them all silently
try:
stamp = self._calculate_timestamp(timestamp)
if logconf.name == 'high_update_rate':
imu = Imu()
imu.header.stamp = stamp
imu.header.frame_id = 'quad'
imu.linear_acceleration.x = data['acc.x'] * 9.81
imu.linear_acceleration.y = data['acc.y'] * 9.81
imu.linear_acceleration.z = data['acc.z'] * 9.81
imu.orientation_covariance[0] = -1
imu.angular_velocity_covariance[0] = -1
variance = 6.0
imu.linear_acceleration_covariance[0] = variance
imu.linear_acceleration_covariance[4] = variance
imu.linear_acceleration_covariance[8] = variance
self._imu_publisher.publish(imu)
orientation = OrientationAnglesStamped()
orientation.header.stamp = stamp
orientation.data.roll = data[
'stabilizer.roll'] * math.pi / 180.0
orientation.data.pitch = data[
'stabilizer.pitch'] * math.pi / 180.0
# Zero out the yaw for now. Crazyflie has a problem with yaw calculation
# and having correct yaw is not critical for this target yet.
orientation.data.yaw = -0.0 * data[
'stabilizer.yaw'] * math.pi / 180.0
self._orientation_pub.publish(orientation)
if abs(orientation.data.roll) > 1.5 or abs(
orientation.data.pitch) > 1.5:
rospy.logwarn(
'Crazyflie FC Comms maximimum attitude angle exceeded')
self._crash_detected = True
elif logconf.name == 'medium_update_rate':
twist = TwistWithCovarianceStamped()
twist.header.stamp = stamp - rospy.Duration.from_sec(0.020)
twist.header.frame_id = 'heading_quad'
twist.twist.twist.linear.x = data['kalman_states.vx']
twist.twist.twist.linear.y = data['kalman_states.vy']
twist.twist.twist.linear.z = 0.0
variance = 0.0001
twist.twist.covariance[0] = variance
twist.twist.covariance[7] = variance
self._velocity_pub.publish(twist)
range_msg = Range()
range_msg.header.stamp = stamp - rospy.Duration.from_sec(0.050)
range_msg.radiation_type = Range.INFRARED
range_msg.header.frame_id = '/short_distance_lidar'
range_msg.field_of_view = 0.3
range_msg.min_range = 0.01
range_msg.max_range = 1.6
self._current_height = data['range.zrange'] / 1000.0
range_msg.range = self._current_height
if range_msg.range > 2.0:
self._over_height_counts = self._over_height_counts + 1
else:
self._over_height_counts = 0
if self._over_height_counts > 10:
rospy.logwarn(
'Crazyflie FC Comms maximimum height exceeded')
self._crash_detected = True
self._range_pub.publish(range_msg)
switch_msg = LandingGearContactsStamped()
switch_msg.header.stamp = stamp
pressed = data['range.zrange'] < 50
switch_msg.front = pressed
switch_msg.back = pressed
switch_msg.left = pressed
switch_msg.right = pressed
self._switch_pub.publish(switch_msg)
elif logconf.name == 'slow_update_rate':
battery = Float64Stamped()
battery.header.stamp = stamp
battery.data = data['pm.vbat']
self._vbat = data['pm.vbat']
self._battery_publisher.publish(battery)
mag = MagneticField()
mag.header.stamp = stamp
mag.magnetic_field.x = data['mag.x']
mag.magnetic_field.y = data['mag.y']
mag.magnetic_field.z = data['mag.z']
self._mag_pub.publish(mag)
else:
rospy.logerr(
'Crazyflie FC Comms received message block with unkown name'
)
except Exception as e:
rospy.logerr("Crazyflie FC Comms error receiving data: {}".format(
str(e)))
rospy.logerr(traceback.format_exc())
ser.write(bytes(0))
#ser.write(0x00)
ser.write(bytes(0))
#ser.write(0x01)
ser.write(bytes(1))
#ser.write(0x06)
ser.write(bytes(6))
Dist_Total = 0
Dist_L = 0
Dist_H = 0
distance = Range()
pub = rospy.Publisher('sonar_frontR_distance', Range, queue_size=10)
rospy.init_node('Lidar', anonymous=True)
while (True and not rospy.is_shutdown()):
while (ser.in_waiting >= 9 and not rospy.is_shutdown()):
#print (ser.read())
#time.sleep(0.1)
if ((b'Y' == ser.read()) and (b'Y' == ser.read())):
GPIO.output(LEDpin, GPIO.LOW)
Dist_L = ser.read()
Dist_H = ser.read()
Dist_Total = (ord(Dist_H) * 256) + (ord(Dist_L))
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Range
ultrasound = Range()
def ultrassond_callback(data):
global ultrasound
ultrasound.range = data.range
rospy.init_node('range_printer', anonymous=True)
rate = rospy.Rate(20)
ultrasound_sub = rospy.Subscriber('/mavros/distance_sensor/hrlv_ez4_pub',
Range, ultrassond_callback)
while not rospy.is_shutdown():
rospy.loginfo(ultrasound.range)
rate.sleep()
import numpy as np
import cv2
from numpy import *
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from sensor_msgs.msg import Imu ,Range
import tf
rospy.init_node('optical_flow', anonymous=False)
poseMsg = PoseWithCovarianceStamped()
imuMsg = Imu()
rangeMsg = Range()
x=0.0
y=0.0
cap = cv2.VideoCapture(1)
R = [[0 for x in range(3)] for x in range(3)]
def imucallback(data):
quaternion = (
data.orientation.x,
data.orientation.y,
data.orientation.z,
data.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
def __init__(self, conf={}):
"""STDRCircularSys.__init__
Arguments:
- conf: configuration dictionary
-- mass: point _mass_
-- sysdim: dimension of system, usually 1,2, or 3D
-- statedim: 1d pointmass has 3 variables (pos, vel, acc) in this model, so sysdim * 3
-- dt: integration time step
-- x0: initial state
-- order: NOT IMPLEMENT (control mode of the system, order = 0 kinematic, 1 velocity, 2 force)
"""
SMPSys.__init__(self, conf)
# state is (pos, vel, acc).T
# self.state_dim
if not hasattr(self, 'x0'):
self.x0 = np.zeros((self.dim_s_proprio, 1))
self.x = self.x0.copy()
self.cnt = 0
if not self.ros:
import sys
print(
"ROS not configured but this robot (%s) requires ROS, exiting"
% (self.__class__.__name__))
sys.exit(1)
# timing
self.rate = rospy.Rate(1 / self.dt)
# pub / sub
# actuator / motors
self.twist = Twist()
self.twist.linear.x = 0
self.twist.linear.y = 0
self.twist.linear.z = 0
self.twist.angular.x = 0
self.twist.angular.y = 0
self.twist.angular.z = 0
# odometry
self.odom = Odometry()
self.sonars = []
self.outdict = {
"s_proprio": np.zeros((self.dim_s_proprio, 1)),
"s_extero": np.zeros((self.dim_s_extero, 1))
}
self.subs["odom"] = rospy.Subscriber("/robot0/odom", Odometry,
self.cb_odom)
for i in range(3):
self.sonars.append(Range())
self.subs["sonar%d" % i] = rospy.Subscriber(
"/robot0/sonar_%d" % i, Range, self.cb_range)
self.pubs["cmd_vel"] = rospy.Publisher("/robot0/cmd_vel",
Twist,
queue_size=2)
# reset robot
self.reset()
self.smdict["s_proprio"] = np.zeros((self.dim_s_proprio, 1))
self.smdict["s_extero"] = np.zeros((self.dim_s_extero, 1))
#!/usr/bin/env python
from __future__ import division
import rospy
import VL53L1X
from sensor_msgs.msg import Range
rospy.init_node('vl53l1x')
# range_pub = rospy.Publisher('~range', Range, queue_size=5)
# TODO: why remmaping is not working?
range_pub = rospy.Publisher('mavros/distance_sensor/rangefinder_3_sub',
Range,
queue_size=10)
msg = Range()
msg.radiation_type = Range.INFRARED
msg.field_of_view = 0.471239
msg.min_range = 0
msg.max_range = 4
msg.header.frame_id = 'rangefinder'
tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
tof.open() # Initialise the i2c bus and configure the sensor
tof.start_ranging(
3) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
rospy.loginfo('vl53l1x: start ranging')
r = rospy.Rate(14)
while not rospy.is_shutdown():
