def set_current_node(self, msg):
# Logic for the first loop
if not self.start_time:
self.start_time = time()
# Otherwise record the timing data
else:
self.end_time = time()
# if the prior message had node information, record info from prior edge
if self.current_node:
self.record_timing()
# Otherwise, reset the timer
else:
self.start_time = time()
# AFTER recording data from prior edge, update info from new message
# If the current message does not contain node information, set all info to None
if msg.data == "Do not record":
self.current_node = None
self.next_node = None
self.hum_cond = None
# Otherwise record edge info
else:
[self.current_node, self.next_node,
self.hum_cond] = msg.data.split(', ')
# Saves file every 5 minutes or so
if time() - self.start_pickle_timer >= 300:
HospGraph.pickle_it(self.hosp_graph, path_and_name)
self.start_pickle_timer = time()
def __init__(self, path_and_name):
self.top_speed = 0.22 # turtlebot's top speed is 0.22 m/s
self.file_path = path_and_name
self.hosp_graph = HospGraph.unpickle_it(self.file_path)
self.tot_start = 0
self.tot_end = 0
def main(self):
for (n1, n2) in self.hosp_graph.edges():
print("--------------------")
print(n1, n2)
array_no = self.hosp_graph[n1][n2]['trav_data_no']
array_hum = self.hosp_graph[n1][n2]['trav_data_hum']
if len(array_no + array_hum) < 2:
self.hosp_graph.remove_edge(n1, n2)
print('edge removed between {} and {}'.format(n1, n2))
continue
self.euclidean_dist(n1, n2)
self.min_distance(n1, n2)
self.clean_data(n1, n2)
self.add_stats(n1, n2)
self.nav_failure_count()
print("total removed: {} of {}".format(self.tot_start - self.tot_end,
self.tot_start))
HospGraph.pickle_it(self.hosp_graph,
self.file_path + '_plus_stats_clean')
self.human_status = "_no"
def set_robot_vel(self):
if self.human_status == "_hum":
velocity = self.human_conditions[
self.condition][1] * self.turtlebot_default_vel
else:
velocity = self.turtlebot_default_vel
new_values = {'max_vel_x': velocity, 'min_vel_x': -velocity}
self.dynam_client.update_configuration(new_values)
rospy.loginfo(velocity)
self.vel_pub.publish(velocity)
if __name__ == "__main__":
rospy.init_node('node_in_graph_py')
path_to_pickle = rospy.get_param('graph_file_path')
hosp_graph = HospGraph.unpickle_it(path_to_pickle)
while not rospy.is_shutdown():
r = RobotNodeInfo(hosp_graph)
plan_sub = rospy.Subscriber('move_base/NavfnROS/plan', Path,
r.get_plan)
amcl_sub = rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped,
r.set_current_pose)
rospy.spin()
def pickle_it(obj_to_pickle, file_path_and_name):
with open(file_path_and_name, 'wb') as f:
pickle.dump(obj_to_pickle, f)
print("Saving information to file named {}".format(file_path_and_name))
if __name__ == '__main__':
hosp_graph_pickle = '/home/toothless/workspaces/research_ws/src/hospital-world/pickles/hospital_trials_2021-02-11_hum_50_70_plus_stats_clean'
ratio_data_filepath = "/home/toothless/workspaces/research_ws/src/hospital-world/pickles/ratio_data_02-22-2021_run02"
iterations = 1000
sample_size = 1000
hosp_graph = HospGraph.unpickle_it(hosp_graph_pickle)
fun_defs = DijkstraMethod()
sampling_planner = Samp.SamplingPlannerClass(hosp_graph)
compare = Choices.CompareMethods(hosp_graph, iterations)
manipulate = myMOO.ManipulateDF()
moo_vals = [
'gausMean', 'gausStd', 'gmmMean0', 'gmmSTD0', 'gmmUpper0', 'gmmLower0',
'gmmMean1', 'gmmSTD1', 'gmmUpper1', 'gmmLower1', 'humDist',
'fourConnDist', 'doors', 'navFail'
]
path_vals = [
'mean_all', 'std_all', 'mean_no', 'std_no', 'pct_hum', 'sq_dist',
'doors', 'nav_fail'
]
weight_options = [1, 5, 10, 100, 1000]
from scripts_simplified_algorithm.cow_goes_MOO import ManipulateDF
path_and_name = '/home/anna/workspaces/research_ws/src/hospital_world/pickles/hospital_trials_2021-01-15_plus_stats_clean'
def try_GMM(costs_gaus):
# costs_dummy = np.array(costs_gaus)
costs_dummy = np.reshape(costs_gaus, (-1, 1))
# costs_gaus_t = np.ndarray.transpose(costs_dummy)
bayes_gmm = mixture.BayesianGaussianMixture(n_components=2)
bayes_data = bayes_gmm.fit(costs_dummy)
return bayes_data
if __name__ == '__main__':
hosp_graph = HospGraph.unpickle_it(path_and_name)
shortest_simple = [
i for i in islice(
shortest_simple_paths(
hosp_graph, 'r00', target='r10', weight='mean_all'), 10)
]
methods = []
for j in range(len(shortest_simple)):
methods.append(Choices.MethodDef('method{}'.format(j), None))
shortest_reformat = [[[methods[i]], shortest_simple[i]]
for i in range(len(shortest_simple))]
sampling_planner = Samp.SamplingPlannerClass(hosp_graph)
iterations = 1000
class MoveRobotAround:
def __init__(self, hosp_graph):
self.current_position = None # Keeps track of robot's current x,y position
self.current_node = None # Keeps track of what node the robot is in
# CHANGE THIS TOO
self.initial_pose = hosp_graph.graph['initial_pose']
self.next_goal = (0.0, 0.0) # Initialize robot's initial location
self.prior_goal = (0.0, 0.0) # Initialize robot's prior goal with its starting position
self.break_goal = (-3.16, 8.22) # Goal to test the move_base- should be outside the bounds
self.result_from_path = 3 # Result of a successful path
self.hosp_graph = hosp_graph # Parameters of given world
self.rooms = [] # List of rooms - used to determine next goal
self._init_rooms_list() # Initialize list of rooms
self.new_plan_pub = rospy.Publisher('new_plan_pub', String, queue_size=10)
def _init_rooms_list(self):
# Initilaize list of rooms
self.rooms = ['r' + '%02d' % i for i in range(self.hosp_graph.graph['num_rooms'])]
def set_current_pose(self, msg):
# Get the current position of the robot and store it
self.current_position = (msg.pose.pose.position.x + self.initial_pose[0],
msg.pose.pose.position.y + self.initial_pose[1])
# print("Current position", self.current_position)
def set_current_node(self, msg):
# Saves robot's current node after listening to topic that broadcasts that data
self.current_node = msg.data
def select_new_goal(self):
# Choose a different room to navigate to
new_room = self.current_node
while new_room == self.current_node:
new_room = choice(self.rooms)
valid = False
while not valid:
# Uniformly select a random point in the new room - numbers are offset by 0.5 so the goal is not close to a wall
self.next_goal = (uniform(self.hosp_graph.nodes[new_room]['node_loc'][0].low + 0.5,
self.hosp_graph.nodes[new_room]['node_loc'][0].high - 0.5),
uniform(self.hosp_graph.nodes[new_room]['node_loc'][1].low + 0.5,
self.hosp_graph.nodes[new_room]['node_loc'][1].high - 0.5))
# Check that it is not in a convex portion of the room
# TODO: Change this to check ALL 'excl' portions of the map / find a better way to ignore those portions
exclusions = [key for key in self.hosp_graph.nodes() if 'ex' in key]
trigger = False
for key in exclusions:
if self.next_goal[0] in self.hosp_graph.nodes[key]['node_loc'][0] and \
self.next_goal[1] in self.hosp_graph.nodes[key]['node_loc'][1]:
trigger = True
if not trigger:
valid = True
# print("Next goal chosen:", self.next_goal)
def get_global_plan(self):
def movebase_client(self, redo=False):
# Code originally copied from https://hotblackrobotics.github.io/en/blog/2018/01/29/action-client-py/
# Create an action client called "move_base" with action definition file "MoveBaseAction"
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# Waits until the action server has started up and started listening for goals.
client.wait_for_server()
if not redo:
# Select a new goal
self.prior_goal = self.next_goal
self.select_new_goal()
else:
# Go back to the last point
self.next_goal = self.prior_goal
# Creates a new goal with the MoveBaseGoal constructor
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
# Move 0.5 meters forward along the x axis of the "map" coordinate frame
goal.target_pose.pose.position.x = self.next_goal[0] - self.initial_pose[0]
goal.target_pose.pose.position.y = self.next_goal[1] - self.initial_pose[1]
goal.target_pose.pose.position.z = 0.0
# print("New goal (amcl): {0} {1}".format(goal.target_pose.pose.position.x, goal.target_pose.pose.position.y))
# No rotation of the mobile base frame w.r.t. map frame
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
# Sends the goal to the action server.
client.send_goal(goal)
for i in range(10):
self.new_plan_pub.publish('yes')
# Waits for the server to finish performing the action.
wait = client.wait_for_result()
# If the result doesn't arrive, assume the Server is not available
if not wait:
rospy.logerr("Action server not available!")
# rospy.signal_shutdown("Action server not available!")
else:
# Result of executing the action
return client.get_state()
if __name__ == '__main__':
rospy.init_node('movebase_client_py')
path_to_pickle = rospy.get_param('graph_file_path')
hosp_graph = HospGraph.unpickle_it(path_to_pickle)
# Make a rowing robot
rowboat_robot = MoveRobotAround(hosp_graph)
# Subscribe to a bunch of stuff to get a bunch of info.
amcl_sub = rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, rowboat_robot.set_current_pose)
graph_node_sub = rospy.Subscriber('node_in_graph', String, rowboat_robot.set_current_node)
# Ugh.
# This was here from before I added in action server stuff and was writing the subscribers.
# I spent two days figuring out why the action server wasn't available.
# Through some random github issue answer, I saw a suggestion to remove a similar line to this, which solved it.
# Leaving it in so future me knows not to make the same mistake and spend days on end debugging.
# You're welcome, future me.
# rospy.spin()
iteration = 0
total_iterations = 10000
# Redo is a flag that, if true, sends the robot back to the previous point because something went wrong
redo = False
try:
while iteration < total_iterations:
# Tried to break it in such a way that I could catch it. Turns out it's too good.
# It either succeeds or can't reach the goal.
rowboat_robot.final_it = False
# if iteration == total_iterations - 2:
# # Out of range
# rowboat_robot.final_it = True
print("##############")
print("Iteration {}".format(iteration))
print("##############")
result = rowboat_robot.movebase_client(redo)
if result:
if result != 3:
rowboat_robot.result_from_path = result
redo = True
iteration += 1
print("Robot did not execute proper path")
else:
redo = False
iteration += 1
print("Arrived at: ", rowboat_robot.current_position)
rospy.loginfo("Goal execution done!")
else:
# For when something goes terribly wrong
print("Something has gone terribly wrong")
break
except rospy.ROSInterruptException:
rospy.loginfo("Navigation test finished.")
raise
print("done!")
