Skip to content
This repository has been archived by the owner on Jul 17, 2019. It is now read-only.

Commit

Permalink
block updates
Browse files Browse the repository at this point in the history
  • Loading branch information
@nzlz
nzlz committed Apr 15, 2016
1 parent c88a919 commit dd3205b
Show file tree
Hide file tree
Showing 4 changed files with 115 additions and 90 deletions.
4 changes: 2 additions & 2 deletions blocks/erle-spider.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,8 +92,8 @@ Blockly.Blocks['spider_turn_degrees'] = {

Blockly.Blocks['spider_avoid_obstacles'] = {
init: function() {
this.appendDummyInput()
.appendField("Avoid Obstacles");
this.appendValueInput("path_angle")
.appendField("Find path");
this.setPreviousStatement(true);
this.setNextStatement(true);
this.setColour(260);
Expand Down
5 changes: 4 additions & 1 deletion generators/python/erle-spider.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,8 +74,11 @@ Blockly.Python['spider_turn_degrees'] = function(block) {
};

Blockly.Python['spider_avoid_obstacles'] = function(block) {

var varName = Blockly.Python.valueToCode(block, 'path_angle', Blockly.Python.ORDER_ATOMIC);

var code = "\n";
code += Blockly.readPythonFile("../blockly/generators/python/scripts/spider/avoid_obstacles.py");
return code;
return code + varName + " = path_center_degrees\n"

};
36 changes: 21 additions & 15 deletions generators/python/scripts/brain/start_slam.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,30 +17,36 @@
ip_add = '192.168.0.10'
command='rosrun urg_node urg_node _ip_address:=' + ip_add
process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE)

if not '/hector_mapping' in ros_nodes:
command='roslaunch hector_mapping mapping_default.launch'
process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE)

if not '/hector_geotiff_node' in ros_nodes:
command='roslaunch hector_geotiff geotiff_mapper.launch'
process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE)

time.sleep(3)
#time.sleep(3)
rospack = rospkg.RosPack()
tiff_path = rospack.get_path('robot_blockly') + '/frontend/pages/maps/'

while '/hector_mapping' in ros_nodes:
#os.system('rostopic pub -r 0.2 syscommand std_msgs/String "savegeotiff"')
os.system('rostopic pub -1 syscommand std_msgs/String "savegeotiff"')
#while '/hector_mapping' in ros_nodes:
rate = rospy.Rate(10)
pub_img = rospy.Publisher('syscommand', String, queue_size=10, latch=2)
pub_img.publish("savegeotiff")

con1 = pub_img.get_num_connections()
#while not pub_img.get_num_connections() > con1:
#while pub_img.get_num_connections() == 0:
# rate.sleep()

time.sleep(3)
for img in glob.glob(tiff_path+"*.tif"):
im_name = img.replace('tif', 'png')
cv_img = cv2.imread(img)
cv2.imwrite(im_name, cv_img)
os.system("ls -t "+tiff_path+"*tfw | tail -n +2 | xargs rm --")#remove all tfw but latest
os.system("ls -t "+tiff_path+"*tif | tail -n +2 | xargs rm --")#remove all tif but latest
os.system("ls -t "+tiff_path+"*png | tail -n +2 | xargs rm --")#remove all png but latest
#os.system('rostopic pub -1 syscommand std_msgs/String "savegeotiff"')
print("creating png")
for img in glob.glob(tiff_path+"*.tif"):
im_name = img.replace('tif', 'png')
cv_img = cv2.imread(img)
cv2.imwrite(im_name, cv_img)
print("removing old files")
os.system("ls -t "+tiff_path+"*tfw | tail -n +2 | xargs rm --")#remove all tfw but latest
os.system("ls -t "+tiff_path+"*tif | tail -n +2 | xargs rm --")#remove all tif but latest
os.system("ls -t "+tiff_path+"*png | tail -n +2 | xargs rm --")#remove all png but latest

ros_nodes = rosnode.get_node_names()
#ros_nodes = rosnode.get_node_names()
160 changes: 88 additions & 72 deletions generators/python/scripts/spider/avoid_obstacles.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,79 +31,95 @@
for e in range (0, 17):
msg.buttons.append(valueButton)

rad_to_deg = 57.2958
###################
## LASER PARAMS. ##
###################
laser = rospy.wait_for_message('/scan', LaserScan, timeout=3)

range_min = laser.range_min #hokuyo 0.019, sick 0.05
range_max = laser.range_max #hoyuko 60, sick 25
angle_increment = laser.angle_increment #hokuyo 0.0043, sick 0.005817
mid_angle = laser.angle_max*rad_to_deg #hokuyo 135, sick 135

################
## AVOID OBS. ##
################
while not rospy.is_shutdown():

laser = rospy.wait_for_message('/scan', LaserScan, timeout=3)

path_distance = 1.2
obstacle_distance = 0.3

path_size = 0
path_beg = 0
path_end = 0
MAX_path_size = 0
MAX_path_beg = 0
MAX_path_end = 0

#range_min = laser.range_min #0.05
range_min = 0.2
range_max = laser.range_max #25.0


#create a list of tuples with valid values
valid_ranges = []
stop = 0
for (i,r) in enumerate(laser.ranges):
if (r >= range_min) and (r <= range_max):
tup = [i,r]
valid_ranges.append(tup)

for (j,w) in enumerate(valid_ranges): #w is a tuple of [i,r]
#print(str(j)+" "+str(w[1]))
if w[1] > path_distance:
if valid_ranges[j-1][1] > path_distance:
#path_size += 1
path_size += w[0] - valid_ranges[j-1][0]
path_end = j
if path_size > MAX_path_size:
MAX_path_size = path_size
MAX_path_beg = path_beg
MAX_path_end = path_end
else:
path_beg = j
path_size = 0
elif w[1] <= obstacle_distance:
stop = 1
print("STOP - "+str(j)+" "+str(w[1]))



path_center = (MAX_path_end+MAX_path_beg)/2
path_center_degrees = path_center*0.005817*57.2958
print("NEW ITERATION")
print("MAX_path_size="+str(MAX_path_size))
print("MAX_path_beg="+str(MAX_path_beg))
print("MAX_path_end="+str(MAX_path_end))
print("Path center="+str(path_center))
print("DEGREES path_center="+str(path_center_degrees))
print("\n")

##QUITAR VALORES ERRORENOS DE 'RANGE' ANTES DE TRATAR
#RANGE sin tratar -> 810-811 valores
#angulo total 270 grados, 2.3557*2*57.2958

if stop == 0:
if path_center_degrees < 135: #left
msg.axes[1] = 1 #forward
msg.axes[2] = 1 #turn left
else: #right
msg.axes[1] = 1 #forward
msg.axes[2] = -1 #turn right
else: #stop == 1
msg.axes[1] = 0
#while not rospy.is_shutdown():

laser = rospy.wait_for_message('/scan', LaserScan, timeout=3)

path_distance = 1.2
obstacle_distance = 0.2

path_size = 0
path_beg = 0
path_end = 0
MAX_path_size = 0
MAX_path_beg = 0
MAX_path_end = 0

#range_min = laser.range_min #0.05
###range_min = 0.2
###range_max = laser.range_max #25.0


#create a list of tuples with valid values
valid_ranges = []
stop = 0
for (i,r) in enumerate(laser.ranges):
if (r >= range_min) and (r <= range_max):
tup = [i,r]
valid_ranges.append(tup)

for (j,w) in enumerate(valid_ranges): #w is a tuple of [i,r]
if w[1] > path_distance:
if valid_ranges[j-1][1] > path_distance:
path_size += w[0] - valid_ranges[j-1][0]
path_end = j
if path_size > MAX_path_size:
MAX_path_size = path_size
MAX_path_beg = path_beg
MAX_path_end = path_end
else:
path_beg = j
path_size = 0
'''elif w[1] <= obstacle_distance:
stop = 1
print("STOP - "+str(j)+" "+str(w[1]))
#detectar fallo y paralelo a la pared?'''


path_center = (MAX_path_end+MAX_path_beg)/2
path_center_degrees = path_center*angle_increment*rad_to_deg
'''
print("NEW ITERATION")
print("MAX_path_size="+str(MAX_path_size))
print("MAX_path_beg="+str(MAX_path_beg))
print("MAX_path_end="+str(MAX_path_end))
print("Path center="+str(path_center))
print("DEGREES path_center="+str(path_center_degrees))
print("\n")
'''

'''
if stop == 0:
if path_center_degrees > (mid_angle -25) and path_center_degrees < (mid_angle +25):
print("GOING FORWARD")
msg.axes[2] = 0
pub.publish(msg)
rate.sleep()
msg.axes[3] = 1
elif path_center_degrees > mid_angle: #left
print("GOING LEFT")
msg.axes[3] = 1
msg.axes[2] = 1 #turn left
else: #right
print("GOING RIGHT")
msg.axes[3] = 1
msg.axes[2] = -1 #turn right
else: #stop == 1
msg.axes[2] = 0
msg.axes[3] = 0
pub.publish(msg)
rate.sleep()'''

0 comments on commit dd3205b

Please sign in to comment.