def nodeCallback():
# Retrieve number from keyboard input.
num = key.getNumber()
# Define the movement array.
delta = movement[num]
# Calculate new Pepper position
pos[0] = pos[0] + delta[0]
pos[1] = pos[1] + delta[1]
pos[2] = pos[2] + delta[2]
# Create the new message
message = location()
message.name = 'Pepper'
message.x = pos[0]
message.y = pos[1]
message.w = pos[2]
# Publish the new position into topic
pose_pub.publish(message)
# Preventing Pepper from getting crazy
crazy = True
while crazy:
if key.getNumber() != num:
crazy = False
def testRRT(rate):
pub = rospy.Publisher('sIA_target', location, queue_size=5)
msg = location()
msg.x = 3
msg.y = 0
msg.w = 0.5
while not rospy.is_shutdown():
print(msg)
pub.publish(msg)
rate.sleep()
def testSetPlacesLocation(rate):
pub = rospy.Publisher('sIA_set_places_location', location, queue_size=5)
msg = location()
msg.name = "Living Room"
msg.x = 2
msg.y = 2
msg.z = 0
msg.w = 2
while not rospy.is_shutdown():
print(msg)
pub.publish(msg)
rate.sleep()
def getPlaceLocationCallback(data):
name = data.name
with open('file.csv', 'rb') as locations_file:
locations_reader = csv.reader(locations_file,
delimiter=',',
quotechar='|')
for i in locations_reader:
response = location()
if name == i[0]:
response.x = i[1]
response.y = i[2]
response.z = 0
response.w = i[3]
else:
response.x = 0
response.y = 0
response.z = 0
response.w = 0
return response
def sIA_RRT_Node():
# ROS node initialization.
rospy.init_node('sIA_RRT', anonymous=True)
# Publishers declaration. Must declare every publisher as a global variable.
global point_pub, target, size, obstacleList, newTarget, newMap, newGoal, finishedMapCallback
newTarget = False
newMap = True
newGoal = False
finishedMapCallback = False
# Then, create the Publisher object for each publisher.
point_pub = rospy.Publisher('sIA_move_to_point', location, queue_size=10)
# Creating the requiered subscriptions to /map, /odom, /sIA_target topics.
obstacleList = []
rospy.Subscriber( '/map', OccupancyGrid, mapCallback )
rospy.Subscriber( '/odom', Odometry, odomCallback )
#rospy.Subscriber( '/sIA_target', location, targetCallback )
# size = 0.5
# for i in range(0, 10):
# x = rnd.random()*10
# y = rnd.random()*10
# obs = [x,y,size]
# obstacleList.append(obs)
rate = rospy.Rate(10)
# The RRT object declaration.
RRTobj = RapidlyRandomTrees([0, 0], [4, 0], obstacleList, [-2, 15] )
# While loop
while not rospy.is_shutdown():
#if newTarget or newMap:
#print("finishedMapCallback " + str(finishedMapCallback))
#print("dentro del if")
#newTarget = False
#newMap = False
#print("new map"+str(newMap))
#RRTobj.end = target
#RRTobj.start = currentPosition
if finishedMapCallback:
RRTobj.obstacleList = obstacleList
path = RRTobj.Planning(animation = False)
print(path)
finishedMapCallback = False
# Trying to iterate over path points.
point_num = 0
while point_num < len(path):
goal = path[len(path)-2]
goalLocation = location()
goalLocation.x = goal[0]
goalLocation.y = goal[1]
goalLocation.z = 0
goalLocation.w = 0
point_pub.publish(goalLocation)
#if newGoal:
#discretizamos el path
#newGoal = False
#print(path)
# goal =
#if m.sqrt((currentPosition[0]-goal[0])** + (currentPosition[1]-goal[1])**) < 0.10:
# newGoal = True
rate.sleep()
#rospy.spin()
#========================================================================================
# NODE START
# This is the decent way to start a ROS node in Python
#========================================================================================
if __name__ == "__main__":
try:
sIA_RRT_Node()
except rospy.ROSInterruptException:
print('An error has ocurred over sIA_RRT node')
pass