def __init__(self, start_node, goal_node, robot_rpm, map_img, animation):
"""
Initialize the explorer with a start node and final goal node
:param start_node: a list of start coordinates and orientation provided by the user
:param goal_node: a list of goal coordinates and orientation provided by the user
:param robot_rpm: a tuple of 2 RPMs for 2 wheels
:param map_img: a tuple of 2-d arrays with information of the map
"""
# Store start and goal nodes
self.start_node = Node(start_node, None, 0, 0, None)
self.goal_node = goal_node
# Store animation
self.animation = animation
# Store RPMs of robot
self.robot_rpm = robot_rpm
# Original map
self.map_img = map_img[0]
# Extended map due to robot radius and clearance
self.check_img = map_img[1]
# Store angular step size and translation step size
self.step_theta = constants.angular_step
# Store map dimensions
self.map_size = constants.map_size[0], constants.map_size[1], (constants.total_angle // self.step_theta)
# Define an empty list to store all generated nodes and path nodes
self.closed_nodes = []
self.path_nodes = []
# Define 3-D arrays to store information about generated nodes and parent nodes
self.parent = np.full(fill_value=constants.no_parent, shape=self.map_size)
# Define video-writer of open-cv to record the exploration and final path
video_format = cv2.VideoWriter_fourcc('X', 'V', 'I', 'D')
self.video_output = cv2.VideoWriter('video_output.avi', video_format, 200.0,
(self.map_size[1], self.map_size[0]))
def build_nodes(hetio: Dict, **kwargs) -> List[Node]:
force_rebuild = kwargs.get("force_rebuild", False)
save_checkpoint = kwargs.get("save_checkpoint", True)
if not force_rebuild:
if os.path.exists(NODES_CHECKPOINT):
return Node.deserialize_bunch(NODES_CHECKPOINT)
else:
log.info("Node checkpoint does not exist, building nodes.")
nodes = [
Node(h["identifier"], h["name"], h["kind"],
h["data"].get("source", None) or h["data"].get("sources", []),
h["data"].get("license", None), h["data"].get("url", None))
for h in hetio["nodes"]
]
assert len(nodes) == len(hetio["nodes"])
umls = kwargs.get("umls", None)
do = kwargs.get("do", None)
if umls is not None:
nodes = add_compound_metadata(hetio, nodes, umls)
if do is not None:
nodes = add_disease_metadata(hetio, nodes, do)
if save_checkpoint:
log.info("Checkpointing nodes...")
Node.serialize_bunch(nodes, NODES_CHECKPOINT)
return nodes
def gen_node_info(self):
overcloud_ip_list = TripleoHelper.find_overcloud_ips()
for node_ip in overcloud_ip_list:
LOG.info('Introspecting node %s' % node_ip)
node = Node('intro-%s' % node_ip,
address=node_ip,
user=self.overcloud_user)
node_mac = None
virsh_domain = None
server_name, _ = node.execute('hostname')
server_name = server_name.rstrip()
if 'overcloud-controller' in server_name:
node_type = 'controller'
elif 'overcloud-novacompute' in server_name:
node_type = 'compute'
else:
raise TripleOInspectorException('Unknown type '
'(controller/compute) %s ' %
server_name)
try:
processutils.execute('ping -c 1 %s' % node_ip)
res, _ = processutils.execute('/usr/sbin/arp -a '
'%s' % node_ip)
node_mac = \
re.search('([0-9a-z]+:){5}[0-9a-z]+', res).group(0)
virsh_domain = \
TripleoHelper.get_virtual_node_name_from_mac(node_mac)
except AttributeError:
LOG.warning("Unable to find MAC address for node {"
"}".format(node_ip))
# find ovs controller and manager
ovs_controller = self.get_ovs_controller(node)
out, _ = node.execute('ovs-vsctl get-manager', as_root=True)
ovs_managers = out.rstrip().split("\n")
if all(ovs_manager == '' for ovs_manager in ovs_managers):
LOG.warning(
"OVS managers for node {} is empty!".format(node_ip))
self.node_info['servers'][server_name] = {
'address': node_ip,
'user': self.overcloud_user,
'type': node_type,
'orig-ctl-mac': node_mac,
'vNode-name': virsh_domain,
'ovs-controller': ovs_controller,
'ovs-managers': ovs_managers
}
def set_node(self,
node_number,
x_coordinate,
y_coordinate,
z_coordinate=None,
values=None):
"""
Modify an existing node. For adding a new node function add_node() has to be used.
Args:
node_number: Number of the node to be modified.
x_coordinate: x-coordinate
y_coordinate: y-coordinate
z_coordinate: z-coordinate
values: dictionary of values
Returns: 0 if successful
"""
if node_number in self:
self.nodes[node_number] = Node(node_number, x_coordinate,
y_coordinate, z_coordinate, values)
return 0
else:
self.log.error(f'Node number {node_number} does not exist.')
def add_node(self,
node_number,
x_coordinate,
y_coordinate,
z_coordinate=None,
values=None):
"""
Adding a node to the grid. Each node number must be used just once, otherwise an error is thrown.
Args:
node_number: node number of the new node (optional)
x_coordinate: x-coordinate
y_coordinate: y-coordinate
z_coordinate: z-coordinate
values: dictionary of values
Returns: added node
"""
if node_number in self:
self.log.error(
f'Node with node_number {node_number} already exists with coordinates'
f' {self.nodes[node_number].coordinates}')
else:
if isinstance(node_number, int):
self.nodes[node_number] = Node(node_number, x_coordinate,
y_coordinate, z_coordinate,
values)
return 0
else:
self.log.error(f'Integer expected got {node_number}')
def insert(self, el):
nod = Node(el, 0)
pos = self._hash_code(el)
if self._table[pos] == 0:
self._table[pos] = nod
self._size += 1
else:
p = self._table[pos]
while p.next_ != 0 and self._R(nod.info, p.next_.info)==False:
p = p.next_
if p.next_ == 0:
p.next_ = nod
self._size += 1
else:
nod.next_ = p.next_
p.next_ = nod
self._size += 1
def build_nodes(repodb: pd.DataFrame, **kwargs) -> List[Node]:
force_rebuild = kwargs.get("force_rebuild", False)
save_checkpoint = kwargs.get("save_checkpoint", True)
if not force_rebuild:
if os.path.exists(NODES_CHECKPOINT):
return Node.deserialize_bunch(NODES_CHECKPOINT)
else:
log.info("Node checkpoint does not exist, building nodes.")
nodes = []
drug_ids = repodb["drug_id"].unique()
disease_ids = repodb["ind_id"].unique()
for drug_id in drug_ids:
match = repodb[repodb["drug_id"] == drug_id]
drug_names = match["drug_name"].unique()
assert len(drug_names) == 1
node = Node(drug_id,
drug_names[0],
kind="Compound",
sources=["RepoDB"])
nodes.append(node)
for disease_id in disease_ids:
match = repodb[repodb["ind_id"] == disease_id]
disease_names = match["ind_name"].unique()
assert len(disease_names) == 1
node = Node(disease_id,
disease_names[0],
kind="Disease",
sources=["RepoDB"])
nodes.append(node)
log.info(f"Built {len(nodes)} nodes from RepoDB.")
if save_checkpoint:
log.info("Checkpointing nodes...")
Node.serialize_bunch(nodes, NODES_CHECKPOINT)
return nodes
def ReadInGraph(file_name):
nodes = dict()
with open(file_name, 'r') as undirected_graph:
for node_line in undirected_graph:
node_line = node_line.split()
starting_node_id = int(node_line.pop(0))
if starting_node_id not in nodes:
nodes[starting_node_id] = Node(starting_node_id)
for connected_node in node_line:
t = connected_node.split(',')
node_id, length = int(t[0]), int(t[1])
if node_id not in nodes:
nodes[node_id] = Node(node_id)
e = Edge(length, nodes[starting_node_id], nodes[node_id])
nodes[starting_node_id].AddEdge(e)
nodes[node_id].AddEdge(e)
return nodes
def ReadGraph(filename):
nodes = dict()
with open(filename, 'r') as G:
header = G.readline()
header = header.split()
vertex_count = int(header[0])
edge_count = int(header[1])
for line in G:
line = line.split()
tail = int(line[0])
head = int(line[1])
weight = int(line[2])
if tail not in nodes:
nodes[tail] = Node(tail)
if head not in nodes:
nodes[head] = Node(head)
edge = Edge(weight, nodes[tail], nodes[head])
nodes[tail].AddEdge(edge)
nodes[head].AddEdge(edge)
return nodes
def create_graph_from_maze(self):
for row in range(len(self.maze)):
for column in range(len(self.maze)):
if self.maze[row, column] == 0:
continue
self.graph_maze[row, column] = Node(value=self.maze[row,
column],
row=row,
column=column,
algorithm=self.algorithm)
# Left
for row in range(len(self.maze)):
for column in range(len(self.maze)):
try:
if column - 1 >= 0:
self.graph_maze[row,
column].left = self.graph_maze[row,
column -
1]
except Exception:
continue
# Right
for row in range(len(self.maze)):
for column in range(len(self.maze)):
try:
self.graph_maze[row,
column].right = self.graph_maze[row,
column + 1]
except Exception:
continue
# Up
for row in range(len(self.maze)):
for column in range(len(self.maze)):
try:
if row - 1 >= 0:
self.graph_maze[row,
column].up = self.graph_maze[row - 1,
column]
except Exception:
continue
# Down
for row in range(len(self.maze)):
for column in range(len(self.maze)):
try:
self.graph_maze[row,
column].down = self.graph_maze[row + 1,
column]
except Exception:
continue
def swap_every_two(head) -> Node:
first = head
second = head.next
while second is not None:
temp = second.val
second.val = first.val
first.val = temp
if second.next is not None:
first = second.next
second = first.next
else:
first, second = None, None
return head
if __name__ == "__main__":
test = Node(1,Node(2,Node(3,Node(4))))
result = swap_every_two(test)
while result is not None:
print(result.val)
result = result.next
test = Node(1,Node(2,Node(3)))
result = swap_every_two(test)
while result is not None:
print(result.val)
result = result.next
def hierarchy(data, parent=None, index=0):
node = Node(parent)
node.index = index
index += 1
stat = {
# 【实体集合】
'entity': set(),
# 下一个可用的编号
'index': index,
# 统计信息
'n_root': 0,
'n_intent': 0,
'n_pickone': 0,
'n_order': 0,
'n_exchangeable': 0,
'n_content': 0,
'n_tag': 0
}
# 检查每个节点类型，对于每种类型的节点，补全初始值，删除无效字段
if 'type' not in data:
raise_error(
'Key "type" not found nearby "...%s...", a node must contains key "type".' % str(data)[:64])
node.data['type'] = data['type']
if node.data['type'] == 'root':
pass
elif node.data['type'] == 'holder':
return (False, )
elif node.data['type'] == 'intent':
if 'intent' not in data:
raise_error(
'Key "intent" not found nearby "...%s...", intent node must contains key "intent".' % str(data)[:64])
node.data['intent'] = data['intent']
_check_set_float(node, data, 'dropout', 0.0, 0.0, 1.0)
_check_set_float(node, data, 'weight', 1.0)
elif node.data['type'] in ('pickone', 'order', 'exchangeable'):
_set_if_exist(node, data, 'name')
_check_set_float(node, data, 'dropout', 0.0, 0.0, 1.0)
if parent != None and parent.data['type'] in ('pickone', 'intent'):
_check_set_float(node, data, 'weight', 1.0)
elif node.data['type'] == 'content':
if 'isEntity' not in data:
node.data['isEntity'] = False
else:
node.data['isEntity'] = bool(data['isEntity'])
if node.data['isEntity']:
if 'entity' not in data:
raise_error(
'Key "entity" not found nearby "...%s...", entity content node must contains key "entity".' % str(data)[:64])
node.data['entity'] = data['entity']
stat['entity'].add(node.data['entity'])
else:
if 'content' not in data or not isinstance(data['content'], Iterable):
return (False, )
node.data['content'] = []
stat['n_tag'] += len(data['content'])
for item in data['content']:
node.data['content'].append(item)
_set_if_exist(node, data, 'name')
if parent != None and parent.data['type'] in ('pickone', 'intent'):
_check_set_float(node, data, 'weight', 1.0)
_check_set_float(node, data, 'dropout', 0.0, 0.0, 1.0)
_check_set_float(node, data, 'cut', 0.0, 0.0, 1.0)
if node.data['cut'] > 0.0:
_check_set_float(node, data, 'word_cut', 0.0, 0.0, 1.0)
else:
raise_error(
'Unknoe node type "%s" nearby "...%s...".' % (node.data['type'], str(data)[:64]))
stat['n_' + node.data['type']] += 1
# 叶节点返回
if node.data['type'] == 'content':
return True, node, stat
# 添加子节点
node.children = []
if 'children' in data and isinstance(data['children'], Iterable):
for child in data['children']:
result = hierarchy(child, node, stat['index'])
if result[0]:
node.children.append(result[1])
r_st = result[2]
stat['entity'].update(r_st['entity'])
stat['index'] = r_st['index']
stat['n_intent'] += r_st['n_intent']
stat['n_pickone'] += r_st['n_pickone']
stat['n_order'] += r_st['n_order']
stat['n_exchangeable'] += r_st['n_exchangeable']
stat['n_content'] += r_st['n_content']
stat['n_tag'] += r_st['n_tag']
if len(node.children) == 0:
return (False,)
if node.data['type'] in ('root', 'pickone', 'intent'):
node.weights = []
for child in node.children:
node.weights.append(child.data['weight'])
node.weights = array(node.weights, dtype=float32)
return True, node, stat
from utils.node import Node
def reverse_list(head):
prev = head
head = head.next
prev.next = None
temp = ''
while temp is not None:
temp = head.next
head.next = prev
prev = head
if temp is not None:
head = temp
return head
if __name__ == "__main__":
test = Node(4, Node(3, Node(2, Node(1))))
list_reversed = reverse_list(test)
while list_reversed is not None:
print(list_reversed.val)
list_reversed = list_reversed.next
def build_environment(root_ast):
"""
environment stack:
- PackageDeclaration
- CompilationUnit
- TypeImports
- PackageImports
"""
abs_tree = root_ast.find_child('CompilationUnit')
pkg = list(abs_tree.select(['PackageDeclaration']))
pkg_name = ''
if len(pkg) == 0 or len(pkg[0].children) == 0:
# default package name
pkg = Node(name='PackageDeclaration', children=[
Node(name='Identifier', value=Token(label='Identifier',
value='MAIN_PKG', pos=-1, line=-1))
])
pkg_name = 'MAIN_PKG'
else:
pkg = pkg[0]
pkg_name = '.'.join(pkg.select_leaf_values(['Identifier']))
# add the package to the environment
pkg_env = Environment(name='PackageDeclaration', node=pkg,
value='.'.join([l.value.value for l in pkg.leafs()])
)
pkg.env = pkg_env # link the node back to here
cu_node = list(abs_tree.select(['CompilationUnit']))
if len(cu_node) != 1:
return pkg_env
cu = Environment(name='CompilationUnit', node=cu_node[0])
cu_node[0].env = cu
# add type imports to CompilationUnit
ti = {}
for i in abs_tree.select(['TypeImports', 'ImportDeclaration']):
i_name = '.'.join(n.value.value for n in i.leafs())
ti[i_name] = i
ti_node = list(abs_tree.select(['TypeImports']))[0]
ti_env = Environment(name='TypeImport', node=ti_node,
names=ti)
cu.children.append(ti_env)
ti_node.env = ti_env
# add package imports to CompilationUnit
pi = {}
for i in abs_tree.select(['PackageImports', 'ImportDeclaration']):
p_name = '.'.join(n.value.value for n in i.leafs())
pi[p_name] = i
# also add the package name itself into the package imports
pi[pkg_name] = pkg
pi_node = list(abs_tree.select(['PackageImports']))[0]
pi_env = Environment(name='PackageImports', node=pi_node,
names=pi)
cu.children.append(pi_env)
pi_node.env = pi_env
# add a class or interface to the package
type_name = ''
cls = abs_tree.find_child('ClassDeclaration')
iface = abs_tree.find_child('InterfaceDeclaration')
if cls != None:
cu.children.append(build_class_env(cls))
type_name = cls.find_child('ClassName').leaf_values()[0]
if iface != None:
cu.children.append(build_interface_env(iface))
type_name = iface.find_child('InterfaceName').leaf_values()[0]
# add the CompilationUnit to the package
cu.canon = '%s.%s' % (pkg_name, type_name)
pkg_env.children.append(cu)
return pkg_env
from utils.node import Node
def get_nth_from_last(linked_list, n):
output = linked_list
for _ in range(n):
linked_list = linked_list.next
while linked_list.next is not None:
output = output.next
linked_list = linked_list.next
return output.val
test_list = Node(0, Node(1, Node(2, Node(3, Node(4, Node(5))))))
assert get_nth_from_last(test_list, 2) == 3
assert get_nth_from_last(test_list, 3) == 2
assert get_nth_from_last(test_list, 5) == 0
start_node = ast.literal_eval(start_config)
# Check if correct no. of elements have been given for the puzzle
if len(start_node) != int(puzzle_size) + 1:
print('You entered', len(start_node), 'elements for the puzzle!')
print('Please enter exactly ' + str(int(puzzle_size) + 1) + ' elements each separated by a comma')
quit()
puzzle = pzl.Puzzle(start_node, goal_matrix)
# Print the start config
print('Initial node:\n', puzzle.initial_node)
print('Goal node:\n', puzzle.goal_node)
# Check solvability of the initial node given by the user
if not puzzle.check_solvability():
print('UNSOLVABLE CONFIG PROVIDED')
else:
# Define start node
start_node = Node(puzzle.initial_node, puzzle.get_final_weight(puzzle.initial_node, 0), 0, 0, -1, None)
# Store start node in open and generated nodes list
puzzle.open_nodes.append(start_node)
puzzle.generated_nodes.append(start_node)
# Store start time for exploration
start_time = time()
# Start exploration to find goal node
while len(puzzle.open_nodes):
# Get the node with lowest weight
current_node = puzzle.open_nodes.pop()
# Add current node to closed nodes and delete it from open nodes
puzzle.closed_nodes.append(current_node)
# Check if current node is goal node
if np.all(puzzle.goal_node == current_node.arr):
break
# Generate child nodes and iterate through them
def parse_node(self, idx, node):
splits = list(filter(lambda x : x != "", node.replace("\n", "").split(" ")))
assert len(splits) == 1, "Wrong init of node"
return Node(idx, int(splits[0]))
def read_node(self):
with open(self.data_dir+"node.txt", "r") as nodes_data:
for i, data in enumerate(nodes_data):
idNode = int(data.replace("\n", ""))
node = Node(idNode)
return node
if __name__ == '__main__':
start_time = time()
# Initialize the explorer class
start_node_coords = tuple(ast.literal_eval(start_node_coords))
goal_node_coords = tuple(ast.literal_eval(goal_node_coords))
explorer = Explorer(start_node_coords, goal_node_coords)
obstacle_map = Map(0, 0)
# Check validity of start and goal nodes
if not (obstacle_map.check_node_validity(start_node_coords[0], obstacle_map.height - start_node_coords[1] - 1)
and obstacle_map.check_node_validity(goal_node_coords[0], obstacle_map.height - goal_node_coords[1] - 1)):
print('One of the points lie in obstacle space!!\nPlease try again')
quit()
# Get the start node and add it to open nodes
start_node = Node(start_node_coords, explorer.get_final_weight(start_node_coords, 0), 0, None)
explorer.open_nodes.append(start_node)
explorer.generated_nodes.append(start_node)
while len(explorer.open_nodes):
current_node = explorer.open_nodes.pop(0)
explorer.closed_nodes.append(current_node)
# Check if current node is the goal node
if current_node.data == goal_node_coords:
break
# Generate child nodes and iterate through them
for child_node in current_node.generate_child_nodes((obstacle_map.width, obstacle_map.height)):
node_repeated = False
# Update final weight of the child node
child_node.weight = explorer.get_final_weight(child_node.data, child_node.cost)
# Make sure the node does not lie in the obstacle space
if obstacle_map.check_node_validity(child_node.data[0], obstacle_map.height - child_node.data[1] - 1):
def get_child_node(self, parent_node, l_vel, r_vel):
"""
Get child node based on wheel velocities
Show exploration by generating intermediate nodes
:param parent_node: node class object of parent
:param l_vel: left-wheel velocity of robot
:param r_vel: right-wheel velocity of robot
:return: node class object of child
"""
# An empty list to store intermediate nodes
inter_nodes = []
valid_path = True
# Define grey
grey = [200, 200, 200]
# Get coordinates and orientation of parent node
parent_node_data = parent_node.get_data()
y, x = parent_node_data[0], parent_node_data[1]
theta = np.pi * parent_node_data[2] * self.step_theta / 180
prev_y, prev_x, prev_theta = y, x, parent_node_data[2]
# Get linear and angular velocities in m/s and rad/s respectively
linear_x = 0.5 * constants.wheel_radius * (l_vel + r_vel) * np.cos(theta) / constants.scaling_factor
linear_y = 0.5 * constants.wheel_radius * (l_vel + r_vel) * np.sin(theta) / constants.scaling_factor
linear_vel = sqrt((linear_x ** 2) + (linear_y ** 2))
angular_vel = (constants.wheel_radius / constants.wheel_distance) * (r_vel - l_vel)
t = 0
while t < constants.total_time:
# Increment time
t += constants.time_step
# Get new coordinates and orientation using time step
x += (0.5 * constants.wheel_radius * (l_vel + r_vel) * np.cos(theta) * constants.time_step *
constants.time_scaling)
y += (0.5 * constants.wheel_radius * (l_vel + r_vel) * np.sin(theta) * constants.time_step *
constants.time_scaling)
theta += ((constants.wheel_radius / constants.wheel_distance) * (r_vel - l_vel) *
constants.time_step * constants.time_scaling)
# Get index of current orientation in grid world to check for parent
theta_index = self.get_orientation_index(theta)
# Check for validity of intermediate node
if (check_node_validity(self.check_img, int(x), self.map_size[0] - int(y)) and
self.parent[int(y)][int(x)][theta_index] == constants.no_parent):
# Add intermediate to its list
inter_nodes.append(([prev_y, prev_x, prev_theta], [y, x, theta], [linear_vel, angular_vel]))
# Update previous coordinates and orientation
prev_x, prev_y, prev_theta = x, y, theta
else:
valid_path = False
break
# Discard entire path if any intermediate point lies within obstacle space
if valid_path:
last_node = None
# Define base-cost of the child node
base_cost = parent_node.get_base_weight()
len_inter_nodes = len(inter_nodes)
# Add exploration to video
for i in range(len_inter_nodes):
prev_node, current_node, _ = inter_nodes[i]
# Update base-cost of the node
base_cost += get_euclidean_distance(prev_node, current_node)
# Get index of orientation of intermediate node
prev_node[2] = self.get_orientation_index(prev_node[2])
current_node[2] = self.get_orientation_index(current_node[2])
# Update parent of the intermediate node
self.parent[int(current_node[0])][int(current_node[1])][int(current_node[2])] = \
np.ravel_multi_index([int(prev_node[0]), int(prev_node[1]), int(prev_node[2])], dims=self.map_size)
# Add nodes to exploration video
if self.animation:
cv2.arrowedLine(self.map_img, (int(prev_node[1]), self.map_size[0] - int(prev_node[0])),
(int(current_node[1]), self.map_size[0] - int(current_node[0])), grey)
self.video_output.write(self.map_img)
# Make last node in the list as the child node and create is node class object
if i == len_inter_nodes - 1:
last_node = Node(current_node, parent_node_data, float('inf'), float('inf'), inter_nodes)
last_node.set_base_weight(base_cost)
last_node.set_weight(self.get_final_weight(current_node, last_node.base_weight))
return last_node
return None
def get_undercloud():
return Node('undercloud',
address=TripleoHelper.get_undercloud_ip(),
user='******',
password='******')
class Explorer:
def __init__(self, start_node, goal_node, robot_rpm, map_img, animation):
"""
Initialize the explorer with a start node and final goal node
:param start_node: a list of start coordinates and orientation provided by the user
:param goal_node: a list of goal coordinates and orientation provided by the user
:param robot_rpm: a tuple of 2 RPMs for 2 wheels
:param map_img: a tuple of 2-d arrays with information of the map
"""
# Store start and goal nodes
self.start_node = Node(start_node, None, 0, 0, None)
self.goal_node = goal_node
# Store animation
self.animation = animation
# Store RPMs of robot
self.robot_rpm = robot_rpm
# Original map
self.map_img = map_img[0]
# Extended map due to robot radius and clearance
self.check_img = map_img[1]
# Store angular step size and translation step size
self.step_theta = constants.angular_step
# Store map dimensions
self.map_size = constants.map_size[0], constants.map_size[1], (constants.total_angle // self.step_theta)
# Define an empty list to store all generated nodes and path nodes
self.closed_nodes = []
self.path_nodes = []
# Define 3-D arrays to store information about generated nodes and parent nodes
self.parent = np.full(fill_value=constants.no_parent, shape=self.map_size)
# Define video-writer of open-cv to record the exploration and final path
video_format = cv2.VideoWriter_fourcc('X', 'V', 'I', 'D')
self.video_output = cv2.VideoWriter('video_output.avi', video_format, 200.0,
(self.map_size[1], self.map_size[0]))
def get_heuristic_score(self, node):
"""
Implement heuristic function for a-star by calculating euclidean distance
Heuristic is nothing but cost to goal
:param: node: tuple containing coordinates and orientation of the current node
:return: distance between the goal node and current node
"""
# Evaluate euclidean distance between goal node and current node and return it
return get_euclidean_distance(self.goal_node, node)
def get_final_weight(self, node, base_cost):
"""
Get final weight for a-star
:param node: tuple containing coordinates and orientation of the current node
:param base_cost: base cost of the current node
:return: final weight for according to method
"""
# Add cost-to-goal and cost-to-come to get final cost and return it
return self.get_heuristic_score(node) + base_cost
def get_orientation_index(self, theta):
"""
Convert theta from radians to a grid world index based on pre-defined angular step
:param theta: orientation of the robot in radians
:return: index of orientation based on angular step
"""
# Limit orientation between 0 to 360
if theta >= 2 * np.pi:
n = int(theta / (2 * np.pi))
theta = theta - n * 2 * np.pi
elif theta < 0:
# Convert negative angle into positive and maintain the orientation between 0 to 360
theta = abs(theta)
if theta > 2 * np.pi:
n = int(theta / (2 * np.pi))
theta = theta - n * 2 * np.pi
# Get orientation in degrees
theta = theta + (180 * theta / np.pi)
# Limit orientation between 0 to 360
if theta > constants.total_angle:
theta = theta - constants.total_angle
# Return index by dividing orientation in degrees by angular step
return int(theta / self.step_theta)
def action_space(self, action):
"""
Define action space
:param action: Varies from 0-7 to call one of the 8 defined actions
:return: a tuple of left and right wheel RPM respectively
"""
if action == 0:
return 0, self.robot_rpm[0]
elif action == 1:
return 0, self.robot_rpm[1]
elif action == 2:
return self.robot_rpm[0], 0
elif action == 3:
return self.robot_rpm[1], 0
elif action == 4:
return self.robot_rpm[0], self.robot_rpm[0]
elif action == 5:
return self.robot_rpm[1], self.robot_rpm[1]
elif action == 6:
return self.robot_rpm[0], self.robot_rpm[1]
return self.robot_rpm[1], self.robot_rpm[0]
def take_action(self, parent_node, action):
"""
Call various actions based on an integer and get new child coordinates and orientation
Applying differential drive formulae to get coordinates and orientation
:param parent_node: a tuple of parent node coordinates and orientation
:param action: Varies from 0-n to call one of the 8 defined actions
:return: child node
"""
# Get action to be performed on parent to generate child
rpm = self.action_space(action)
# Convert rpm into left and right wheel velocities respectively
lw_velocity = rpm[0] * (2 * np.pi / 60)
rw_velocity = rpm[1] * (2 * np.pi / 60)
return self.get_child_node(parent_node, lw_velocity, rw_velocity)
def explore(self):
"""
Method to explore the map to find the goal
:return: nothing
"""
# Initialize priority queue
node_queue = PriorityQueue()
start_node = self.start_node.get_data()
self.parent[int(start_node[0])][int(start_node[1])][int(start_node[2])] = constants.start_parent
# Add start node to priority queue
node_queue.put(self.start_node)
# Start exploring
while not node_queue.empty():
# Get node with minimum total cost
current_node = node_queue.get()
self.closed_nodes.append(current_node)
# Add node to generated nodes array
# Check for goal node
if (self.get_heuristic_score(current_node.get_data()) <= constants.goal_thresh or
current_node.get_data() == self.goal_node):
self.path_nodes.append(current_node)
break
# Generate child nodes from current node
for i in range(constants.max_actions):
child_node = self.take_action(current_node, i)
if child_node is not None:
# Add child node to priority queue
node_queue.put(child_node)
def get_child_node(self, parent_node, l_vel, r_vel):
"""
Get child node based on wheel velocities
Show exploration by generating intermediate nodes
:param parent_node: node class object of parent
:param l_vel: left-wheel velocity of robot
:param r_vel: right-wheel velocity of robot
:return: node class object of child
"""
# An empty list to store intermediate nodes
inter_nodes = []
valid_path = True
# Define grey
grey = [200, 200, 200]
# Get coordinates and orientation of parent node
parent_node_data = parent_node.get_data()
y, x = parent_node_data[0], parent_node_data[1]
theta = np.pi * parent_node_data[2] * self.step_theta / 180
prev_y, prev_x, prev_theta = y, x, parent_node_data[2]
# Get linear and angular velocities in m/s and rad/s respectively
linear_x = 0.5 * constants.wheel_radius * (l_vel + r_vel) * np.cos(theta) / constants.scaling_factor
linear_y = 0.5 * constants.wheel_radius * (l_vel + r_vel) * np.sin(theta) / constants.scaling_factor
linear_vel = sqrt((linear_x ** 2) + (linear_y ** 2))
angular_vel = (constants.wheel_radius / constants.wheel_distance) * (r_vel - l_vel)
t = 0
while t < constants.total_time:
# Increment time
t += constants.time_step
# Get new coordinates and orientation using time step
x += (0.5 * constants.wheel_radius * (l_vel + r_vel) * np.cos(theta) * constants.time_step *
constants.time_scaling)
y += (0.5 * constants.wheel_radius * (l_vel + r_vel) * np.sin(theta) * constants.time_step *
constants.time_scaling)
theta += ((constants.wheel_radius / constants.wheel_distance) * (r_vel - l_vel) *
constants.time_step * constants.time_scaling)
# Get index of current orientation in grid world to check for parent
theta_index = self.get_orientation_index(theta)
# Check for validity of intermediate node
if (check_node_validity(self.check_img, int(x), self.map_size[0] - int(y)) and
self.parent[int(y)][int(x)][theta_index] == constants.no_parent):
# Add intermediate to its list
inter_nodes.append(([prev_y, prev_x, prev_theta], [y, x, theta], [linear_vel, angular_vel]))
# Update previous coordinates and orientation
prev_x, prev_y, prev_theta = x, y, theta
else:
valid_path = False
break
# Discard entire path if any intermediate point lies within obstacle space
if valid_path:
last_node = None
# Define base-cost of the child node
base_cost = parent_node.get_base_weight()
len_inter_nodes = len(inter_nodes)
# Add exploration to video
for i in range(len_inter_nodes):
prev_node, current_node, _ = inter_nodes[i]
# Update base-cost of the node
base_cost += get_euclidean_distance(prev_node, current_node)
# Get index of orientation of intermediate node
prev_node[2] = self.get_orientation_index(prev_node[2])
current_node[2] = self.get_orientation_index(current_node[2])
# Update parent of the intermediate node
self.parent[int(current_node[0])][int(current_node[1])][int(current_node[2])] = \
np.ravel_multi_index([int(prev_node[0]), int(prev_node[1]), int(prev_node[2])], dims=self.map_size)
# Add nodes to exploration video
if self.animation:
cv2.arrowedLine(self.map_img, (int(prev_node[1]), self.map_size[0] - int(prev_node[0])),
(int(current_node[1]), self.map_size[0] - int(current_node[0])), grey)
self.video_output.write(self.map_img)
# Make last node in the list as the child node and create is node class object
if i == len_inter_nodes - 1:
last_node = Node(current_node, parent_node_data, float('inf'), float('inf'), inter_nodes)
last_node.set_base_weight(base_cost)
last_node.set_weight(self.get_final_weight(current_node, last_node.base_weight))
return last_node
return None
def generate_path(self):
"""
Generate path using back-tracking
:return: a list containing path nodes
"""
# Exit if exploration failed
if not len(self.path_nodes):
print('No path found')
return False
# Define colors
red = [0, 0, 255]
blue = [255, 0, 0]
green = [0, 255, 0]
# Open text file to write velocities
vel_txt = open('output_files/commander.txt', 'w+')
# Get first node nearest to the goal node to start backtracking
last_node = self.path_nodes[0]
print('Finding path...')
# Iterate until we reach the initial node
while last_node.get_data() != self.start_node.get_data():
for node in self.closed_nodes:
if node.get_data() == last_node.get_parent():
self.path_nodes.append(node)
last_node = node
break
print('Path found')
# Iterate through path nodes
for i in range(len(self.path_nodes) - 2, -1, -1):
# Get intermediate nodes of each node in path-nodes' list
current_sub_nodes = self.path_nodes[i].get_sub_nodes()
# Iterate through intermediate nodes' list to display path to be taken by the robot
for j in range(0, len(current_sub_nodes)):
current_node_data = current_sub_nodes[j]
vel_txt.write(str(current_node_data[2][0]) + ',' + str(current_node_data[2][1]) + '\n')
if self.animation:
cv2.line(self.map_img,
(int(current_node_data[0][1]), self.map_size[0] - int(current_node_data[0][0])),
(int(current_node_data[1][1]), self.map_size[0] - int(current_node_data[1][0])),
blue)
self.video_output.write(self.map_img)
if self.animation:
# Draw start and goal node to the video frame in the form of filled circle
cv2.circle(self.map_img,
(int(self.path_nodes[-1].data[1]), self.map_size[0] - int(self.path_nodes[-1].data[0])),
int(constants.robot_radius), red, -1)
cv2.circle(self.map_img,
(int(self.path_nodes[0].data[1]), self.map_size[0] - int(self.path_nodes[0].data[0])),
int(constants.robot_radius), green, -1)
# Show path for longer time
for _ in range(1000):
self.video_output.write(self.map_img)
return True
from utils.node import Node
def rotate_list(head: Node, k: int) -> Node:
new_tail = head
while k > 2:
new_tail = new_tail.next
k -= 1
new_head = new_tail.next
new_tail.next = None
tail = new_head
while tail.next is not None:
tail = tail.next
tail.next = head
return new_head
if __name__ == "__main__":
test: Node = Node(1, Node(2, Node(3, Node(4, Node(5)))))
rotated_list: Node = rotate_list(test, 3)
rotated_list.print_linked_list()
