def map_callback(self, map_msg):
laser_sigma = self.laser_sigma_hit
self.map = map_msg
self.grid = np.zeros((map_msg.info.height, map_msg.info.width))
rospy.loginfo('building Likelihood map... ')
rospy.logwarn('using laser_sigma = %f', laser_sigma)
world_map = map_utils.Map(map_msg)
rospy.loginfo('building KDTree')
from sklearn.neighbors import KDTree
occupied_points = []
all_positions = []
for i in range(world_map.grid.shape[0]):
for j in range(world_map.grid.shape[1]):
all_positions.append(world_map.cell_position(i, j))
if world_map.grid[i, j] > .9:
occupied_points.append(world_map.cell_position(i, j))
kdt = KDTree(occupied_points)
rospy.loginfo('Constructing likelihood field from KDTree.')
likelihood_field = map_utils.Map(world_map.to_message())
dists = kdt.query(all_positions, k=1)[0][:]
probs = np.exp(-(dists**2) / (2 * laser_sigma**2))
likelihood_field.grid = probs.reshape(likelihood_field.grid.shape)
rospy.logwarn('Done building likelihood field')
self.likelihood_pub.publish(likelihood_field.to_message())
def map_callback(self, map_msg):
""" Create and publish the costmap. """
world_map = map_utils.Map(map_msg)
cost_map = map_utils.Map(map_msg)
# first inflate the obstacles...
dilation_size = int(self.robot_radius / world_map.resolution) * 2 + 1
rospy.loginfo("{}".format(dilation_size))
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (dilation_size,
dilation_size))
grid_occupied = np.zeros(world_map.grid.shape)
occupied_indices = world_map.grid == 100
grid_occupied[occupied_indices] = 1
rospy.loginfo("dilating costmap...")
grid_dilated = cv2.dilate(grid_occupied, kernel)
indices = np.logical_and(grid_dilated == 1, world_map.grid != -1)
# fully dilated grid...
cost_map.grid[grid_dilated == 1] = 100
rospy.loginfo('building KDTree')
from sklearn.neighbors import KDTree
occupied_points = []
all_positions = []
for i in range(cost_map.grid.shape[0]):
for j in range(cost_map.grid.shape[1]):
all_positions.append(cost_map.cell_position(i, j))
if cost_map.grid[i, j] == 100:
occupied_points.append(cost_map.cell_position(i, j))
kdt = KDTree(occupied_points)
dists = kdt.query(all_positions, k=1)[0][:]
probs = np.exp(-(dists**2) / (2 * self.sigma**2))
dist_costs = probs.reshape(cost_map.grid.shape) * 100
dist_costs = np.array(dist_costs, dtype='int8')
indices = np.logical_and(dist_costs > cost_map.grid,
dist_costs > 0)
cost_map.grid[indices] = dist_costs[indices]
# no cell should have zero cost...
cost_map.grid[cost_map.grid == 0] = 1
# import matplotlib.pyplot as plt
# plt.imshow(world_map.grid,interpolation='none')
# plt.show()
rospy.loginfo("publishing costmap...")
self.costmap_pub.publish(cost_map.to_message())
def __init__(self):
rospy.init_node('map_node')
self.map_msg = None
self.pos = None
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped,
self.position_callback)
seenmap_pub = rospy.Publisher('/seenmap', OccupancyGrid, queue_size=10)
self.tf_listener = tf.TransformListener()
while self.map_msg is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for map...")
rospy.sleep(1)
occmap = map_utils.Map(self.map_msg)
seenmap = map_utils.Map(width=self.map_msg.info.width,
height=self.map_msg.info.height,
resolution=self.map_msg.info.resolution,
origin_x=self.map_msg.info.origin.position.x,
origin_y=self.map_msg.info.origin.position.y)
while (self.pos is None and not rospy.is_shutdown()):
rospy.loginfo("Waiting for position...")
rospy.sleep(.1)
# set initial values of seenmap. -1 = unknown, 0 = unseen, 1 = seen
it = np.nditer(occmap.grid, flags=['multi_index'])
while not it.finished:
if it[0] < 0:
seenmap.grid[it.multi_index] = -1
elif it[0] == 0:
seenmap.grid[it.multi_index] = 0
else:
seenmap.grid[it.multi_index] = 1
it.iternext()
while not rospy.is_shutdown():
# draw triangle
pts = self.calc_triangle()
one_row, one_col = occmap._cell_index(pts[0], pts[1])
two_row, two_col = occmap._cell_index(pts[2], pts[3])
three_row, three_col = occmap._cell_index(self.pos.position.x,
self.pos.position.y)
triangle = np.array([[one_col, one_row], [two_col, two_row],
[three_col, three_row]], np.int32)
cv2.fillConvexPoly(seenmap.grid, triangle, 1)
# publish to /seenmap
seenmap_pub.publish(seenmap.to_message())
def __init__(self):
rospy.init_node('seenmap_server_a')
seenmap_pub = rospy.Publisher('/seenmap_a', OccupancyGrid, queue_size=5)
self.map_msg = None
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
while self.map_msg is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for map...")
rospy.sleep(1)
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind(("", 13008))
sock.listen(1)
sock, addr = sock.accept()
except Exception as e:
sock.close()
rospy.loginfo("SEENMAP_SERVER ERROR:")
rospy.loginfo(e)
sys.exit()
seenmap = map_utils.Map(width = self.map_msg.info.width,
height = self.map_msg.info.height,
resolution = self.map_msg.info.resolution,
origin_x = self.map_msg.info.origin.position.x,
origin_y = self.map_msg.info.origin.position.y)
while True:
compressed_message = self.recv_msg(sock)
message = zlib.decompress(compressed_message)
if not message is None:
seenmap.grid = pickle.loads(message)
og = seenmap.to_message()
seenmap_pub.publish(og)
def __init__(self):
rospy.init_node('random_nav')
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
self.map_msg = None
while self.map_msg is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for map...")
rospy.sleep(.1)
self.map = map_utils.Map(self.map_msg)
self.demo_map()
def __init__(self):
rospy.init_node('goal_client')
self.og = None
rospy.Subscriber('/seenmap', OccupancyGrid, self.seenmap_callback)
host = "134.126.125.237" # ip of server
port = 13006
BUFFER_SIZE = 1024
self.goal_reached = True
# set up socket
connected = False
count = 0
while not connected:
try:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((host, port))
connected = True
except Exception as e:
if count <= 10:
rospy.loginfo("FAILED TO CONNECT. RETRYING...")
connected = False
count += 1
rospy.sleep(1)
else:
rospy.loginfo("I QUIT")
sys.exit()
while self.og is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for seen_map...")
rospy.sleep(1)
rospy.loginfo("GOAL CLIENT ONLINE")
s.send("CLIENT READY")
while not rospy.is_shutdown():
self.seenmap = map_utils.Map(self.og)
try:
goal = pickle.loads(s.recv(BUFFER_SIZE))
if self.seen(goal):
s.send("SEEN")
else:
s.send("UNSEEN")
except Exception as e:
s.close()
rospy.loginfo("TCP_TEST ERROR:")
rospy.loginfo(e)
sys.exit()
def __init__(self):
super().__init__('random_nav')
self.subscription = self.create_subscription(OccupancyGrid, 'map',
self.map_callback, 10)
self.map_msg = None
# Need help
while self.map_msg is None:
rospy.loginfo("Waiting for map...")
rospy.sleep(.1)
self.map = map_utils.Map(self.map_msg)
self.demo_map()
def __init__(self):
rospy.init_node('random_nav')
self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.state_names = {}
self.state_names[GoalStatus.PENDING] = "PENDING"
self.state_names[GoalStatus.ACTIVE] = "ACTIVE"
self.state_names[GoalStatus.PREEMPTED] = "PREEMPTED"
self.state_names[GoalStatus.SUCCEEDED] = "SUCCEEDED"
self.state_names[GoalStatus.ABORTED] = "ABORTED"
self.state_names[GoalStatus.REJECTED] = "REJECTED"
self.state_names[GoalStatus.RECALLED] = "RECALLED"
self.state_names[GoalStatus.LOST] = "LOST"
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
self.map_msg = None
while self.map_msg is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for map...")
rospy.sleep(.1)
self.map = map_utils.Map(self.map_msg)
self.demo_map()
# REPEAT THE FOLLOWING UNTIL ROSPY IS SHUT DOWN:
#
# GENERATE A RANDOM GOAL LOCATION:
# *GENERATE RANDOM LOCATIONS WHERE X AND Y ARE BOTH
# IN THE RANGE [-10, 10].
# *CONTINUE GENERATING RANDOM GOAL LOCATIONS UNTIL ONE IS
# AT A FREE LOCATION IN THE MAP (see demo_map below)
#
# ATTEMPT TO NAVIGATE TO THE GOAL:
# * SEND THE DESIRED GOAL IN THE MAP COORDINATE FRAME
# (see provided action_nav.py file)
x_target = 0
y_target = 0
while not rospy.is_shutdown():
#while not self.map.get_cell(x_target, y_target) == 0:
x_target = random.uniform(-10, 10)
y_target = random.uniform(-10, 10)
if self.map.get_cell(x_target, y_target) == 0:
self.goto_point(x_target, y_target)
def __init__(self):
rospy.init_node('random_nav')
self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
self.map_msg = None
while self.map_msg is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for map...")
rospy.sleep(.1)
self.map = map_utils.Map(self.map_msg)
while not rospy.is_shutdown():
x_goal = 100000
y_goal = 100000
while self.map.get_cell(x_goal, y_goal) != 0:
x_goal = random.random() * 20.0 - 10.0
y_goal = random.random() * 20.0 - 10.0
rospy.loginfo("Random goal found: ({}, {})".format(x_goal, y_goal))
goal = self.goal_message(x_goal, y_goal, 0)
rospy.loginfo("Waiting for server.")
self.ac.wait_for_server()
rospy.loginfo("Sending goal.")
self.ac.send_goal(goal)
rospy.loginfo("Goal Sent.")
# Wait until the server reports a result.
self.ac.wait_for_result()
rospy.loginfo(self.ac.get_goal_status_text())
def callback(self, og):
seenmap = map_utils.Map(og)
message = pickle.dumps(seenmap.grid)
compressed_message = zlib.compress(message)
self.send_msg(compressed_message)
def __init__(self):
rospy.init_node("detect_node")
mins = 4
self.closeToVictimforPicture = False
self.takeOnePicture = False
self.finding = True
self.doNewVictim = True
self.finishedGoingHome = False
self.time = 60 * mins
self.startTime = None
self.ranTime = None
self.tf_listener = tf.TransformListener()
self.listOfVictims = []
self.bridge = CvBridge()
self.map_msg = None
self.imgCounter = 1
self.victim = Victim()
self.thresh_hold = 1
self.victim_hold = .3
self.curX = 2.86 #hardcoded based off of current intital_pose.yaml
self.curY = -1.77 #hardcoded based off of current intital_pose.yaml
self.listOfPoints = [(self.curX, self.curY)]
self.first = True
self.xpostive = 1
self.ypostive = 1
self.victim.id = 1
self.ID = 1
self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.state_names = {}
self.state_names[GoalStatus.PENDING] = "PENDING"
self.state_names[GoalStatus.ACTIVE] = "ACTIVE"
self.state_names[GoalStatus.PREEMPTED] = "PREEMPTED"
self.state_names[GoalStatus.SUCCEEDED] = "SUCCEEDED"
self.state_names[GoalStatus.ABORTED] = "ABORTED"
self.state_names[GoalStatus.REJECTED] = "REJECTED"
self.state_names[GoalStatus.RECALLED] = "RECALLED"
self.state_names[GoalStatus.LOST] = "LOST"
self.pub = rospy.Publisher('victim', Victim, queue_size=10)
self.twistPub = rospy.Publisher('/cmd_vel_mux/input/navi',
Twist,
queue_size=1)
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/camera/rgb/image_raw', Image, self.icallback)
rospy.Subscriber('/visualization_marker',
Marker,
self.detect_callback,
queue_size=1)
while self.map_msg is None and not rospy.is_shutdown():
#print "in while-loop1/init/Detector_Alvar/detect_alvar.py"
rospy.loginfo("Waiting for map...")
rospy.sleep(.1)
self.map = map_utils.Map(self.map_msg)
while self.startTime is None:
self.startTime = self.map_msg.header.stamp
self.count = 0
x_target = 0
y_target = 0
while not rospy.is_shutdown() and self.finding and len(
self.listOfVictims) != 4 and (
self.time -
(self.ranTime.secs - self.startTime.secs)) > 30:
x_target = random.uniform(-10, 10)
y_target = random.uniform(-10, 10)
if self.map.get_cell(x_target, y_target) == 0:
if self.checkPoint(x_target, y_target):
self.goto_point(x_target, y_target)
while (not self.finding and not rospy.is_shutdown()):
rospy.sleep(.1)
self.finishedGoingHome = True
self.goto_point(self.curX, self.curY)
print "done victims are here \n" + str(self.listOfVictims)
def costmap_callback(self, costmap):
"""
Get the costmap and covert it to a Map object.
"""
self.costmap = map_utils.Map(costmap)
def __init__(self):
rospy.init_node('goal_server')
teammate_marker_pub = rospy.Publisher('teammate_marker',
Marker,
queue_size=10)
self.map_msg = None
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
goal_pub = rospy.Publisher('/new_goal', MoveBaseGoal, queue_size=10)
rospy.Subscriber('/goal_reached', Bool, self.goal_reached_callback)
self.og = None
self.seenmap = None
rospy.Subscriber('/seenmap', OccupancyGrid, self.seenmap_callback)
# set up map for choosing goals
while self.map_msg is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for map...")
rospy.sleep(1)
self.map = map_utils.Map(self.map_msg)
self.goal_reached = True
# Set up port for incomming traffic
host = ""
port = 13005
BUFFER_SIZE = 1024
goal_msg = MoveBaseGoal()
goal_msg.target_pose.header.frame_id = "map"
try:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
except:
rospy.loginfo("FAILED TO CREATE SOCKET")
sys.exit()
try:
s.bind(("", port))
s.listen(1)
conn, addr = s.accept()
#rospy.loginfo("WAITING FOR CONNECTION")
except Exception as e:
rospy.loginfo("FAILED TO BIND")
rospy.loginfo(e)
sys.exit()
rospy.loginfo("GOAL SERVER ONLINE")
# main loop
while not rospy.is_shutdown():
self.seenmap = map_utils.Map(self.og)
try:
goal = self.random_goal()
conn.send(pickle.dumps(goal))
response = conn.recv(BUFFER_SIZE)
if response == "UNSEEN" and self.goal_reached == True:
rospy.loginfo("GOAL CHOSEN (%f, %f)", goal[0], goal[1])
goal_msg.target_pose.header.stamp = rospy.get_rostime()
goal_msg.target_pose.pose.position.x = goal[0]
goal_msg.target_pose.pose.position.y = goal[1]
goal_pub.publish(goal_msg)
except Exception as e:
s.close()
rospy.loginfo("SEENMAP SERVER ERROR:")
rospy.loginfo(e)
sys.exit()
rospy.sleep(5)
def __init__(self):
rospy.init_node('robo_cleanup')
# Subscribers/Publishers
self.map_msg = None
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped,
self.position_callback)
rospy.Subscriber('new_goal', MoveBaseGoal, self.new_goal_callback)
rospy.Subscriber('messes_to_clean', Float32MultiArray, self.mess_arr_callback)
self.mv_base = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=10)
self.goal_reached = rospy.Publisher('/goal_reached', Bool, queue_size=10)
# Other globals
self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.position = None
self.goal = None
self.searching = True
self.cleaning = False
self.cur_mess = None
self.mess_id = 1
self.tf_listener = tf.TransformListener()
self.old_marker = Point()
self.old_marker.x = 0
self.old_marker.y = 0
# Wait for the map and the robot's position to be initialized
while self.map_msg is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for map...")
rospy.sleep(1)
self.map = map_utils.Map(self.map_msg)
while (self.position is None and not rospy.is_shutdown()):
rospy.loginfo("Waiting for position...")
rospy.sleep(.1)
# Initially set the safezone to the start location
self.safezone = self.position
counter = 0
# Main execution loop
while not rospy.is_shutdown():
if (self.searching and self.goal is not None): # if in searching state AND has a valid goal
# if first goal
if counter == 0:
goal = self.goal_message(self.goal.x, self.goal.y, 0)
self.ac.send_goal(goal)
counter += 1
# if close enough to the goal OR the goal has been reached OR the goal has been abandoned
if (self.close_enough(self.position.position.x,self.position.position.y, self.goal.x, self.goal.y, .7)
or self.ac.get_state() is 4 or self.ac.get_state() is 3):
goal = self.goal_message(self.goal.x, self.goal.y, 0)
self.ac.send_goal(goal)
self.goal = None
if self.cleaning: # if in cleaning state
length = len(self.mess_arr)/2
count = 0
# retreive each mess in self.mess_arr
for i in range(0, length):
self.drive_to_mess(self.mess_arr[i+count], self.mess_arr[i+1+count])
self.take_to_safezone()
count += 1
self.cleaning = False
def __init__(self):
"""
The main control loop for searching and recording victim locations
"""
# States for the state machine
self.state = enum(SEARCHING=1, INVESTIGATING=2, GO_HOME=3)
rospy.init_node('random_nav')
# setup publishers and subscribers
self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.Subscriber('/visualization_marker', Marker, self.marker_callback)
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped,
self.position_callback)
rospy.Subscriber('/camera/rgb/image_raw', Image, self.image_callback)
self.marker_pub = rospy.Publisher('victim_marker', Marker,
queue_size=10)
self._talk_pub = rospy.Publisher('/robotsound', SoundRequest,
queue_size=10)
rospy.Subscriber('map', OccupancyGrid, self.map_callback)
self.tf_listener = tf.TransformListener()
self.current_goal = (1000,1000) # current nav goal
self.TIME = 450 # search time for robot
self.old_goals = [] # list of previous nav goals
self.cv_bridge = CvBridge() # for image conversion
self.image = None # raw rgb image
self.map_msg = None # sets up map
self.victims = [] # list of found victims
self.start_time = rospy.get_time() # start time of the robot
self.home = False # signals when to go home
self.position = None # robot's current position
self.cur_state = self.state.SEARCHING # current state of the robot
self.cur_marker = Point() # ar marker currently seen
self.first_goal = True # signals if it is the 1st goal
self.goals = 0 # diagnostic count of old goals
old_state = None # previous state of the robot
# Set up a SoundRequest message:
sound_request = SoundRequest()
sound_request.sound = sound_request.SAY
sound_request.command = sound_request.PLAY_ONCE
sound_request.arg = "Report is Ready!"
while self.map_msg is None and not rospy.is_shutdown():
rospy.loginfo("Waiting for map...")
rospy.sleep(.1)
self.map = map_utils.Map(self.map_msg)
while (self.position is None and not rospy.is_shutdown()):
rospy.loginfo("Waiting for position...")
rospy.sleep(.1)
# sets the home postion
home_pos = self.position
# generate the first random goal
x_goal = 100000
y_goal = 100000
while self.map.get_cell(x_goal, y_goal) != 0:
x_goal = random.random() * 20.0 - 10.0
y_goal = random.random() * 20.0 - 10.0
rospy.loginfo("Random goal found: ({}, {})".format(x_goal,
y_goal))
# main execution loop
while not rospy.is_shutdown():
local_marker = self.cur_marker
local_state = self.cur_state
# Go back to start after certain time limit
local_state = self.go_home(local_state)
# Set the goal to a random point
if local_state is self.state.SEARCHING:
goals = self.search(x_goal, y_goal, old_state)
x_goal = goals[0]
y_goal = goals[1]
# Set the goal to the starting position
elif local_state is self.state.GO_HOME:
x_goal = home_pos.position.x
y_goal = home_pos.position.y
# Set the goal to the victim's position
elif local_state is self.state.INVESTIGATING:
local_state = self.investigate(local_state, local_marker)
# Only set a new goal if the state has changed
if (old_state is not local_state):
goal = self.goal_message(x_goal, y_goal, 0)
self.go_to_point(goal)
self.old_goals.append((x_goal, y_goal))
self.current_goal = (x_goal,y_goal)
old_state = local_state
self.cur_state = local_state
# if going home, wait to reach goal then break out of loop
if self.home:
self.ac.wait_for_result()
self._talk_pub.publish(sound_request)
rospy.sleep(3)
break
