├── .idea └── workspace.xml ├── CMakeLists.txt ├── LTLMP ├── CMakeLists.txt ├── __init__.py ├── __init__.pyc ├── msg │ ├── activity.msg │ ├── activity_pose.msg │ ├── confirmation.msg │ ├── knowledge.msg │ └── pose.msg └── src │ ├── RTLTL.py │ ├── __init__.py │ ├── formula_parser │ ├── __init__.py │ ├── lexer.py │ ├── ltlf2dfa.py │ ├── ltlf_parser.py │ └── parser.py │ ├── gazebo_HMI_plugin.py │ ├── ros_gazebo_HMI_base.py │ └── utils │ ├── __init__.py │ ├── misc_func.py │ ├── naive_evaluator.py │ ├── pyb_plan_utils.py │ ├── tree_visualizer.py │ └── workspace.py ├── figure └── coverpage.png └── readme.md /.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 14 | 15 | 20 | 21 | 22 | 24 | 25 | 27 | 28 | 29 | 30 | 33 | { 34 | "keyToString": { 35 | "RunOnceActivity.OpenProjectViewOnStart": "true", 36 | "RunOnceActivity.ShowReadmeOnStart": "true", 37 | "WebServerToolWindowFactoryState": "false", 38 | "last_opened_file_path": "/home/joseph/yzchen_ws/task_planning/ltl_mq_opensource/src", 39 | "settings.editor.selected.configurable": "project.propVCSSupport.DirectoryMappings" 40 | } 41 | } 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 70 | 71 | 72 | 92 | 93 | 94 | 95 | 96 | 97 | 1690950025846 98 | 116 | 117 | 118 | 119 | 121 | 122 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /LTLMP/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rtltl) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | rospy 8 | std_msgs 9 | message_generation 10 | geometry_msgs 11 | ) 12 | 13 | 14 | ## Generate messages in the 'msg' folder 15 | add_message_files( 16 | FILES 17 | # activity.msg 18 | confirmation.msg 19 | pose.msg 20 | knowledge.msg 21 | activity_pose.msg 22 | ) 23 | 24 | 25 | ## Generate added messages and services with any dependencies listed here 26 | generate_messages( 27 | DEPENDENCIES 28 | std_msgs 29 | geometry_msgs 30 | # activity.msg 31 | ) 32 | 33 | catkin_package( 34 | # INCLUDE_DIRS include 35 | # LIBRARIES ltl3 36 | CATKIN_DEPENDS roscpp rospy std_msgs 37 | # DEPENDS system_lib 38 | geometry_msgs 39 | ) 40 | 41 | ########### 42 | ## Build ## 43 | ########### 44 | 45 | ## Specify additional locations of header files 46 | ## Your package locations should be listed before other locations 47 | # include_directories(include) 48 | include_directories( 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | 52 | -------------------------------------------------------------------------------- /LTLMP/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JINXER000/multiple-task-temporal-logic-planning/16bc21cac81b733efd0e657791ac492d2f0696fc/LTLMP/__init__.py -------------------------------------------------------------------------------- /LTLMP/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JINXER000/multiple-task-temporal-logic-planning/16bc21cac81b733efd0e657791ac492d2f0696fc/LTLMP/__init__.pyc -------------------------------------------------------------------------------- /LTLMP/msg/activity.msg: -------------------------------------------------------------------------------- 1 | int64 header 2 | string type 3 | float64 x 4 | float64 y 5 | float64 psi 6 | -------------------------------------------------------------------------------- /LTLMP/msg/activity_pose.msg: -------------------------------------------------------------------------------- 1 | int64 header 2 | string type 3 | geometry_msgs/PoseStamped tgt_pose 4 | -------------------------------------------------------------------------------- /LTLMP/msg/confirmation.msg: -------------------------------------------------------------------------------- 1 | int64 confheader 2 | string name 3 | int8 done 4 | string info 5 | -------------------------------------------------------------------------------- /LTLMP/msg/knowledge.msg: -------------------------------------------------------------------------------- 1 | string object 2 | int8 source 3 | float64 x 4 | float64 y 5 | -------------------------------------------------------------------------------- /LTLMP/msg/pose.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 psi 4 | -------------------------------------------------------------------------------- /LTLMP/src/RTLTL.py: -------------------------------------------------------------------------------- 1 | 2 | import networkx as nx 3 | import numpy as np 4 | from networkx.classes.digraph import DiGraph 5 | from utils.misc_func import get_automaton_dict, LineSegment, LinesegIntersectsAABB, getLabel, \ 6 | isWithinAABB, randomPointAABB, pybCheckCollision, collisionFreeAABB, ptInObs 7 | 8 | import pybullet as p 9 | from random import uniform 10 | import rospy 11 | 12 | class RT_LTL_planner(): 13 | class Node(): 14 | def __init__(self, node_id, pos, cost2Root, b_state, label): 15 | self.node_id = node_id 16 | self.pos = pos 17 | self.cost2Root = cost2Root 18 | self.b_state = b_state 19 | self.label = label 20 | self.same_pos_nodes =set() 21 | self.visited = False 22 | 23 | def __hash__(self): 24 | return hash(self.node_id) 25 | 26 | def __eq__(self, other): 27 | return (self.__class__ == other.__class__ and self.node_id == other.node_id) 28 | 29 | def __init__(self, root, goal, workspace, evaluator, para, automaton, segment, is_hybrid = False): 30 | self.para = para 31 | 32 | self.tree = DiGraph(init_pose = root.pos) 33 | self.tree.add_node(root) 34 | self.init_state = root 35 | self.goal = goal 36 | self.nodesAroundGoal = [] 37 | self.goalFound = False 38 | self.evaluator = evaluator 39 | # workspace 40 | self.workspace = workspace 41 | self.dim = len(self.workspace.workspace_range) 42 | 43 | # variables for real-time 44 | self.rt_root = self.init_state 45 | self.cur_target = None 46 | self.sparse_r = 5 47 | self.cur_path = [] 48 | self.rand_near_nbrs = set() 49 | self.rewireRand = set() 50 | self.visited_set = set() 51 | self.rewireRoot = set() 52 | self.pushedToRewireRoot = set() # has been pushed to rewireRoot within current iter 53 | self.next_wp = None 54 | self.old_dynamic_obs = (-10, -10) 55 | self.old_dynobs_aabb = None 56 | 57 | # variables for LTL 58 | aut_dict = get_automaton_dict(automaton) 59 | self.rt_root.b_state = aut_dict[self.rt_root.b_state] 60 | self.automaton = self.unifyAutomata(automaton, aut_dict) 61 | self.accept = self.automaton.graph['accept'] 62 | self.segment = segment 63 | self.unreachable_b_states = self.preprocessDFA() 64 | 65 | 66 | self.possible_paths = [] 67 | self.timeKeeper = None 68 | self.detect_obs_move = False 69 | self.detect_TS_change = False 70 | # for node_id, only increase. The reason for designing this parameter is that 71 | # if using len(self.tree.nodes()), some nodes will be deleted, and id will be duplicated. 72 | self.added_nodes_cnt = 1 73 | self.task_initialized = True 74 | self.rbt_pos = self.rt_root.pos 75 | self.root_just_changed = False 76 | 77 | self.bad_regs = set() 78 | self.dist_root2next = 0 79 | self.dist_root2rbt = 0 80 | self.dist_next2rbt = 0 81 | 82 | self.is_hybrid = is_hybrid 83 | self.changed_regs = set() 84 | self.nodes_pos_dict = {root.pos: root} 85 | 86 | self.old_dynobs_AABB = None 87 | 88 | 89 | 90 | def unifyAutomata(self, automaton, aut_nodes_dict): 91 | 92 | renamed_graph = automaton.copy() 93 | renamed_graph = nx.relabel_nodes(renamed_graph, aut_nodes_dict) 94 | 95 | initial_set = set() 96 | for nd in renamed_graph.graph['initial']: 97 | initial_set.add(aut_nodes_dict[nd]) 98 | renamed_graph.graph['initial'] = list(initial_set) 99 | 100 | accept_set = set() 101 | for nd in renamed_graph.graph['accept']: 102 | accept_set.add(aut_nodes_dict[nd]) 103 | renamed_graph.graph['accept'] = list(accept_set) 104 | 105 | return renamed_graph 106 | 107 | def reverseVisit(self, b_node, reachable_set): 108 | reachable_set.add(b_node) 109 | 110 | for b_parent in self.automaton.pred[b_node]: 111 | if b_parent == b_node: 112 | continue 113 | if b_parent in reachable_set: 114 | continue 115 | self.reverseVisit(b_parent, reachable_set) 116 | 117 | # mark unreachable nodes 118 | def preprocessDFA(self): 119 | for ac_node in self.accept: 120 | 121 | reachable_set = set() 122 | self.reverseVisit(ac_node, reachable_set) 123 | 124 | all_b_nodes = set(self.automaton.nodes()) 125 | unreachable = all_b_nodes.difference(reachable_set) 126 | 127 | return unreachable 128 | 129 | 130 | 131 | 132 | def checkCrossLabel(self, node, x_new, new_label): 133 | # not changing b_state if crossing 134 | for (region, aabb) in iter(self.workspace.regions.items()): 135 | j1 = LinesegIntersectsAABB(aabb, LineSegment(node.pos, x_new)) 136 | 137 | inspect_label = getLabel(self.workspace.reg_centers[region], self.workspace, self.is_hybrid) 138 | j2 = inspect_label!= new_label 139 | j3 = inspect_label!= node.label 140 | if j1 and j2 and j3: 141 | return False 142 | return True 143 | 144 | 145 | def checkBuchiTran(self, q_b, x_label, q_b_new): 146 | 147 | if q_b is None or q_b_new is None or\ 148 | q_b not in self.automaton.nodes or q_b_new not in self.automaton.nodes: 149 | return False 150 | 151 | b_state_succ = self.automaton.succ[q_b] 152 | if q_b_new not in b_state_succ: 153 | return False 154 | 155 | tran_valid = self.automaton.edges[q_b, q_b_new]['guard'].check(x_label) 156 | # bad states 157 | is_astray = q_b_new in self.unreachable_b_states or q_b in self.unreachable_b_states 158 | 159 | ok = tran_valid and not is_astray 160 | return ok 161 | 162 | def near_succ(self, inspect_node): 163 | near_nodes = set() 164 | checked_set = set([inspect_node.pos]) 165 | cost_dict = {(inspect_node.pos, inspect_node.pos): 0} 166 | radius = self.near_dist 167 | 168 | for node in self.tree.nodes: 169 | if self.evaluator.euclideanDist(inspect_node.pos, node.pos) > radius: 170 | continue 171 | 172 | if node.b_state not in self.automaton.succ[inspect_node.b_state]: 173 | continue 174 | 175 | # check trans 176 | trans_ok = self.checkBuchiTran(inspect_node.b_state, inspect_node.label, node.b_state) 177 | if not trans_ok: 178 | continue 179 | 180 | # for DFA, once state is accepting, we don't care its outward edge 181 | acc2other = inspect_node.b_state in self.accept and node.b_state not in self.accept 182 | if acc2other: 183 | continue 184 | 185 | if node.pos not in checked_set: 186 | # check obs 187 | no_collision = collisionFreeAABB(node.pos, inspect_node.pos, self.workspace, self.old_dynobs_aabb) 188 | safe_label = self.checkCrossLabel(node, inspect_node.pos, inspect_node.label) 189 | if not (no_collision and safe_label): 190 | continue 191 | 192 | cost_dict[(inspect_node.pos, node.pos)] = self.evaluator.getCost(node, inspect_node.pos) 193 | checked_set.add(node.pos) 194 | 195 | near_nodes.add(node) 196 | 197 | return near_nodes, cost_dict 198 | 199 | 200 | def extend(self, new_node, near_nodes, cost_dict): 201 | added = False 202 | min_cost = 1e7 203 | best_nbr = () 204 | 205 | # near_nodes are nodes that are collision-free, and have distinct position 206 | for diff_x_node in near_nodes: 207 | # iterate over S^{rcd} 208 | for node in diff_x_node.same_pos_nodes: 209 | if node.node_id == new_node.node_id: 210 | continue 211 | 212 | j2 = self.checkBuchiTran(node.b_state, node.label, new_node.b_state) 213 | # don't go from accept to non-accept 214 | j3 = node.b_state in self.accept and new_node.b_state not in self.accept 215 | 216 | if j2 and not j3: 217 | cost = self.calc_cost2Root(node) + cost_dict[(new_node.pos, node.pos)] 218 | if cost < min_cost: 219 | added = True 220 | best_nbr = node 221 | min_cost = cost 222 | 223 | if added: 224 | new_node.cost2Root=min_cost 225 | self.addNode(new_node, best_nbr) 226 | 227 | return added 228 | 229 | def rewire(self, new_node, near_nodes, cost_dict): 230 | rewired_nodes = set() 231 | for node in near_nodes: 232 | if node.node_id == new_node.node_id: 233 | continue 234 | # if b_state is None, assign new 235 | if node.cost2Root == np.inf and node.b_state is None: 236 | raise NotImplementedError() 237 | else: 238 | # note: as near_nodes are produced by near_succ(), there is no need to do checkBuchiTran() 239 | 240 | cost = self.calc_cost2Root(new_node) + cost_dict[(new_node.pos, node.pos)] 241 | delta_c = self.calc_cost2Root(node) - cost 242 | 243 | if delta_c > 1e-3: 244 | pred_node_list = list(self.tree.pred[node].keys()) 245 | if len(pred_node_list): 246 | pred_node = pred_node_list[0] 247 | self.tree.remove_edge(pred_node, node) 248 | # else: isolated node 249 | node.cost2Root = cost 250 | self.tree.add_edge(new_node, node) 251 | 252 | rewired_nodes.add(node) 253 | return rewired_nodes 254 | 255 | # one step further on automaton. 256 | def checkNode2Goal(self, node, is_hybrid, approx=False): 257 | if self.segment == 'suffix': 258 | raise 'Not Implemented' 259 | 260 | # filter out nodes blocked by dyn obs and isolated nodes 261 | nd_cost = self.calc_cost2Root(node) 262 | if nd_cost == np.inf: 263 | return False 264 | 265 | goal_updated = False 266 | 267 | if node.b_state in self.accept: 268 | pred_nodes = list(self.tree.pred[node].keys()) 269 | # if root satisfy LTL 270 | if len(pred_nodes) == 0: 271 | goal_updated = self.update_target(node) 272 | return goal_updated 273 | # else use the previous as the goal 274 | else: 275 | q_n = pred_nodes[0] 276 | if q_n.b_state not in self.accept: 277 | goal_updated = self.update_target(q_n) 278 | # accept -->(1)--> accept 279 | return goal_updated 280 | 281 | # node.b_state not in self.accept 282 | for b_accept in self.accept: 283 | nd_label = getLabel(node.pos, self.workspace, is_hybrid) 284 | if self.checkBuchiTran(node.b_state, nd_label, b_accept): 285 | goal_updated = self.update_target(node) 286 | 287 | return goal_updated 288 | 289 | def pos_near(self, pos1, pos2, dist): 290 | if self.evaluator.euclideanDist(np.array(pos1), np.array(pos2)) < dist: 291 | return True 292 | else: 293 | return False 294 | 295 | # similar to homotopy path 296 | def is_same_solution(self, sol1, sol2): 297 | if not len(sol1) or not len(sol2): 298 | raise 'invalid input' 299 | #1 if the ending point is different 300 | if not self.pos_near(sol1[-1].pos,sol2[-1].pos, 20): 301 | return False 302 | 303 | #2 check the changing point list: different length or shape 304 | if len(sol1) != len(sol2): 305 | return False 306 | 307 | for i in range(len(sol1)): 308 | if sol1[i].label != sol2[i].label or not self.pos_near(sol1[i].pos,sol2[i].pos, self.para['sameSol_radius']): 309 | return False 310 | 311 | return True 312 | 313 | def retrace_path(self, goal_node): 314 | path = [goal_node] 315 | pred_ls = list(self.tree.pred[goal_node].keys()) 316 | s = goal_node 317 | while len(pred_ls)!=0: 318 | s = self.tree.pred[s].keys()[0] 319 | path.insert(0, s) 320 | pred_ls = list(self.tree.pred[s].keys()) 321 | return path 322 | 323 | # find similar path; if found, update it; if not found, put the tgt into pq. 324 | # if root.automaton changed, try to rewire and check reachability. 325 | def update_target(self, node): 326 | updated = False 327 | if node.b_state is None: 328 | raise KeyError() 329 | 330 | node_path = self.retrace_path(node) 331 | node_trace = self.extract_trace(node_path) 332 | if len(node_trace) == 0: 333 | return False 334 | 335 | if self.cur_target is None: 336 | self.cur_target = node 337 | self.goalFound = True 338 | self.possible_paths.append(node_path) 339 | return True 340 | 341 | node_cost = self.calc_cost2Root(node) 342 | 343 | is_similar = False 344 | for i in range(len(self.possible_paths)): 345 | path_cmp = self.possible_paths[i] 346 | trace_cmp = self.extract_trace(path_cmp) 347 | is_similar = self.is_same_solution(trace_cmp, node_trace) 348 | 349 | if is_similar: 350 | # compare cost 351 | end_node = trace_cmp[-1] 352 | if node_cost < self.calc_cost2Root(end_node): 353 | # assign address? 354 | self.possible_paths[i] = node_path 355 | updated = True 356 | 357 | # can be similar but not that good 358 | break 359 | 360 | if not is_similar: 361 | self.possible_paths.append(node_path) 362 | updated = True 363 | 364 | if updated: 365 | self.possible_paths = sorted(self.possible_paths, key=lambda trace: self.calc_cost2Root(trace[-1])) 366 | self.cur_target = self.possible_paths[0][-1] 367 | 368 | return updated 369 | 370 | def calc_cost2Root(self, node): 371 | # next_wp is the R_aux 372 | is_pass_nxt = False 373 | is_bad_node = node.cost2Root == np.inf 374 | cost = 0 375 | pred_ls = list(self.tree.pred[node].keys()) 376 | s = node 377 | while len(pred_ls) != 0: 378 | if self.next_wp is not None and self.next_wp == s: 379 | is_pass_nxt = True 380 | 381 | parent = self.tree.pred[s].keys()[0] 382 | # check parent is valid 383 | if parent.cost2Root == np.inf: 384 | is_bad_node = True 385 | 386 | # check automaton tran 387 | parent_label = parent.label 388 | parent_bs = parent.b_state 389 | child_bs = s.b_state 390 | 391 | tran_valid = self.checkBuchiTran(parent_bs, parent_label, child_bs) 392 | if not tran_valid: 393 | s.cost2Root = np.inf 394 | is_bad_node = True 395 | 396 | cost += self.evaluator.euclideanDist(s.pos, parent.pos) 397 | s = parent 398 | pred_ls = list(self.tree.pred[s].keys()) 399 | 400 | if is_bad_node: 401 | node.cost2Root = np.inf 402 | return np.inf 403 | else: 404 | if is_pass_nxt: 405 | cost -= self.dist_root2next 406 | 407 | node.cost2Root = cost 408 | return cost 409 | 410 | # check the trace satisfy the specification 411 | def examine_trace(self, path): 412 | 413 | # implicitely check transition 414 | path_cost = self.calc_cost2Root(path[-1]) 415 | 416 | if path_cost == np.inf: 417 | return False 418 | 419 | last_bs = path[-1].b_state 420 | last_label = path[-1].label 421 | for b_accept in self.accept: 422 | if self.checkBuchiTran(last_bs, last_label, b_accept): 423 | return True 424 | 425 | return False 426 | 427 | # get event-driven trace 428 | def extract_trace(self, path): 429 | if len(path) <2: 430 | return path 431 | 432 | trace = [] # no starting point 433 | for id in range(0,len(path)-1): 434 | if path[id].label!= path[id+1].label: 435 | trace.append(path[id]) 436 | 437 | trace.append(path[-1]) 438 | return trace 439 | 440 | def ntr_node(self, better_node, worse_node): 441 | 442 | # rewire worse node 443 | succ_worse_nodes = list(self.tree.succ[worse_node]) 444 | for succ_worse_node in succ_worse_nodes: 445 | # rewire then do dfs on tree2 446 | self.tree.remove_edge(worse_node, succ_worse_node) 447 | self.tree.add_edge(better_node, succ_worse_node) 448 | 449 | prev_worse_node = list(self.tree.pred[worse_node])[0] 450 | self.tree.remove_edge(prev_worse_node, worse_node) 451 | worse_node.cost2Root = np.inf 452 | self.safe_remove_node(worse_node) 453 | 454 | # replace worse nodes in possible path 455 | for path in self.possible_paths: 456 | if path[-1] == worse_node: 457 | path[-1] = better_node 458 | 459 | def safe_remove_node(self, node): 460 | 461 | if len(node.same_pos_nodes): 462 | node.same_pos_nodes.remove(node) 463 | 464 | same_pos_nd_remain = None 465 | if len(node.same_pos_nodes): 466 | same_pos_nd_remain = list(node.same_pos_nodes)[0] 467 | 468 | # take care of pos_node_dict 469 | if self.nodes_pos_dict[node.pos] == node: 470 | 471 | if same_pos_nd_remain is None: 472 | # delete the entry 473 | self.nodes_pos_dict.pop(node.pos) 474 | else: 475 | self.nodes_pos_dict[node.pos] = same_pos_nd_remain 476 | 477 | self.tree.remove_node(node) 478 | 479 | def addNode(self, node, parent = None): 480 | self.tree.add_node(node) 481 | self.nodes_pos_dict[node.pos] = node 482 | self.added_nodes_cnt +=1 483 | 484 | if parent: 485 | self.tree.add_edge(parent, node) 486 | 487 | def isIsolated(self, node): 488 | if self.tree.in_degree(node) ==0 and self.tree.out_degree(node) ==0: 489 | return True 490 | else: 491 | return False 492 | 493 | def mergeNode(self, s_m, depth): 494 | better_node = s_m 495 | 496 | # pre-order 497 | same_state_nodes = [nd for nd in s_m.same_pos_nodes \ 498 | if nd.b_state == s_m.b_state and nd != s_m] # and len(list(self.tree.pred[nd]))!=0 499 | 500 | same_cpy = same_state_nodes[:] 501 | # delete isolated 502 | for nd in same_cpy: 503 | if self.isIsolated(nd): 504 | same_state_nodes.remove(nd) 505 | self.safe_remove_node(nd) 506 | 507 | same_state_num = len(same_state_nodes) 508 | 509 | if same_state_num >=1: 510 | # deal with father-son relation first. father cannot be bad. 511 | same_state_node = same_state_nodes[0] 512 | if same_state_node == list(self.tree.pred[s_m].keys())[0]: 513 | worse_node = s_m 514 | better_node = same_state_node 515 | else: 516 | worse_node = same_state_node 517 | 518 | if self.calc_cost2Root(s_m) > self.calc_cost2Root(same_state_node) and self.calc_cost2Root(same_state_node) <1e6: 519 | better_node = same_state_node 520 | worse_node = s_m 521 | 522 | self.ntr_node(better_node, worse_node) 523 | 524 | 525 | worse_node.visited = True 526 | 527 | better_node.visited = True 528 | 529 | better_succ_list = list(self.tree.succ[better_node]) 530 | # NOTE: better_succ_list can contain worse nodes which is deleted by its siblings. visited flag for the worse nodes are true. 531 | for child in better_succ_list: 532 | if child == self.rt_root: 533 | continue 534 | 535 | # child was visited as a worse node. no succ already. 536 | if child.visited: 537 | continue 538 | 539 | self.mergeNode(child, depth+1) 540 | 541 | 542 | # 2 step: 1, dfs b_state; 2. do the merging 543 | def merge_branch(self, subtree_root, parent): 544 | 545 | trans_valid = self.propagate_b_state(parent, subtree_root) 546 | if not trans_valid: 547 | return 548 | 549 | print('merge branch rooted at '+ str(subtree_root.pos)) 550 | 551 | self.propagateState(subtree_root, 0) 552 | 553 | # DFS post-order on subtree_root. 554 | self.mergeNode(subtree_root, 0) 555 | 556 | 557 | def propagateState(self, subroot, depth): 558 | 559 | succ_list = list(self.tree.succ[subroot]) 560 | # for merge_branch 561 | subroot.visited = False 562 | 563 | # NOTE: succ_list can contain worse nodes which is deleted by its siblings 564 | for child in succ_list: 565 | if child == self.rt_root: 566 | continue 567 | 568 | # can revise b_satate here 569 | trans_valid = self.propagate_b_state(subroot, child) 570 | if not trans_valid: 571 | continue 572 | 573 | self.propagateState(child, depth+1) 574 | 575 | def propagate_b_state(self, parent, child): 576 | 577 | 578 | child_bs_valid = self.checkBuchiTran(parent.b_state, parent.label, child.b_state) 579 | if child_bs_valid: 580 | return True 581 | 582 | buchi_succ = self.automaton.succ[parent.b_state] 583 | buchi_candidates = [] 584 | 585 | for b_state in buchi_succ: 586 | if b_state != child.b_state and self.checkBuchiTran(parent.b_state, parent.label, b_state): 587 | buchi_candidates.append(b_state) 588 | if len(buchi_candidates)>1: # not a DFA! 589 | raise NameError('too many candidates') 590 | elif len(buchi_candidates) ==1: 591 | 592 | # add old state, in case of rewire it back 593 | same_state_nodes =[nd for nd in child.same_pos_nodes 594 | if nd.b_state== child.b_state and nd != child] 595 | if len(same_state_nodes)==0: 596 | old_id = self.added_nodes_cnt 597 | old_state_node = self.Node(old_id, child.pos, np.inf, child.b_state, child.label) 598 | self.addNode(old_state_node) 599 | 600 | child.same_pos_nodes.add(old_state_node) 601 | old_state_node.same_pos_nodes = child.same_pos_nodes # addr of a list 602 | 603 | child.b_state = buchi_candidates[0] 604 | child_bs_valid = True 605 | else: # if DFA, won't enter. for safety LTL only. 606 | child.cost2Root = np.inf 607 | 608 | return child_bs_valid 609 | 610 | 611 | def updateBestPath(self): 612 | updated = False 613 | 614 | # has been sorted in validate_update_sols() 615 | possible_new_tgt = self.possible_paths[0][-1] 616 | if possible_new_tgt!= self.cur_target: 617 | updated = True 618 | self.cur_target = possible_new_tgt 619 | 620 | self.cur_path = self.retrace_path(self.cur_target) 621 | if len(self.cur_path)>1: 622 | self.next_wp = self.cur_path[1] 623 | self.dist_root2next = self.evaluator.euclideanDist(self.next_wp.pos, self.rt_root.pos) 624 | else: 625 | self.next_wp = self.cur_path[0] 626 | self.dist_root2next = 0.0 627 | return updated 628 | 629 | def validate_update_sols(self): 630 | # check if the all candidates are valid and leading to accept (cur_path should be the 1st of candidates) 631 | resume_path = False 632 | 633 | valid_paths = [] 634 | for path in self.possible_paths: 635 | updated_path= self.retrace_path(path[-1]) 636 | if self.examine_trace(updated_path): 637 | valid_paths.append(updated_path) 638 | 639 | if len(valid_paths)==0: 640 | self.codeRestart() 641 | return resume_path 642 | 643 | self.possible_paths = sorted(valid_paths, key=lambda trace: self.calc_cost2Root(trace[-1])) 644 | # update cur_target, cur_path 645 | self.updateBestPath() 646 | self.goalFound = True 647 | 648 | resume_path = True 649 | 650 | return resume_path 651 | 652 | def changeRoot(self, new_root): 653 | require_prop = False 654 | self.tree.remove_edge(self.rt_root, new_root) 655 | self.rt_root.cost2Root = new_root.cost2Root 656 | new_root.cost2Root = 0 657 | 658 | self.tree.add_edge(new_root, self.rt_root) 659 | 660 | reverse_trans_valid = self.checkBuchiTran(new_root.b_state, new_root.label, self.rt_root.b_state) 661 | if not reverse_trans_valid: 662 | require_prop = True 663 | 664 | self.rt_root = new_root 665 | 666 | return require_prop 667 | 668 | 669 | 670 | def get_bad_regs(self): 671 | self.bad_regs.clear() 672 | buchi_succ = self.automaton.succ[self.rt_root.b_state] 673 | cur_b_state = self.rt_root.b_state 674 | for b_state in buchi_succ: 675 | if self.checkBuchiTran(self.rt_root.b_state, self.rt_root.label, b_state): 676 | cur_b_state = b_state 677 | break 678 | 679 | b_state_succ = self.automaton.succ[cur_b_state] 680 | for bad_b in self.unreachable_b_states: 681 | if bad_b not in b_state_succ: 682 | continue 683 | 684 | for reg in self.workspace.regions: 685 | prop = self.workspace.properties[reg] 686 | lead_to_bad = self.automaton.edges[cur_b_state, bad_b]['guard'].check(prop) 687 | if lead_to_bad: 688 | self.bad_regs.add(reg) 689 | 690 | 691 | 692 | 693 | def sudden_stop(self): 694 | require_prop = False 695 | 696 | exactly_on_root = self.dist_root2rbt< self.para['changeRoot_radius'] 697 | exactly_on_nxt = self.goalFound and self.dist_next2rbt < self.para['changeRoot_radius'] 698 | if exactly_on_root: 699 | add_node_pos = self.rt_root.pos 700 | self.dist_root2rbt = 0 701 | elif exactly_on_nxt: # if meet obs and no valid sols, don't enter this condition 702 | add_node_pos = self.next_wp.pos 703 | require_prop = self.changeRoot(self.next_wp) 704 | self.dist_root2rbt = 0 705 | else: 706 | add_node_pos = tuple(self.rbt_pos) 707 | 708 | # if property change no affect, then will not merge branch. 709 | curpos_id = self.added_nodes_cnt 710 | cur_label = getLabel(add_node_pos, self.workspace, self.is_hybrid) 711 | 712 | if self.goalFound: 713 | prev_b_state = self.next_wp.b_state 714 | else: 715 | prev_b_state = self.rt_root.b_state 716 | 717 | curpos_node = self.Node(curpos_id, add_node_pos, self.dist_root2rbt, prev_b_state, cur_label) 718 | if not exactly_on_root and not exactly_on_nxt: 719 | curpos_node.same_pos_nodes.add(curpos_node) 720 | 721 | self.addNode(curpos_node, self.rt_root) 722 | 723 | 724 | old_root = self.rt_root 725 | require_prop = self.changeRoot(curpos_node) 726 | if not self.detect_obs_move: 727 | self.merge_branch(old_root, curpos_node) 728 | 729 | 730 | self.visited_set.clear() 731 | self.rewireRoot.clear() 732 | self.pushedToRewireRoot.clear() 733 | 734 | if exactly_on_root: 735 | print("happen to be on root") 736 | # delete added dummy root 737 | self.tree.remove_edge(curpos_node, old_root) 738 | self.safe_remove_node(curpos_node) 739 | self.rt_root = old_root 740 | elif exactly_on_nxt: 741 | print("happen to be on next_wp") 742 | # delete added dummy root 743 | self.tree.remove_edge(curpos_node, old_root) 744 | self.safe_remove_node(curpos_node) 745 | self.rt_root = old_root 746 | 747 | return require_prop 748 | 749 | def change_labels(self, reg_name): 750 | for node in self.tree.nodes: 751 | if isWithinAABB(node.pos, self.workspace.regions[reg_name]): 752 | node.label = getLabel(node.pos, self.workspace, is_hybrid=self.is_hybrid) 753 | 754 | def codeRestart(self): 755 | # disable current plan 756 | self.cur_target = None 757 | self.goalFound = False 758 | self.cur_path = [] 759 | self.next_wp = None 760 | self.dist_root2next = 0 761 | self.possible_paths = [] 762 | 763 | self.check_all_nodes_to_goal() 764 | 765 | # caution: must assure that the cost is not inf 766 | def check_all_nodes_to_goal(self, is_hybrid = False): 767 | if self.goalFound: 768 | return 769 | 770 | # check rt_root first 771 | goal_updated = self.checkNode2Goal(self.rt_root, is_hybrid, approx=True) 772 | if goal_updated: 773 | print('Already fulfill the task!') 774 | self.updateBestPath() 775 | return 776 | 777 | for node in list(self.tree.nodes()): 778 | 779 | goal_updated = self.checkNode2Goal(node, is_hybrid, approx=True) 780 | if goal_updated: 781 | print('got shortcut') 782 | self.updateBestPath() 783 | break 784 | 785 | # NOTE: invalid_set is deleted 786 | def blockNodes(self): 787 | move_obs_AABB = self.old_dynobs_AABB 788 | for child in list(self.tree.nodes()): 789 | if child == self.rt_root: 790 | continue 791 | 792 | pred_ls = list(self.tree.pred[child].keys()) 793 | if len(pred_ls) ==0: 794 | continue 795 | parent = pred_ls[0] 796 | 797 | # make move_obs_bdr an AABB, 798 | is_intersect = LinesegIntersectsAABB(move_obs_AABB, LineSegment(parent.pos, child.pos)) 799 | if is_intersect: 800 | if isWithinAABB(parent.pos, move_obs_AABB): 801 | # in case parent is root 802 | if parent.cost2Root !=0: 803 | parent.cost2Root = np.inf 804 | 805 | child.cost2Root = np.inf 806 | 807 | 808 | def handle_perception(self, reg_name): 809 | obs_no_affect = False 810 | 811 | if self.detect_obs_move: 812 | self.blockNodes() 813 | 814 | resume_path = self.validate_update_sols() 815 | if not resume_path: 816 | self.sudden_stop() 817 | self.root_just_changed = True 818 | else: 819 | obs_no_affect = True 820 | 821 | # if resume_path is false, then self.codeRestart() is already called in self.validate_update_sols() 822 | resume_path = True 823 | # deal with move obs and new observation 824 | if self.detect_TS_change: 825 | 826 | self.change_labels(reg_name) 827 | 828 | self.sudden_stop() 829 | self.root_just_changed = True 830 | 831 | # if cur plan is invalid on automaton, or blocked by obs (redandent checking) 832 | # or (self.cur_target and self.calc_cost2Root(self.cur_target)== np.inf) 833 | resume_path = self.validate_update_sols() 834 | 835 | return obs_no_affect 836 | 837 | def sample(self): 838 | if self.para['bias_sample']: 839 | x_rand = None 840 | rand_num = uniform(0,1) 841 | 842 | reg_num = len(self.workspace.regions) 843 | rand_sect = self.para['reg_sample_ratio'] /reg_num 844 | 845 | 846 | i=0 847 | for reg_name, reg_bbx in self.workspace.regions.iteritems(): 848 | if rand_num < 1- i*rand_sect and rand_num >= 1- (i+1)*rand_sect: 849 | x_rand = randomPointAABB(reg_bbx) 850 | 851 | return x_rand 852 | i +=1 853 | 854 | x_rand = randomPointAABB(self.workspace.ws_AABB) 855 | 856 | return x_rand 857 | 858 | def calc_near_dist(self): 859 | # self.near_dist = 50 860 | # follow the way in RT-RRT* 861 | self.near_dist = np.sqrt(self.workspace.length*self.workspace.width*self.para['k_max']/np.pi / self.tree.number_of_nodes()) 862 | 863 | def nearest_samp(self, x_rand): 864 | # combine old near() and nearest_samp() 865 | min_dis = np.inf 866 | node_nearest = [] 867 | checked_set= set() 868 | cost_dict = {} 869 | rand_label = getLabel(x_rand, self.workspace, self.is_hybrid) 870 | 871 | for node in self.tree.nodes: 872 | if self.isIsolated(node) and node!= self.rt_root: 873 | continue 874 | 875 | x= node.pos 876 | dis = self.evaluator.euclideanDist(x_rand,x) 877 | 878 | # get the nearest: at least one exists 879 | if dis < min_dis: 880 | node_nearest = [node] 881 | min_dis = dis 882 | elif dis == min_dis: # no needs 883 | node_nearest.append(node) 884 | # near() 885 | if dis > self.near_dist: 886 | continue 887 | 888 | if node.pos not in checked_set: 889 | # check obs 890 | no_collision = collisionFreeAABB(node.pos, x_rand, self.workspace, self.old_dynobs_aabb) 891 | safe_label = self.checkCrossLabel(node, x_rand, rand_label) 892 | if not (no_collision and safe_label): 893 | continue 894 | 895 | cost_dict[(x_rand, node.pos)] = self.evaluator.getCost(node, x_rand) 896 | checked_set.add(node.pos) 897 | 898 | # this is why nearest_samp() is useful in simple case 899 | self.rand_near_nbrs.add(node) 900 | 901 | 902 | return node_nearest[0], cost_dict 903 | 904 | 905 | 906 | # naive steer 907 | def steer(self, x_rand, x_nearest): 908 | if self.evaluator.euclideanDist(x_rand, x_nearest) <= self.para['step_size']: 909 | return x_rand 910 | else: 911 | diff = np.subtract(x_rand, x_nearest) 912 | x_new = np.asarray(x_nearest) + self.para['step_size']* diff / np.linalg.norm(diff) 913 | return tuple(x_new) 914 | 915 | def expandAndRewire(self): 916 | self.calc_near_dist() 917 | # min < dist < max 918 | self.near_dist = min(self.near_dist, self.para['max_near_dist']) 919 | self.near_dist = max(self.near_dist, self.para['min_near_dist']) 920 | 921 | x_rand = self.sample() 922 | 923 | require_extend = True 924 | node_nearest, cost_dict = self.nearest_samp(x_rand) 925 | x_nearest = node_nearest.pos 926 | # if sampled at the existed node 927 | if self.evaluator.euclideanDist(x_nearest, x_rand) < 1e-2: 928 | require_extend = False 929 | # NOTE: rand_near_nodes are nodes around x_rand, not x_new! 930 | x_new = self.steer(x_rand, node_nearest.pos) 931 | new_label = getLabel(x_new, self.workspace, self.is_hybrid) 932 | 933 | if ptInObs(x_new, self.workspace): 934 | require_extend = False 935 | 936 | if require_extend and len(self.rand_near_nbrs) < self.para['k_max']: # or self.evaluator.euclideanDist(x_new, node_nearest) > self.sparse_r: 937 | # do rrt* things 938 | 939 | added_nodes = set() 940 | not_added_nodes = set() 941 | for b_state in self.automaton.nodes: 942 | 943 | # NOTE: In prefix, this job is done by checkNode2Goal 944 | if b_state in self.accept: 945 | continue 946 | # think about until expression 947 | if b_state in self.unreachable_b_states: 948 | continue 949 | 950 | new_node_id = self.added_nodes_cnt 951 | new_node = self.Node(new_node_id, x_new, np.inf, b_state,new_label) 952 | 953 | added = self.extend(new_node, self.rand_near_nbrs, cost_dict) 954 | if added: 955 | 956 | added_nodes.add(new_node) 957 | self.rewireRand.add(new_node) 958 | 959 | # check reach 960 | goal_updated = self.checkNode2Goal(new_node,self.is_hybrid) 961 | if goal_updated: 962 | print('find better goal') 963 | else: 964 | not_added_nodes.add(new_node) 965 | 966 | # add nodes from not_added_nodes to G 967 | for nd in not_added_nodes: 968 | nd.node_id= self.added_nodes_cnt 969 | self.addNode(nd) 970 | added_nodes.add(nd) 971 | 972 | for nd in added_nodes: 973 | nd.same_pos_nodes = added_nodes 974 | 975 | else: 976 | self.rewireRand.add(node_nearest) 977 | for nd in node_nearest.same_pos_nodes: 978 | if nd != node_nearest and self.calc_cost2Root(nd)!=np.inf: 979 | self.rewireRand.add(nd) 980 | 981 | self.rewireRandNode() 982 | 983 | self.rewireFromRoot() 984 | 985 | return x_rand 986 | 987 | 988 | def rewireRandNode(self): 989 | extendRewireRand = set() 990 | while len(self.rewireRand): 991 | Xr = self.rewireRand.pop() 992 | # NOTE: rewireRand is never cleared, nodes may be deleted by merge_branch 993 | if Xr.cost2Root == np.inf: 994 | continue 995 | 996 | near_nodes, cost_dict = self.near_succ(Xr) 997 | rewired_nodes = self.rewire(Xr, near_nodes, cost_dict) 998 | extendRewireRand = extendRewireRand.union(rewired_nodes) 999 | 1000 | idx = 0 1001 | while len(extendRewireRand) and idx<3: #self.allowRewiring(0.5*self.para['time_for_rewire']): # 1002 | idx +=1 1003 | Xr = extendRewireRand.pop() 1004 | # NOTE: rewireRand is never cleared, nodes may be deleted by merge_branch 1005 | if Xr.cost2Root == np.inf: 1006 | continue 1007 | 1008 | near_nodes, cost_dict = self.near_succ(Xr) 1009 | rewired_nodes = self.rewire(Xr, near_nodes, cost_dict) 1010 | extendRewireRand = extendRewireRand.union(rewired_nodes) 1011 | 1012 | 1013 | def allowRewiring(self, threth): 1014 | cur_time = rospy.Time.now() 1015 | if (cur_time- self.timeKeeper).to_sec() < threth: 1016 | return True 1017 | else: 1018 | return False 1019 | 1020 | 1021 | def rewireFromRoot(self): 1022 | if len(self.rewireRoot)== 0: 1023 | self.rewireRoot.add(self.rt_root) 1024 | 1025 | idx = 0 1026 | while len(self.rewireRoot) and self.allowRewiring(self.para['time_for_rewire']): # idx <=5: # 1027 | idx +=1 1028 | Xs = self.rewireRoot.pop() 1029 | # deleted 1030 | if Xs.visited == True: 1031 | continue 1032 | 1033 | near_nodes, cost_dict = self.near_succ(Xs) 1034 | # delete the parent node of Xs 1035 | if len(list(self.tree.pred[Xs].keys())): 1036 | Xs_parent = list(self.tree.pred[Xs].keys())[0] 1037 | if Xs_parent in near_nodes: 1038 | near_nodes.remove(Xs_parent) 1039 | obs_check_dict = None 1040 | rewired_nodes = self.rewire(Xs, near_nodes, cost_dict) 1041 | # add nodes which are not pushed into rewireRoot in current iter 1042 | self.rewireRoot = self.rewireRoot.union(rewired_nodes.difference(self.pushedToRewireRoot)) 1043 | self.pushedToRewireRoot = self.pushedToRewireRoot.union(rewired_nodes) 1044 | 1045 | def handle_self_state(self): 1046 | if len(self.cur_path) ==1 and self.evaluator.euclideanDist(self.rbt_pos, np.array(self.rt_root.pos))< self.para['goal_radius']: 1047 | self.task_initialized = False 1048 | print('TASK COMPLETE!') 1049 | 1050 | return True 1051 | 1052 | if self.seg_num >=2: 1053 | updated_root_id= -1 1054 | for i in range(1, self.seg_num): 1055 | 1056 | pos_to_arrive = self.cur_path[i].pos 1057 | if self.evaluator.euclideanDist(self.rbt_pos, np.array(pos_to_arrive))< self.para['changeRoot_radius']: 1058 | updated_root_id = i 1059 | break 1060 | 1061 | if updated_root_id >1: 1062 | print('Fast forward motion takes over!') 1063 | 1064 | # should change root 1065 | if updated_root_id> 0: 1066 | for j in range(0, updated_root_id): 1067 | self.cur_path.pop(0) #actually rt_root 1068 | new_root = self.cur_path[0] 1069 | 1070 | old_root = self.rt_root 1071 | require_prop = self.changeRoot(new_root) 1072 | if require_prop: 1073 | self.merge_branch(old_root, new_root) 1074 | 1075 | self.root_just_changed = True 1076 | 1077 | resume_path = self.validate_update_sols() 1078 | if not resume_path: 1079 | print('current path invalid') 1080 | 1081 | self.visited_set.clear() 1082 | self.rewireRoot.clear() 1083 | self.pushedToRewireRoot.clear() 1084 | 1085 | return False 1086 | 1087 | def plan_iteration(self, rbt_pos, rviz_mgr): 1088 | finished = False 1089 | 1090 | # in case self.detect_obs_move is set true during iteration. 1091 | # if obs_to_proc == 1, then set self.detect_obs_move as 0. 1092 | obs_to_proc = self.detect_obs_move 1093 | 1094 | self.rbt_pos = rbt_pos 1095 | if not self.task_initialized: 1096 | # rviz_mgr.clear_all() 1097 | return False, False 1098 | 1099 | self.dist_root2rbt = self.evaluator.euclideanDist(self.rbt_pos, np.array(self.rt_root.pos)) 1100 | 1101 | if self.next_wp is not None: 1102 | self.dist_next2rbt = self.evaluator.euclideanDist(self.rbt_pos, np.array(self.next_wp.pos)) 1103 | 1104 | if len(self.changed_regs): 1105 | reg = self.changed_regs.pop() 1106 | else: 1107 | reg = None 1108 | obs_no_affect = self.handle_perception(reg) 1109 | 1110 | if not self.root_just_changed and self.goalFound: 1111 | finished = self.handle_self_state() 1112 | 1113 | self.timeKeeper = rospy.Time.now() 1114 | x_rand = self.expandAndRewire() 1115 | 1116 | if len(self.possible_paths): 1117 | self.validate_update_sols() 1118 | 1119 | # post process 1120 | self.get_bad_regs() 1121 | 1122 | if rviz_mgr: 1123 | rviz_mgr.populateRviz() 1124 | 1125 | rviz_mgr.draw_rtroot(self.rt_root) 1126 | edges_list = list(self.tree.edges()) 1127 | rviz_mgr.display_edges(edges_list) 1128 | nd_ls = list(self.tree.nodes()) 1129 | leaf_ls = [nd for nd in nd_ls if self.tree.out_degree(nd) == 0] 1130 | rviz_mgr.draw_leaves(leaf_ls) 1131 | rviz_mgr.draw_empty() 1132 | # rviz_mgr.draw_rewired_root(self.pushedToRewireRoot) 1133 | rviz_mgr.drawFinalPath(self.cur_path) 1134 | 1135 | 1136 | self.rand_near_nbrs.clear() 1137 | self.detect_goal_changed = False 1138 | if obs_to_proc: 1139 | self.detect_obs_move = False 1140 | self.detect_TS_change = (len(self.changed_regs) != 0) 1141 | self.root_just_changed = False 1142 | 1143 | 1144 | return finished, obs_no_affect 1145 | 1146 | 1147 | def unify_automata(self, automaton, aut_nodes_dict): 1148 | 1149 | 1150 | 1151 | renamed_graph = automaton.copy() 1152 | renamed_graph = nx.relabel_nodes(renamed_graph, aut_nodes_dict) 1153 | 1154 | initial_set = set() 1155 | for nd in renamed_graph.graph['initial']: 1156 | initial_set.add(aut_nodes_dict[nd]) 1157 | renamed_graph.graph['initial'] = list(initial_set) 1158 | 1159 | accept_set = set() 1160 | for nd in renamed_graph.graph['accept']: 1161 | accept_set.add(aut_nodes_dict[nd]) 1162 | renamed_graph.graph['accept'] = list(accept_set) 1163 | 1164 | return renamed_graph 1165 | 1166 | def reset_task(self, new_buchi): 1167 | aut_dict = get_automaton_dict(new_buchi) 1168 | self.automaton = self.unify_automata(new_buchi, aut_dict) 1169 | self.accept = self.automaton.graph['accept'] 1170 | self.unreachable_b_states = self.preprocessDFA() 1171 | 1172 | self.possible_paths = [] 1173 | self.detect_TS_change = False 1174 | self.bad_regs = set() 1175 | self.dist_root2next = 0 1176 | self.dist_root2rbt = 0 1177 | self.dist_next2rbt = 0 1178 | 1179 | self.cur_target = None 1180 | self.cur_path = [] 1181 | self.rand_near_nbrs = set() 1182 | self.rewireRand = set() 1183 | self.visited_set = set() 1184 | self.rewireRoot = set() 1185 | self.pushedToRewireRoot = set() # has been pushed to rewireRoot within current iter 1186 | self.detect_goal_changed = False 1187 | self.detect_obs_move = False 1188 | self.invalid_set = set() 1189 | self.next_wp = None 1190 | 1191 | self.goalFound = False 1192 | self.nodesAroundGoal = [] 1193 | 1194 | self.task_initialized = True 1195 | 1196 | 1197 | def wrap_plan(self): 1198 | plan_skeleton = '' 1199 | for nd in self.cur_path: 1200 | node_text = '-> ' 1201 | node_text += '({:.2f},{:.2f}) '.format(nd.pos[0], nd.pos[1]) 1202 | if len(nd.label): 1203 | node_text+= '['+ list(nd.label)[0] +']' 1204 | plan_skeleton += node_text 1205 | 1206 | return plan_skeleton 1207 | 1208 | 1209 | def supplement_nodes(self): 1210 | # in case 1st time plan 1211 | if len(self.tree.nodes)<=1: 1212 | return 1213 | 1214 | new_b_set = set(self.automaton.nodes()) 1215 | new_b_set = new_b_set.difference(self.accept) 1216 | for pos, node in self.nodes_pos_dict.items(): 1217 | existed_b_list = [nd.b_state for nd in node.same_pos_nodes] 1218 | existed_b_set = set(existed_b_list) 1219 | lack_b_set = new_b_set.difference(existed_b_set) 1220 | 1221 | # add lost nodes 1222 | for b_state in lack_b_set: 1223 | lack_id = self.added_nodes_cnt 1224 | lack_node = self.Node(lack_id, pos, np.inf, b_state, node.label) 1225 | self.tree.add_node(lack_node) 1226 | self.added_nodes_cnt +=1 1227 | 1228 | node.same_pos_nodes.add(lack_node) 1229 | lack_node.same_pos_nodes = node.same_pos_nodes # addr of a list 1230 | 1231 | def reset_tree(self, rviz_mgr): 1232 | # in case 1st time plan 1233 | if len(self.tree.nodes)<=1: 1234 | return 1235 | 1236 | if len(self.automaton.graph['initial'])!=1: 1237 | raise NotImplementedError('more initials') 1238 | 1239 | # dummy_init 1240 | dummy_id = self.added_nodes_cnt 1241 | dummy_label = getLabel(self.rt_root.pos, self.workspace) 1242 | init_b_state = self.automaton.graph['initial'][0] 1243 | dummy_node = self.Node(dummy_id, self.rt_root.pos, 0, init_b_state, dummy_label) 1244 | dummy_node.same_pos_nodes.add(dummy_node) 1245 | self.addNode(dummy_node, self.rt_root) 1246 | self.added_nodes_cnt +=1 1247 | 1248 | old_root = self.rt_root 1249 | require_prop = self.changeRoot(dummy_node) 1250 | self.merge_branch(old_root, dummy_node) 1251 | 1252 | # delete added dummy root 1253 | self.tree.remove_edge(dummy_node, old_root) 1254 | self.safe_remove_node(dummy_node) 1255 | self.rt_root = old_root 1256 | -------------------------------------------------------------------------------- /LTLMP/src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JINXER000/multiple-task-temporal-logic-planning/16bc21cac81b733efd0e657791ac492d2f0696fc/LTLMP/src/__init__.py -------------------------------------------------------------------------------- /LTLMP/src/formula_parser/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JINXER000/multiple-task-temporal-logic-planning/16bc21cac81b733efd0e657791ac492d2f0696fc/LTLMP/src/formula_parser/__init__.py -------------------------------------------------------------------------------- /LTLMP/src/formula_parser/lexer.py: -------------------------------------------------------------------------------- 1 | import ply.lex as lex 2 | 3 | tokens = ( 4 | "SYMBOL", 5 | "AND", "OR", 6 | "NOT", 7 | "TRUE", 8 | "LPAREN", "RPAREN") 9 | 10 | t_SYMBOL = r"[a-z]+[a-z0-9_]*" 11 | t_TRUE = r"1" 12 | t_AND = r"&&" 13 | t_OR = r"\|\|" 14 | t_NOT = r"!" 15 | t_LPAREN = r"$" 16 | t_RPAREN = r"$" 17 | 18 | t_ignore = " " 19 | 20 | def t_error(t): 21 | print "Illegal character '%s'" % t.value[0] 22 | 23 | def get_lexer(): 24 | return lex.lex() 25 | -------------------------------------------------------------------------------- /LTLMP/src/formula_parser/ltlf2dfa.py: -------------------------------------------------------------------------------- 1 | #!/home/joseph/yzchen_ws/env/py3torch/bin/python3 2 | #/home/joseph/yzchen_ws/task_planning/ltl_ros_ws/src/src/P_MAS_TG/rrt_collection/formula_parser/ltlf2dfa.py 3 | import sys 4 | sys.path.append('/home/joseph/yzchen_ws/env/LTLf2DFA/') 5 | sys.path.append('/home/joseph/yzchen_ws/env/LTLf2DFA/ltlf2dfa/') 6 | from ltlf2dfa.parser.ltlf import LTLfParser 7 | 8 | 9 | if __name__ == '__main__': 10 | parser = LTLfParser() 11 | # formula_str = "F (pond && F grassland) && FG base" 12 | # formula_str = "(!grassland U pond) && F grassland " 13 | formula_str = "F (pond && F grassland)" 14 | formula = parser(formula_str) 15 | 16 | # dfa = formula.to_dfa() 17 | 18 | # path = "until.gv" 19 | # fp= open(path, "w") 20 | # test_gv = fp.write(dfa) 21 | # fp.close() -------------------------------------------------------------------------------- /LTLMP/src/formula_parser/ltlf_parser.py: -------------------------------------------------------------------------------- 1 | import re 2 | import networkx as nx 3 | from networkx.classes.digraph import DiGraph 4 | 5 | from parser import parse as parse_guard 6 | 7 | 8 | 9 | def find_symbols(formula): 10 | regex = re.compile(r"[a-z]+[a-z0-9]*") 11 | matches = regex.findall(formula) 12 | symbols = list() 13 | for match in matches: 14 | symbols += [match] 15 | symbols = list(set(symbols)) 16 | symbols.sort() 17 | return symbols 18 | 19 | 20 | def find_states(edges, dot_text): 21 | initial = set() 22 | accept = set() 23 | for (f,t) in edges.keys(): 24 | if f == 'init': 25 | initial.add(t) 26 | 27 | research = re.search(r'doublecircle];\s(\d+)', dot_text) 28 | accept.add(research.group(1)) 29 | 30 | return (list(initial), list(accept)) 31 | 32 | def getDFA(path, formula): 33 | fp= open(path) 34 | test_gv = fp.read() 35 | fp.close() 36 | 37 | G = DiGraph(nx.nx_pydot.read_dot(path)) 38 | 39 | initial, accept = find_states(G.edges, test_gv) 40 | symbols = find_symbols(formula) 41 | 42 | DFA = DiGraph(initial=initial, accept=accept, symbols=symbols) 43 | 44 | # G.graph['initial'] = initial 45 | # G.graph['accept'] = accept 46 | for nd in G.nodes: 47 | if nd == 'init': 48 | continue 49 | DFA.add_node(nd) 50 | 51 | for (ef,et) in G.edges.keys(): 52 | if ef == 'init': 53 | continue 54 | guard_formula = G.edges[(ef,et)]['label'].strip('"') 55 | guard_formula = guard_formula.replace('~', '!') 56 | guard_formula = guard_formula.replace('&', '&&') 57 | guard_formula = guard_formula.replace('|', '||') 58 | guard_formula = guard_formula.replace('true', '1') 59 | 60 | guard_expr = parse_guard(guard_formula) 61 | # G[ef][et]["guard"] = guard_expr 62 | DFA.add_edge(ef, et, guard=guard_expr, guard_formula=guard_formula) 63 | 64 | 65 | return DFA 66 | 67 | import os 68 | import subprocess 69 | root_dir = r'/home/dyl/yzchen_ws/task_ws/hmi_misc/' 70 | ltl_path = os.path.join(root_dir, 'output_ltl.txt') 71 | buchi_path = os.path.join(root_dir, 'rqt_dfa.gv') 72 | 73 | def call_LTLf2DFA(formula): 74 | 75 | with open(ltl_path, 'w') as fp: 76 | fp.write(formula) 77 | # translate to DFA 78 | ltl2aut_cmd = 'python3 /home/dyl/yzchen_ws/task_ws/LTLf2DFA/ltlf2dfa.py' 79 | process = subprocess.Popen(ltl2aut_cmd.split(), stdout=subprocess.PIPE) 80 | output, error = process.communicate() 81 | 82 | # directly read gv 83 | DFA = getDFA(buchi_path, formula) 84 | 85 | return DFA 86 | 87 | if __name__ == '__main__': 88 | path = "mona.gv" 89 | formula = '<> (pond && <> grassland) && <>[] pond' 90 | dfa = getDFA(path, formula) 91 | 92 | # regex = re.match(r"doublecircle];\s([0-9])", test_gv) 93 | 94 | # research = re.search(r'doublecircle];\s(\d+)', test_gv) 95 | 96 | 97 | 98 | 99 | 100 | print('TODO: get accept') 101 | -------------------------------------------------------------------------------- /LTLMP/src/formula_parser/parser.py: -------------------------------------------------------------------------------- 1 | from lexer import get_lexer 2 | import itertools 3 | 4 | 5 | class Expression(object): 6 | name = "Expression" 7 | def __iter__(self): 8 | raise NotImplementedError() 9 | 10 | def check(self, label): 11 | raise NotImplementedError() 12 | 13 | def distance(self, label): 14 | raise NotImplementedError() 15 | 16 | def nnf(self): 17 | return self 18 | 19 | class SymbolExpression(Expression): 20 | def __init__(self, name): 21 | self.name = name 22 | self.symbol = name 23 | 24 | def __repr__(self): 25 | return "SymbolExpression(%s)" % str(self.symbol) 26 | 27 | def __iter__(self): 28 | for expr in [self]: 29 | yield expr 30 | 31 | def children(self): 32 | return [] 33 | 34 | def check(self, label): 35 | return self.symbol in label 36 | 37 | def distance(self, label): 38 | if self.symbol in label: 39 | return 0 40 | else: 41 | return 1 42 | 43 | class NotSymbolExpression(Expression): 44 | def __init__(self, name): 45 | self.name = "!%s" % name 46 | self.symbol = name 47 | 48 | def __repr__(self): 49 | return "NotSymbolExpression(%s)" % str(self.symbol) 50 | 51 | def __iter__(self): 52 | for expr in [self]: 53 | yield expr 54 | 55 | def children(self): 56 | return [] 57 | 58 | def check(self, label): 59 | return self.symbol not in label 60 | 61 | def distance(self, label): 62 | if self.symbol not in label: 63 | return 0 64 | else: 65 | return 1 66 | 67 | class TrueExpression(Expression): 68 | name = "TRUE" 69 | def __init__(self): 70 | pass 71 | 72 | def __repr__(self): 73 | return "TrueExpression()" 74 | 75 | def __iter__(self): 76 | for expr in [self]: 77 | yield expr 78 | 79 | def children(self): 80 | return [] 81 | 82 | def check(self, label): 83 | return True 84 | 85 | def distance(self, label): 86 | return 0 87 | 88 | class NotExpression(Expression): 89 | name = "NOT" 90 | def __init__(self, inner): 91 | self.inner = inner 92 | 93 | def __repr__(self): 94 | return "NotExpression(%s)" % str(self.inner) 95 | 96 | def __iter__(self): 97 | for expr in itertools.chain([self], self.inner): 98 | yield expr 99 | 100 | def children(self): 101 | return [self.inner] 102 | 103 | def check(self, label): 104 | return not self.inner.check(label) 105 | 106 | def nnf(self): 107 | if isinstance(self.inner, SymbolExpression): 108 | s = NotSymbolExpression(self.inner.name) 109 | return s 110 | elif isinstance(self.inner, ORExpression): 111 | left = NotExpression(self.inner.left).nnf() 112 | right = NotExpression(self.inner.right).nnf() 113 | s = ANDExpression(left, right) 114 | return s 115 | elif isinstance(self.inner, ANDExpression): 116 | left = NotExpression(self.inner.left).nnf() 117 | right = NotExpression(self.inner.right).nnf() 118 | s = ORExpression(left, right) 119 | return s 120 | # revised by joseph 121 | elif isinstance(self.inner, NotExpression): 122 | s = self.inner.inner 123 | return s 124 | raise Exception("Unexpected child of NotExpression") 125 | 126 | class BinExpression(Expression): 127 | def __init__(self, left, right): 128 | self.left = left 129 | self.right = right 130 | 131 | def __iter__(self): 132 | for expr in itertools.chain([self], self.left, self.right): 133 | yield expr 134 | 135 | def children(self): 136 | return [self.left, self.right] 137 | 138 | def nnf(self): 139 | self.left = self.left.nnf() 140 | self.right = self.right.nnf() 141 | return self 142 | 143 | class ORExpression(BinExpression): 144 | name = "OR" 145 | def __repr__(self): 146 | return "ORExpression(%s, %s)" % (str(self.left), str(self.right)) 147 | 148 | def check(self, label): 149 | return self.left.check(label) or self.right.check(label) 150 | 151 | def distance(self, label): 152 | ldist = self.left.distance(label) 153 | rdist = self.right.distance(label) 154 | return min([ldist, rdist]) 155 | 156 | class ANDExpression(BinExpression): 157 | name = "AND" 158 | def __repr__(self): 159 | return "ANDExpression(%s, %s)" % (str(self.left), str(self.right)) 160 | 161 | def check(self, label): 162 | return self.left.check(label) and self.right.check(label) 163 | 164 | def distance(self, label): 165 | return self.left.distance(label) + self.right.distance(label) 166 | 167 | class Parser(object): 168 | def __init__(self, formula): 169 | lexer = get_lexer() 170 | lexer.input(formula) 171 | self.formula = formula 172 | self.tokens = list(lexer) 173 | 174 | def symbols(self): 175 | syms = list() 176 | for token in self.tokens: 177 | if token.type == "SYMBOL": 178 | syms += [token.value] 179 | return list(set(syms)) 180 | 181 | def parse(self): 182 | expr = self.orx() # recursively construct syntax tree: or; and; not 183 | expr = expr.nnf() # transform the tree to negative normal form 184 | expr.formula = self.formula 185 | return expr 186 | 187 | def orx(self): 188 | lhs = self.andx() 189 | if len(self.tokens) == 0 or self.tokens[0].type == "RPAREN": 190 | return lhs 191 | elif self.tokens[0].type == "OR": 192 | self.tokens.pop(0) 193 | rhs = self.andx() 194 | lhs = ORExpression(lhs, rhs) 195 | while len(self.tokens) > 0 and self.tokens[0].type == "OR": 196 | self.tokens.pop(0) 197 | rhs = self.andx() 198 | lhs = ORExpression(lhs, rhs) 199 | return lhs 200 | else: 201 | raise Exception("Expected OR, RPAREN or nothing but got %s" % self.tokens[0]) 202 | 203 | def andx(self): 204 | lhs = self.notx() 205 | if len(self.tokens) == 0 or self.tokens[0].type in ["OR", "RPAREN"]: 206 | return lhs 207 | elif self.tokens[0].type == "AND": 208 | self.tokens.pop(0) 209 | rhs = self.notx() 210 | lhs = ANDExpression(lhs, rhs) 211 | while len(self.tokens) > 0 and self.tokens[0].type == "AND": 212 | self.tokens.pop(0) 213 | rhs = self.notx() 214 | lhs = ANDExpression(lhs, rhs) 215 | return lhs 216 | else: 217 | raise Exception("Expected OR, AND or nothing but got %s" % self.tokens[0]) 218 | 219 | def notx(self): 220 | if self.tokens[0].type == "NOT": 221 | self.tokens.pop(0) 222 | return NotExpression(self.parx()) 223 | else: 224 | return self.parx() 225 | 226 | def parx(self): 227 | if self.tokens[0].type == "LPAREN": 228 | self.tokens.pop(0) 229 | expr = self.orx() 230 | if self.tokens[0].type != "RPAREN": 231 | raise Exception("Expected RPAREN but got %s" % self.tokens[0]) 232 | self.tokens.pop(0) 233 | elif self.tokens[0].type == "SYMBOL": 234 | expr = SymbolExpression(self.tokens[0].value) 235 | self.tokens.pop(0) 236 | elif self.tokens[0].type == "TRUE": 237 | expr = TrueExpression() 238 | self.tokens.pop(0) 239 | else: 240 | raise Exception("Expected LPAREN or SYMBOL but got %s" % self.tokens[0]) 241 | return expr 242 | 243 | def parse(formula): 244 | parser = Parser(formula) 245 | return parser.parse() 246 | 247 | if __name__ == '__main__': 248 | guard_expr = parse('!(a || b) && c') 249 | # guard_expr = parse('!a || b') 250 | # syntax tree: or; and; not; paranthes; or; and; not; symbol 251 | 252 | # NOTE: formula itself won't be transformed to nnf 253 | -------------------------------------------------------------------------------- /LTLMP/src/gazebo_HMI_plugin.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from utils.workspace import Workspace3D 3 | from utils.misc_func import getLabel 4 | from RTLTL import RT_LTL_planner 5 | from utils.naive_evaluator import evaluator 6 | from formula_parser.ltlf_parser import getDFA 7 | from ros_gazebo_HMI_base import gazebo_HMI_base, DiscreteMode 8 | 9 | import rospy 10 | 11 | import os 12 | 13 | 14 | root_dir = r'/home/joseph/yzchen_ws/task_planning/ltl_translate/ltl-amdp/language/models/torch/rqt_IO' 15 | buchi_path = os.path.join(root_dir, 'dfa_remote.gv') 16 | 17 | class motionA2B_HMI_RTMQ_ros(gazebo_HMI_base): 18 | def __init__(self, workspace, para): 19 | 20 | super(motionA2B_HMI_RTMQ_ros, self).__init__(workspace, para) 21 | 22 | 23 | def translate_LTL(self, formula): 24 | # invalid input (get from rosparam) 25 | if formula == 0 or formula[0] == '0': 26 | return 0 27 | 28 | DFA = getDFA(buchi_path, formula) 29 | self.pub_msg_rqt('Automata constructed', self.exeInfo_pub) 30 | return DFA 31 | 32 | def on_initialize(self, init_pos, workspace): 33 | init_label = getLabel(init_pos, workspace) 34 | 35 | root = RT_LTL_planner.Node(0, init_pos, 0, self.automaton.graph['initial'][0], init_label) 36 | root.same_pos_nodes.add(root) 37 | dist_eval = evaluator() 38 | self.LTLMP = RT_LTL_planner(root, None, workspace, dist_eval, self.para, self.automaton, 'prefix') 39 | 40 | rospy.Timer(rospy.Duration(1), self.checkShortCut) 41 | 42 | 43 | def on_update(self): 44 | # in case during planning, the tree is reset by rqt_monitor() 45 | self.mutex.acquire() 46 | 47 | if self.mode == DiscreteMode.planning and self.LTLMP.goalFound: 48 | self.mode = DiscreteMode.executing 49 | initplan_time = (rospy.Time.now() - self.initplan_start_time).to_sec() 50 | print('initplan_time is ' + str(initplan_time)) 51 | self.pub_msg_rqt('initplan_time is ' + str(initplan_time) + 's', self.exeInfo_pub) 52 | 53 | plan_skeleton = self.LTLMP.wrap_plan() 54 | self.pub_msg_rqt(plan_skeleton, self.trace_pub) 55 | 56 | ready_obs = False 57 | if self.LTLMP.detect_obs_move: 58 | ready_obs = True 59 | 60 | ready_TS = False 61 | if self.LTLMP.detect_TS_change: 62 | ready_TS = True 63 | 64 | finished, obs_no_affect = self.LTLMP.plan_iteration(self.rbt_pos, self.rviz_mgr) 65 | 66 | # if need replan 67 | if ((ready_obs and obs_no_affect == False) or ready_TS) and self.doing_replan == False: 68 | self.doing_replan = True 69 | self.replan_start_time = rospy.Time.now() 70 | 71 | log_text = "Prior knowledge incorrect, replanning!" 72 | if not ready_TS: 73 | log_text = "Blocked by obstacles, replanning! " 74 | self.pub_msg_rqt(log_text, self.exeInfo_pub) 75 | 76 | # if the replan succeed 77 | if self.doing_replan == True and self.LTLMP.goalFound == True and obs_no_affect == False: 78 | self.doing_replan = False 79 | self.replan_total_times += 1 80 | replan_time = (rospy.Time.now() - self.replan_start_time).to_sec() 81 | self.replan_total_secs += replan_time 82 | print('replan using ' + str(replan_time) + ' s') 83 | self.pub_msg_rqt('replan using ' + str(replan_time) + 's', self.exeInfo_pub) 84 | 85 | plan_skeleton = self.LTLMP.wrap_plan() 86 | self.pub_msg_rqt(plan_skeleton, self.trace_pub) 87 | 88 | if finished: 89 | finish_time = rospy.Time.now() 90 | total_time = (finish_time - self.start_time).to_sec() 91 | print('elapse time is ' + str(total_time)) 92 | if self.replan_total_times > 0: 93 | print('avg replan is ' + str(self.replan_total_secs / self.replan_total_times)) 94 | 95 | self.mode = DiscreteMode.notInit 96 | self.pub_msg_rqt('elapse time is ' + str(total_time) + 's', self.exeInfo_pub) 97 | 98 | if self.LTLMP.goalFound: 99 | ## use astar 100 | path_seg = [] 101 | # # if cur_pose is not the 1st wp 102 | for nd in self.LTLMP.cur_path: 103 | path_seg.append(nd.pos) 104 | 105 | if len(self.LTLMP.rt_root.label): 106 | if len(nd.label) and list(nd.label)[0] != list(self.LTLMP.rt_root.label)[0]: 107 | break 108 | else: 109 | if len(nd.label): 110 | break 111 | # for fast forward motion 112 | self.LTLMP.seg_num = len(path_seg) 113 | if len(path_seg): 114 | self.pub_temp_goal(path_seg[-1]) 115 | 116 | self.mutex.release() 117 | 118 | 119 | def on_reset_task(self): 120 | # reset the planner 121 | self.LTLMP.reset_task(self.automaton) 122 | self.LTLMP.supplement_nodes() 123 | self.LTLMP.reset_tree(self.rviz_mgr) 124 | 125 | def checkShortCut(self, event): 126 | if not self.doing_replan and self.mode != DiscreteMode.planning: 127 | return 128 | self.LTLMP.check_all_nodes_to_goal() 129 | 130 | 131 | if __name__ == '__main__': 132 | # temp for test 133 | rospy.set_param('workspace_choice', 'Vicon room') 134 | 135 | workspace = Workspace3D() 136 | 137 | para = dict() 138 | para['step_size'] = np.inf 139 | para['real_time_plot'] = False 140 | para['goal_radius'] = 1 141 | para['time_for_rewire'] = 0.2 142 | 143 | # tell if to change root 144 | para['changeRoot_radius'] = 0.4 145 | # same solution radius 146 | para['sameSol_radius'] = 2 147 | # region bias sampling 148 | para['reg_sample_ratio'] = 0.24 149 | # near dist 150 | para['min_near_dist'] = 3 151 | para['max_near_dist'] = 5 152 | para['k_max'] = 100 153 | 154 | para['bias_sample'] = True 155 | 156 | para['fov_dist'] = 2 157 | para['reg_change_ls'] = ['l1', 'l3'] 158 | 159 | node = motionA2B_HMI_RTMQ_ros(workspace, para) 160 | 161 | -------------------------------------------------------------------------------- /LTLMP/src/ros_gazebo_HMI_base.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | from utils.misc_func import genWorkspacePntCld, fovSense, setup_pybullet 5 | from utils.tree_visualizer import rviz_maker 6 | import threading 7 | 8 | import rospy 9 | from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped 10 | from nav_msgs.msg import Path 11 | from sensor_msgs.msg import PointCloud2 12 | import std_msgs.msg 13 | import sensor_msgs.point_cloud2 as pcl2 14 | from nav_msgs.msg import Odometry 15 | from std_msgs.msg import String 16 | from tf.transformations import quaternion_from_euler 17 | 18 | from ltl3.msg import activity_pose 19 | 20 | from enum import Enum 21 | DiscreteMode = Enum('DiscreteMode', ('notInit', 'planning', 'executing')) 22 | 23 | robot_name = '' 24 | 25 | class gazebo_HMI_base(object): 26 | def __init__(self, workspace, para): 27 | rospy.init_node('HMI_LTL', anonymous=False) 28 | 29 | self.node_init_common(para, workspace) 30 | 31 | self.exeInfo_pub = rospy.Publisher("/HMI/exe_info", String, queue_size=1) 32 | self.trace_pub = rospy.Publisher("/HMI/plan_skeleton", String, queue_size=1) 33 | 34 | # customized initialize 35 | self.MP_initialized = False 36 | # wait until got rbt pose 37 | init_pos = self.rbt_pos 38 | while init_pos is None: 39 | init_pos = self.rbt_pos 40 | 41 | goal = None 42 | self.rviz_mgr = rviz_maker(init_pos, goal, workspace, 1, True) 43 | 44 | self.automaton = None 45 | rospy.Timer(rospy.Duration(1.0), self.rqt_monitor) 46 | rospy.set_param('cur_task', 0) 47 | 48 | setup_pybullet(workspace, use_gui = False) 49 | 50 | # wait for formula 51 | while self.mode == DiscreteMode.notInit or self.automaton == 0: 52 | pass 53 | 54 | 55 | self.on_initialize(init_pos, workspace) 56 | 57 | self.MP_initialized = True 58 | 59 | rospy.Timer(rospy.Duration(0.1), self.rbt_routine) 60 | 61 | self.start_time = rospy.Time.now() 62 | rospy.spin() 63 | 64 | 65 | # MUST IMPLEMENT THIS 66 | def translate_LTL(self, formula): 67 | raise NotImplementedError('LTLf2DFA or LTL2BA?') 68 | 69 | def on_reset_task(self): 70 | raise NotImplementedError('This planner does not support multi-query') 71 | 72 | def rqt_monitor(self, event): 73 | 74 | 75 | if self.mode == DiscreteMode.notInit: 76 | self.mutex.acquire() 77 | 78 | self.rviz_mgr.populateRviz() 79 | formula = rospy.get_param('cur_task') 80 | 81 | automaton = self.translate_LTL(formula) 82 | if automaton != 0: 83 | 84 | self.automaton = automaton 85 | 86 | print ('get task: '+ formula) 87 | # for multi query 88 | if self.MP_initialized: 89 | self.on_reset_task() 90 | 91 | self.start_time = rospy.Time.now() 92 | self.initplan_start_time = rospy.Time.now() 93 | 94 | self.mode = DiscreteMode.planning 95 | rospy.set_param('cur_task', 0) 96 | 97 | self.mutex.release() 98 | else: # abort 99 | pass 100 | 101 | 102 | 103 | def pub_msg_rqt(self, text, puber): 104 | info_msg = String() 105 | info_msg.data = text 106 | puber.publish(info_msg) 107 | 108 | 109 | 110 | 111 | def node_init_common(self, para, workspace): 112 | self.para = para 113 | 114 | self.mutex = threading.Lock() 115 | 116 | self.mode = DiscreteMode.notInit 117 | self.doing_replan = False 118 | self.replan_start_time = None 119 | self.replan_total_secs = 0 120 | self.replan_total_times = 0 121 | self.start_time = rospy.Time.now() 122 | self.initplan_start_time = rospy.Time.now() 123 | 124 | self.pynt_stamp_idx = 0 125 | self.pynt_pub = rospy.Publisher("/forbid_reg_cloud", PointCloud2, queue_size=1) 126 | 127 | self.cmd_header = 0 128 | self.temp_goal_pub = rospy.Publisher(robot_name+"/move_act", activity_pose) 129 | self.path_pub = rospy.Publisher('/cur_path', Path, queue_size=1) 130 | 131 | 132 | # moving obs related 133 | # not for real test! 134 | self.obs_move_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1) 135 | self.obs_initialize() 136 | self.new_goal_sub = rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, self.obs_move_CB, queue_size=1) 137 | self.new_dynamic_obs = workspace.dynamic_obs_center 138 | 139 | # rospy.Timer(rospy.Duration(0.2), self.move_obs_fake) 140 | 141 | self.rbt_pos = None 142 | self.rbt_odom_sub = rospy.Subscriber(robot_name+'/odom', Odometry, self.rbt_odom_CB, queue_size=1) #/iris_0 143 | 144 | 145 | 146 | # MUST IMPLEMENT THIS 147 | def on_initialize(self, init_pos, workspace): 148 | raise NotImplementedError('must implement initialize') 149 | 150 | def rbt_routine(self, event): 151 | # perception 152 | # self.LTLMP.workspace.update_dynamic_obstacle(self.new_dynamic_obs) 153 | self.do_perception() 154 | self.pub_pntcld() 155 | 156 | # planning 157 | self.on_update() 158 | 159 | # MUST IMPLEMENT THIS 160 | def on_update(self): 161 | raise NotImplementedError('must implement update') 162 | 163 | def rbt_odom_CB(self, msg): 164 | # tuples are immutable therefore a = sign does not create a reference but a copy as expected. 165 | 166 | rbt_pos_x = msg.pose.pose.position.x 167 | rbt_pos_y = msg.pose.pose.position.y 168 | rbt_pos_z = msg.pose.pose.position.z 169 | self.rbt_pos = (rbt_pos_x, rbt_pos_y, rbt_pos_z) 170 | 171 | 172 | 173 | # same condition for each planner 174 | def do_perception(self): 175 | reg_name = fovSense(self.rbt_pos, self.para['fov_dist'], self.LTLMP.workspace, self.para['reg_change_ls']) 176 | if reg_name is not None and len(self.rviz_mgr.changed_regs)==0: 177 | 178 | self.LTLMP.workspace.properties[reg_name] = '' 179 | 180 | self.LTLMP.detect_TS_change = True 181 | self.LTLMP.changed_regs.add(reg_name) 182 | self.rviz_mgr.changed_regs.add( reg_name) 183 | 184 | self.obs_detection() 185 | 186 | def obs_detection(self): 187 | sensed_dynamic_obs = self.circle_sense_obs(self.rbt_pos, self.LTLMP.workspace.dynamic_obs_center) 188 | 189 | # sensed_dynamic_obs = sense_obs(self.rbt_pos, self.para['fov_dist'], self.LTLMP.workspace, 'd3') 190 | if sensed_dynamic_obs: 191 | cur_dynobs_pos = self.LTLMP.workspace.dynamic_obs_center 192 | obs_diff = self.LTLMP.evaluator.euclideanDist(self.LTLMP.old_dynamic_obs, cur_dynobs_pos) 193 | if obs_diff>3: 194 | self.LTLMP.detect_obs_move = True 195 | print('OBS move detected') 196 | 197 | self.LTLMP.old_dynamic_obs = cur_dynobs_pos 198 | self.LTLMP.old_dynobs_aabb = self.LTLMP.workspace.dyn_obs['d3'] 199 | 200 | 201 | def obs_initialize(self): 202 | self.dyn_obs_mover_msg = PoseWithCovarianceStamped() 203 | self.obs_move_dir = -1 204 | self.dyn_obs_mover_msg.header.stamp= rospy.Time.now() 205 | self.dyn_obs_mover_msg.header.frame_id = 'map' 206 | self.dyn_obs_mover_msg.pose.pose.position.x = -2.5 207 | self.dyn_obs_mover_msg.pose.pose.position.y = -0.5 208 | self.dyn_obs_mover_msg.pose.pose.position.z = -1 209 | self.obs_move_pub.publish(self.dyn_obs_mover_msg) 210 | 211 | def move_obs_fake(self, event): 212 | move_speed = 0.02 213 | cur_obs_axis = self.dyn_obs_mover_msg.pose.pose.position.x 214 | if cur_obs_axis>3.5: 215 | self.obs_move_dir = -1 216 | elif cur_obs_axis<-1: 217 | self.obs_move_dir = 1 218 | 219 | self.dyn_obs_mover_msg.pose.pose.position.x +=self.obs_move_dir*move_speed 220 | self.obs_move_pub.publish(self.dyn_obs_mover_msg) 221 | 222 | def obs_move_CB(self, msg): # covairance have pose.pose 223 | # self.mutex.acquire() 224 | 225 | new_obs_x= msg.pose.position.x/self.rviz_mgr.scale 226 | new_obs_y = msg.pose.position.y/self.rviz_mgr.scale 227 | 228 | # new_dynamic_obs = (new_obs_x, new_obs_y) 229 | # self.LTLMP.workspace.update_dynamic_obstacle(new_dynamic_obs) 230 | # print('received new dyn obs pos: '+new_dynamic_obs) 231 | 232 | # self.mutex.release() 233 | 234 | 235 | def circle_sense_obs(self, rbt_pos, obs_pos): 236 | obs_dist = self.LTLMP.evaluator.euclideanDist(rbt_pos, obs_pos) 237 | if obs_dist < self.para['fov_dist']: 238 | return True 239 | else: 240 | return False 241 | 242 | 243 | 244 | 245 | def pub_pntcld(self): 246 | # if pynt changes, then 247 | # self.handle_pynt_changes() 248 | 249 | cld_pynts = genWorkspacePntCld(self.LTLMP.workspace, self.LTLMP.bad_regs, self.LTLMP.old_dynobs_aabb) 250 | 251 | 252 | #header 253 | header = std_msgs.msg.Header() 254 | header.stamp = rospy.Time.now() 255 | header.frame_id = 'map' 256 | # header.frame_id = str(self.pynt_stamp_idx) 257 | #create pcl from points 258 | scaled_polygon_pcl = pcl2.create_cloud_xyz32(header, cld_pynts) 259 | 260 | self.pynt_pub.publish(scaled_polygon_pcl) 261 | 262 | def calc_heading(self, pt1, pt2): 263 | pt_diff_x = pt2[0] - pt1[0] 264 | pt_diff_y = pt2[1] - pt1[1] 265 | yaw = np.arctan2(pt_diff_y, pt_diff_x) 266 | quat = quaternion_from_euler(0, 0, yaw) 267 | return quat 268 | 269 | 270 | def prepare_path_msg(self, path_seg): 271 | path_out = Path() 272 | path_out.header.stamp = rospy.Time.now() 273 | path_out.header.frame_id = "map" 274 | 275 | start_pos = path_seg[0] 276 | for nd_pos in path_seg: 277 | nd_pos_msg = PoseStamped() 278 | nd_pos_msg.pose.position.x = nd_pos[0] 279 | nd_pos_msg.pose.position.y = nd_pos[1] 280 | nd_pos_msg.pose.position.z = 1 281 | # calc heading 282 | if start_pos != nd_pos: 283 | quat = self.calc_heading(start_pos, nd_pos) 284 | nd_pos_msg.pose.orientation.w = 1 285 | nd_pos_msg.pose.orientation.x = 0 286 | nd_pos_msg.pose.orientation.y = 0 287 | nd_pos_msg.pose.orientation.z = 0 288 | else: 289 | nd_pos_msg.pose.orientation.w = 1 290 | nd_pos_msg.pose.orientation.x = 0 291 | nd_pos_msg.pose.orientation.y = 0 292 | nd_pos_msg.pose.orientation.z = 0 293 | 294 | path_out.poses.append(nd_pos_msg) 295 | 296 | return path_out 297 | 298 | 299 | 300 | def pub_temp_goal(self, nxt_pos = None): 301 | act_pos = activity_pose() 302 | act_pos.header = self.cmd_header 303 | 304 | if nxt_pos is None: 305 | # in case arrive target 306 | if self.LTLMP.next_wp is None: 307 | return 308 | nxt_pos = self.LTLMP.next_wp.pos 309 | 310 | goal_msg = PoseStamped() 311 | goal_msg.header.stamp = rospy.Time.now() 312 | goal_msg.header.frame_id = 'map' 313 | 314 | 315 | goal_msg.pose.orientation.w = 1 316 | goal_msg.pose.orientation.x = 0 317 | goal_msg.pose.orientation.y = 0 318 | goal_msg.pose.orientation.z = 0 319 | 320 | if type(nxt_pos) is str: 321 | act_pos.type = nxt_pos 322 | goal_msg.pose.position.x = self.rbt_pos[0] 323 | goal_msg.pose.position.y = self.rbt_pos[1] 324 | goal_msg.pose.position.z = self.rbt_pos[2] 325 | 326 | else: 327 | act_pos.type = "pass_by" 328 | goal_msg.pose.position.x = nxt_pos[0] 329 | goal_msg.pose.position.y = nxt_pos[1] 330 | goal_msg.pose.position.z = nxt_pos[2] 331 | 332 | 333 | self.cmd_header+=1 334 | 335 | act_pos.tgt_pose = goal_msg 336 | self.temp_goal_pub.publish(act_pos) 337 | -------------------------------------------------------------------------------- /LTLMP/src/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JINXER000/multiple-task-temporal-logic-planning/16bc21cac81b733efd0e657791ac492d2f0696fc/LTLMP/src/utils/__init__.py -------------------------------------------------------------------------------- /LTLMP/src/utils/misc_func.py: -------------------------------------------------------------------------------- 1 | import random 2 | 3 | import pybullet as p 4 | from pyb_plan_utils import connect, get_aabb_extent, load_model, disconnect, wait_if_gui, create_box, set_point, dump_body, \ 5 | TURTLEBOT_URDF, HideOutput, LockRenderer, joint_from_name, set_euler, get_euler, get_point, \ 6 | set_joint_position, get_joint_positions, pairwise_collision, stable_z, wait_for_duration, get_link_pose, \ 7 | link_from_name, get_pose, euler_from_quat, multiply, invert, draw_pose, unit_point, unit_quat, \ 8 | remove_debug, get_aabb, draw_aabb, get_subtree_aabb, ROOMBA_URDF, set_all_static, assign_link_colors, \ 9 | set_camera_pose, RGBA, draw_point, AABB, get_aabb_center, aabb_intersection 10 | 11 | # RGBA colors (alpha is transparency) 12 | RED = RGBA(1, 0, 0, 1) 13 | TAN = RGBA(0.824, 0.706, 0.549, 1) 14 | 15 | from itertools import product 16 | 17 | def get_automaton_dict(automaton, off_set=0): 18 | aut_nodes_dict = {} 19 | auto_nodes_ls = list(automaton.nodes) 20 | for i in range(len(auto_nodes_ls)): 21 | aut_nodes_dict[auto_nodes_ls[i]] = i + off_set 22 | 23 | return aut_nodes_dict 24 | 25 | 26 | def swap_dict(dictionary): 27 | return {value: key for key, value in dictionary.items()} 28 | 29 | # AABB defined in pyb_plan_utils 30 | 31 | class LineSegment: 32 | def __init__(self, start_point, end_point): 33 | self.start = start_point 34 | self.end = end_point 35 | 36 | def LinesegIntersectsAABB(aabb, line): 37 | tmin = 0.0 38 | tmax = 1.0 39 | 40 | for i in range(3): 41 | if abs(line.end[i] - line.start[i]) < 1e-9: # line segment is parallel to slab in this dimension 42 | if line.start[i] < aabb.lower[i] or line.start[i] > aabb.upper[i]: # line segment is outside of slab 43 | return False 44 | else: 45 | ood = 1.0 / (line.end[i] - line.start[i]) 46 | t1 = (aabb.lower[i] - line.start[i]) * ood 47 | t2 = (aabb.upper[i] - line.start[i]) * ood 48 | # t1 is the lower, t2 is the upper 49 | if t1 > t2: 50 | t1, t2 = t2, t1 51 | tmin = max(tmin, t1) 52 | tmax = min(tmax, t2) 53 | if tmin > tmax: 54 | return False 55 | 56 | return True 57 | 58 | # NOTE: if both points are inside obs, it will tell us NO COLLISION! 59 | def pybCheckCollision(p0, p1, dyn_obs_bdr= None): 60 | if p0 == p1: 61 | raise NameError("Cannot use it to check a point!") 62 | 63 | result = p.rayTest(p0, p1) 64 | if result[0][0] == -1: 65 | return False 66 | else: 67 | return True 68 | 69 | def collisionFreeAABB(p0, p1, workspace, dyn_obs_aabb= None): 70 | return not pybCheckCollision(p0, p1, dyn_obs_aabb) 71 | 72 | # not changing b_state if crossing 73 | for (obs, aabb) in iter(workspace.obstacles.items()): 74 | is_intersect = LinesegIntersectsAABB(aabb, LineSegment(p0, p1)) 75 | if is_intersect: 76 | return False 77 | 78 | # check dynamic obstacle 79 | if dyn_obs_aabb is None: 80 | return True 81 | 82 | for (d_obs, aabb) in iter(workspace.dyn_obs.items()): 83 | is_intersect = LinesegIntersectsAABB(aabb, LineSegment(p0, p1)) 84 | if is_intersect: 85 | return False 86 | return True 87 | 88 | def ptInObs(x, workspace): 89 | # obstacles 90 | for (obs, obs_AABB) in iter(workspace.obstacles.items()): 91 | if isWithinAABB(x, obs_AABB): 92 | return True 93 | return False 94 | 95 | 96 | def getLabel(x, workspace, is_hybrid = False): 97 | ret_label_set = set() 98 | 99 | # obstacles 100 | if ptInObs(x, workspace): 101 | ret_label_set.add('o') 102 | return ret_label_set 103 | 104 | # regions 105 | for (reg, reg_AABB) in iter(workspace.regions.items()): 106 | if isWithinAABB(x, reg_AABB): 107 | ret_lab = workspace.properties[reg] 108 | ret_label_set.add(ret_lab) 109 | 110 | # for hybrid 111 | if is_hybrid: 112 | media_lab = workspace.in_which_media(x) 113 | ret_label_set.add(media_lab) 114 | 115 | return ret_label_set 116 | 117 | def isWithinAABB(point, aabb): 118 | return all(min_coord <= point_coord <= max_coord for min_coord, point_coord, max_coord in zip(aabb.lower, point, aabb.upper)) 119 | 120 | def randomPointAABB(aabb): 121 | """ 122 | Samples a random point within an AABB. 123 | 124 | Args: 125 | aabb: A tuple of two lists of 3 floats each representing the min and max coordinates of the AABB. 126 | 127 | Returns: 128 | A list of 3 floats representing the coordinates of the sampled point. 129 | """ 130 | min_coords, max_coords = aabb 131 | rand_pt = [random.uniform(min_coord, max_coord) for min_coord, max_coord in zip(min_coords, max_coords)] 132 | return tuple(rand_pt) 133 | 134 | 135 | def fovSense(rbt_pos, fov_dist, workspace, reg_ls =[], obs_ls = [] ): 136 | fov_ll = [rbt_pos[i] - fov_dist * 0.5 for i in range(len(rbt_pos))] 137 | fov_ur = [rbt_pos[i] + fov_dist * 0.5 for i in range(len(rbt_pos))] 138 | fov_aabb = AABB(fov_ll, fov_ur) 139 | 140 | if len(reg_ls) and len(obs_ls): 141 | raise NameError('Cannot deal with them simultaneously!') 142 | 143 | if len(reg_ls): 144 | for reg_name in reg_ls: 145 | sensed = aabb_intersection(fov_aabb, workspace.regions[reg_name]) 146 | if sensed: 147 | return reg_name 148 | else: 149 | for obs_name in obs_ls: 150 | sensed = aabb_intersection(fov_aabb, workspace.dyn_obs[obs_name]) 151 | if sensed: 152 | return obs_name 153 | 154 | return None 155 | 156 | ############# pyb drawing 157 | 158 | def pyb_draw_sphere_marker(position, radius, color): 159 | vs_id = p.createVisualShape(p.GEOM_SPHERE, radius=radius, rgbaColor=color) 160 | marker_id = p.createMultiBody(basePosition=position, baseCollisionShapeIndex=-1, baseVisualShapeIndex=vs_id) 161 | return marker_id 162 | 163 | def pyb_remove_marker(marker_id): 164 | p.removeBody(marker_id) 165 | 166 | def pyb_draw_edges(edges, line_color=[0, 0, 1], line_width=1): 167 | for edge in edges: 168 | start = edge[0].pos 169 | end = edge[1].pos 170 | p.addUserDebugLine(start, end, line_color, line_width) 171 | 172 | def pyb_remove_edges(): 173 | p.removeAllUserDebugItems() 174 | 175 | 176 | 177 | def setup_pybullet(workspace, use_gui = False): 178 | # Creates a pybullet world and a visualizer for it 179 | connect(use_gui=use_gui) 180 | if use_gui: 181 | set_camera_pose(camera_point=[5, -5, 5], target_point=unit_point()) # Sets the camera's position 182 | identity_pose = (unit_point(), unit_quat()) 183 | origin_handles = draw_pose(identity_pose, 184 | length=1.0) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE) 185 | 186 | 187 | # Bodies are described by an integer index 188 | floor = create_box(w=workspace.width, l=workspace.length, h=0.001, color=TAN) # Creates a tan box object for the floor 189 | set_point(floor, [0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor 190 | 191 | for obs_aabb in workspace.obstacles.values(): 192 | center = get_aabb_center(obs_aabb) 193 | extent = get_aabb_extent(obs_aabb) 194 | obs = create_box(w = extent[0], l = extent[1], h= extent[2], color=RED) 195 | set_point(obs, center) 196 | print('Position:', get_point(obs)) 197 | # wait_for_duration(1.0) 198 | 199 | set_all_static() 200 | 201 | 202 | 203 | 204 | # generate pointcloud inside AABB 205 | def genPntCld(aabb, shrink_dis = 0): 206 | lower, upper = aabb 207 | ll_x, ll_y, ll_z = lower 208 | ur_x, ur_y, ur_z = upper 209 | X_list = range(int(ll_x+shrink_dis), int(ur_x+1-shrink_dis)) 210 | y_list = range(int(ll_y+shrink_dis), int(ur_y+1-shrink_dis)) 211 | z_list = range(int(ll_z+shrink_dis), int(ur_z+1-shrink_dis)) 212 | pnt_cld = list(product(X_list, y_list, z_list)) 213 | 214 | return pnt_cld 215 | 216 | 217 | def genWorkspacePntCld(workspace, bad_regs, dynobs_aabb): 218 | whole_cld = [] 219 | 220 | for block_reg in bad_regs: 221 | bad_aabb = workspace.regions[block_reg] 222 | cld = genPntCld(bad_aabb) 223 | whole_cld +=cld 224 | 225 | if dynobs_aabb is not None: 226 | cld = genPntCld(dynobs_aabb, shrink_dis = 0.1) 227 | whole_cld +=cld 228 | 229 | return whole_cld 230 | 231 | 232 | # for unit test 233 | if __name__ =='__main__': 234 | point = [5,5,5] 235 | from pyb_plan_utils import AABB 236 | 237 | aabb = AABB([0,0,0], [10,10,10]) 238 | if isWithinAABB(point, aabb): 239 | print('OK') 240 | else: 241 | print('fuck') 242 | -------------------------------------------------------------------------------- /LTLMP/src/utils/naive_evaluator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class evaluator: 4 | def __init__(self): 5 | pass 6 | 7 | def euclideanDist(self, p0, p1): 8 | return np.linalg.norm(np.subtract(p0,p1)) 9 | 10 | def getCost(self, near_node, x_new): 11 | return self.euclideanDist(near_node.pos, x_new) 12 | 13 | def getHeuristic(self, x_new, goal_pos): 14 | return self.euclideanDist(goal_pos, x_new) -------------------------------------------------------------------------------- /LTLMP/src/utils/tree_visualizer.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from visualization_msgs.msg import Marker 3 | from geometry_msgs.msg import Point 4 | import numpy as np 5 | from tf.transformations import quaternion_from_euler 6 | 7 | 8 | class rviz_maker(object): 9 | def __init__(self, start_pos, end_pos, workspace, scale_factor, has_b_state): 10 | self.start_p = start_pos 11 | self.end_p = end_pos 12 | self.workspace = workspace 13 | self.scale = scale_factor 14 | self.has_b_state = has_b_state 15 | self.vertex_vis = Marker() 16 | self.changed_regs = set() 17 | self.marker_pub = rospy.Publisher("visualization_marker", Marker, queue_size=1) 18 | self.clear_all() 19 | 20 | def clear_all(self): 21 | edges_vis = Marker() 22 | edges_vis.header.frame_id = "map" 23 | edges_vis.header.stamp = rospy.rostime.Time.now() 24 | edges_vis.type = Marker.LINE_LIST 25 | edges_vis.action = Marker.DELETEALL 26 | edges_vis.ns = "edges" 27 | edges_vis.id = 3 28 | self.marker_pub.publish(edges_vis) 29 | 30 | vertex = Marker() 31 | vertex.header.frame_id = "map" 32 | vertex.header.stamp = rospy.rostime.Time.now() 33 | vertex.type = Marker.POINTS 34 | vertex.action = Marker.DELETEALL 35 | vertex.ns = "leaves" 36 | vertex.id = 4 37 | self.marker_pub.publish(vertex) 38 | 39 | rob_vis = Marker() 40 | rob_vis.header.frame_id = "map" 41 | rob_vis.header.stamp = rospy.rostime.Time.now() 42 | rob_vis.type = Marker.CUBE 43 | rob_vis.action = Marker.DELETEALL 44 | rob_vis.ns = "rbt" 45 | rob_vis.id = 1 46 | self.marker_pub.publish(rob_vis) 47 | 48 | fcs_vis = Marker() 49 | fcs_vis.header.frame_id = "map" 50 | fcs_vis.header.stamp = rospy.rostime.Time.now() 51 | fcs_vis.type = Marker.LINE_STRIP 52 | fcs_vis.action = Marker.DELETEALL 53 | fcs_vis.ns = "path" 54 | fcs_vis.id = 6 55 | self.marker_pub.publish(fcs_vis) 56 | 57 | vertex_vis = Marker() 58 | vertex_vis.header.frame_id = "map" 59 | vertex_vis.header.stamp = rospy.rostime.Time.now() 60 | vertex_vis.type = Marker.POINTS 61 | vertex_vis.action = Marker.ADD 62 | 63 | vertex_vis.ns = "rewired" 64 | vertex_vis.id = 4 65 | self.marker_pub.publish(vertex_vis) 66 | 67 | def display_edges(self, edge_list): 68 | edges_vis = Marker() 69 | edges_vis.header.frame_id = "map" 70 | edges_vis.header.stamp = rospy.rostime.Time.now() 71 | edges_vis.type = Marker.LINE_LIST 72 | edges_vis.action = Marker.ADD 73 | 74 | edges_vis.ns = "edges" 75 | edges_vis.id = 3 76 | 77 | edges_vis.scale.x = 0.02 78 | edges_vis.color.g = 0.5 79 | edges_vis.color.b = 0.5 80 | edges_vis.color.a = 0.7 81 | edges_vis.pose.orientation.w = 1 82 | 83 | invalid_edge_vis = Marker() 84 | invalid_edge_vis.header.frame_id = "map" 85 | invalid_edge_vis.header.stamp = rospy.rostime.Time.now() 86 | invalid_edge_vis.type = Marker.LINE_LIST 87 | invalid_edge_vis.action = Marker.ADD 88 | 89 | invalid_edge_vis.ns = "invalid_edges" 90 | invalid_edge_vis.id = 3 91 | 92 | invalid_edge_vis.scale.x = 0.02 93 | invalid_edge_vis.color.g = 0.2 94 | invalid_edge_vis.color.b = 0.2 95 | invalid_edge_vis.color.r = 0.5 96 | invalid_edge_vis.color.a = 0.7 97 | invalid_edge_vis.pose.orientation.w = 1 98 | 99 | BS_change_vis = Marker() 100 | BS_change_vis.header.frame_id = "map" 101 | BS_change_vis.header.stamp = rospy.rostime.Time.now() 102 | BS_change_vis.type = Marker.LINE_LIST 103 | BS_change_vis.action = Marker.ADD 104 | 105 | BS_change_vis.ns = "b_change_edges" 106 | BS_change_vis.id = 3 107 | 108 | BS_change_vis.scale.x = 0.02 109 | BS_change_vis.color.g = 0.5 110 | BS_change_vis.color.b = 0.0 111 | BS_change_vis.color.r = 1.0 112 | BS_change_vis.color.a = 0.7 113 | BS_change_vis.pose.orientation.w = 1 114 | 115 | for edge in edge_list: 116 | pt1 = Point() 117 | pt1.x = edge[0].pos[0] * self.scale 118 | pt1.y = edge[0].pos[1] * self.scale 119 | pt1.z = edge[0].pos[2] * self.scale 120 | pt2 = Point() 121 | pt2.x = edge[1].pos[0] * self.scale 122 | pt2.y = edge[1].pos[1] * self.scale 123 | pt2.z = edge[1].pos[2] * self.scale 124 | 125 | if self.has_b_state: 126 | pt1.z = int(edge[0].b_state) 127 | pt2.z = int(edge[1].b_state) 128 | 129 | if edge[0].cost2Root > 1e5 or edge[1].cost2Root > 1e5: 130 | 131 | invalid_edge_vis.points.append(pt1) 132 | invalid_edge_vis.points.append(pt2) 133 | continue 134 | elif edge[0].b_state is not None and edge[0].b_state != edge[1].b_state: 135 | BS_change_vis.points.append(pt1) 136 | BS_change_vis.points.append(pt2) 137 | continue 138 | 139 | if edge[0].cost2Root > 1e5 or edge[1].cost2Root > 1e5: 140 | 141 | invalid_edge_vis.points.append(pt1) 142 | invalid_edge_vis.points.append(pt2) 143 | else: 144 | edges_vis.points.append(pt1) 145 | edges_vis.points.append(pt2) 146 | 147 | self.marker_pub.publish(edges_vis) 148 | self.marker_pub.publish(invalid_edge_vis) 149 | self.marker_pub.publish(BS_change_vis) 150 | 151 | def draw_empty(self): 152 | vertex_vis = Marker() 153 | vertex_vis.header.frame_id = "map" 154 | vertex_vis.header.stamp = rospy.rostime.Time.now() 155 | vertex_vis.type = Marker.POINTS 156 | vertex_vis.action = Marker.ADD 157 | 158 | vertex_vis.ns = "rewired" 159 | vertex_vis.id = 4 160 | 161 | vertex_vis.scale.x = 0.06 162 | vertex_vis.color.r = 1.0 163 | vertex_vis.color.a = 1 164 | 165 | self.marker_pub.publish(vertex_vis) 166 | 167 | def draw_rewired_root(self, rewired_nodes): 168 | if len(rewired_nodes) == 0: 169 | return 170 | 171 | vertex_vis = Marker() 172 | vertex_vis.header.frame_id = "map" 173 | vertex_vis.header.stamp = rospy.rostime.Time.now() 174 | vertex_vis.type = Marker.POINTS 175 | vertex_vis.action = Marker.ADD 176 | 177 | vertex_vis.ns = "rewired" 178 | vertex_vis.id = 4 179 | 180 | vertex_vis.scale.x = 0.06 181 | vertex_vis.color.r = 1.0 182 | vertex_vis.color.a = 1 183 | 184 | for nd in rewired_nodes: 185 | p1 = nd.pos 186 | 187 | pt1 = Point() 188 | pt1.x = p1[0] * self.scale 189 | pt1.y = p1[1] * self.scale 190 | pt1.z = p1[2] * self.scale 191 | 192 | if self.has_b_state: 193 | pt1.z = int(self.nd.b_state) 194 | 195 | vertex_vis.points.append(pt1) 196 | 197 | self.marker_pub.publish(vertex_vis) 198 | 199 | def populateRviz(self): 200 | self.populateStart() 201 | self.populateEnd() 202 | self.populateWorkspace(self.workspace.regions, False) 203 | self.populateWorkspace(self.workspace.obstacles, True) 204 | self.draw_dyn_obs() 205 | 206 | def draw_ws_water(self): 207 | water_vis = Marker() 208 | water_vis.header.frame_id = "map" 209 | water_vis.header.stamp = rospy.rostime.Time.now() 210 | water_vis.type = Marker.CUBE 211 | water_vis.action = Marker.ADD 212 | 213 | water_vis.ns = "water" 214 | water_vis.id = 1 215 | 216 | water_vis.color.r = 0 217 | water_vis.color.g = 1 218 | water_vis.color.b = 1 219 | water_vis.color.a = 0.1 220 | 221 | water_vis.scale.x = 10 222 | water_vis.scale.y = 20 223 | water_vis.scale.z = 0.2 224 | water_vis.pose.position.x = 50 * self.scale 225 | water_vis.pose.position.y = 0 * self.scale 226 | 227 | water_vis.pose.position.z = 0 228 | 229 | water_vis.pose.orientation.w = 1 230 | self.marker_pub.publish(water_vis) 231 | 232 | def populateExt(self, coords): 233 | bdr = Marker() 234 | bdr.header.frame_id = "map" 235 | bdr.header.stamp = rospy.rostime.Time.now() 236 | bdr.type = Marker.LINE_LIST 237 | bdr.action = Marker.ADD 238 | bdr.ns = "ws_ext" 239 | bdr.id = 0 240 | 241 | bdr.scale.x = 0.2 242 | bdr.color.r = 0.6 243 | bdr.color.g = 0.6 244 | bdr.color.b = 0.9 245 | bdr.color.a = 0.8 246 | bdr.pose.orientation.w = 1 247 | 248 | for idx in range(len(coords) - 1): 249 | pt0 = Point() 250 | pt1 = Point() 251 | pt0.x = coords[idx][0] * self.scale 252 | pt0.y = coords[idx][1] * self.scale 253 | pt1.x = coords[idx + 1][0] * self.scale 254 | pt1.y = coords[idx + 1][1] * self.scale 255 | bdr.points.append(pt1) 256 | bdr.points.append(pt0) 257 | self.marker_pub.publish(bdr) 258 | 259 | def populateWorkspace(self, regions, is_obs=True): 260 | idx_offset = 5 261 | if is_obs: 262 | idx_offset += 10 263 | for key in regions.keys(): 264 | aabb = regions[key] 265 | lower, upper = aabb 266 | coords = np.array([(lower[0], lower[1]), (upper[0], lower[1]), (upper[0], upper[1]), (lower[0], upper[1]), (lower[0], lower[1])]) 267 | 268 | # x, y = poly.exterior.xy 269 | # coords = np.column_stack((x, y)) # coords = [(0, 0), (1, 1), (1, 0)] 270 | 271 | bdr = Marker() 272 | bdr.header.frame_id = "map" 273 | bdr.header.stamp = rospy.rostime.Time.now() 274 | bdr.type = Marker.LINE_LIST 275 | bdr.action = Marker.ADD 276 | bdr.ns = "regs" 277 | bdr.id = idx_offset 278 | idx_offset += 1 279 | 280 | bdr.scale.x = 0.2 281 | if is_obs: 282 | bdr.color.r = 0.8 283 | bdr.color.g = 0.3 284 | bdr.color.b = 0.3 285 | elif key in self.changed_regs: 286 | bdr.color.r = 0.6 287 | bdr.color.g = 0.6 288 | bdr.color.b = 0.0 289 | else: 290 | bdr.color.r = 0.3 291 | bdr.color.g = 1 292 | bdr.color.b = 0.3 293 | bdr.color.a = 0.8 294 | bdr.pose.orientation.w = 1 295 | 296 | for idx in range(len(coords) - 1): 297 | pt0 = Point() 298 | pt1 = Point() 299 | pt0.x = coords[idx][0] * self.scale 300 | pt0.y = coords[idx][1] * self.scale 301 | pt1.x = coords[idx + 1][0] * self.scale 302 | pt1.y = coords[idx + 1][1] * self.scale 303 | bdr.points.append(pt1) 304 | bdr.points.append(pt0) 305 | 306 | self.marker_pub.publish(bdr) 307 | 308 | txt = Marker() 309 | # compute center 310 | x_center = sum([pt[0] for pt in coords]) / len(coords) 311 | y_center = sum([pt[1] for pt in coords]) / len(coords) 312 | 313 | # place text 314 | txt.header.frame_id = "map" 315 | txt.header.stamp = rospy.rostime.Time.now() 316 | txt.type = Marker.TEXT_VIEW_FACING 317 | txt.ns = "text" 318 | txt.id = idx_offset 319 | idx_offset += 1 320 | txt.action = Marker.ADD 321 | txt.pose.position.x = x_center * self.scale 322 | txt.pose.position.y = y_center * self.scale 323 | txt.pose.orientation.w = 1 324 | 325 | txt.scale.z = 1 326 | if is_obs: 327 | txt.color.r = 0 328 | txt.color.g = 0 329 | txt.color.b = 0 330 | txt.text = 'obs' 331 | else: 332 | txt.color.r = 0 333 | txt.color.g = 0 334 | txt.color.b = 0 335 | txt.text = key 336 | # txt.text = properties[key] 337 | txt.color.a = 1 338 | 339 | # self.txt_vis_list.append(txt) 340 | self.marker_pub.publish(txt) 341 | 342 | def display_text(self, text): 343 | txt = Marker() 344 | # compute center 345 | x_center = 50 346 | y_center = 50 347 | 348 | # place text 349 | txt.header.frame_id = "map" 350 | txt.header.stamp = rospy.rostime.Time.now() 351 | txt.type = Marker.TEXT_VIEW_FACING 352 | txt.ns = "user_text" 353 | txt.id = 0 354 | txt.action = Marker.ADD 355 | txt.pose.position.x = x_center * self.scale 356 | txt.pose.position.y = y_center * self.scale 357 | txt.pose.orientation.w = 1 358 | 359 | txt.scale.z = 0.7 360 | txt.color.r = 0.7 361 | txt.color.g = 0.8 362 | txt.color.b = 0.95 363 | txt.text = text 364 | txt.color.a = 1.0 365 | 366 | self.marker_pub.publish(txt) 367 | 368 | def populateStart(self): 369 | if self.start_p is None: 370 | return 371 | 372 | pt1 = Point() 373 | pt1.x = self.start_p[0] * self.scale 374 | pt1.y = self.start_p[1] * self.scale 375 | pt1.z = self.start_p[2] * self.scale 376 | 377 | vertex = Marker() 378 | vertex.header.frame_id = "map" 379 | vertex.header.stamp = rospy.rostime.Time.now() 380 | vertex.type = Marker.POINTS 381 | vertex.action = Marker.ADD 382 | 383 | vertex.ns = "start" 384 | vertex.id = 0 385 | 386 | vertex.scale.x = vertex.scale.y = vertex.scale.z = 0.16 387 | 388 | vertex.color.g = 1.0 389 | vertex.color.a = 1 390 | 391 | vertex.points.append(pt1) 392 | 393 | self.marker_pub.publish(vertex) 394 | 395 | def populateEnd(self): 396 | if self.end_p is None: 397 | return 398 | 399 | vertex = Marker() 400 | vertex.header.frame_id = "map" 401 | vertex.header.stamp = rospy.rostime.Time.now() 402 | vertex.type = Marker.CUBE 403 | vertex.action = Marker.ADD 404 | 405 | vertex.ns = "end" 406 | vertex.id = 1 407 | 408 | vertex.scale.x = vertex.scale.y = vertex.scale.z = 0.1 409 | vertex.color.g = 1.0 410 | vertex.color.r = 0.7 411 | vertex.color.a = 1 412 | 413 | vertex.pose.position.x = self.end_p[0] * self.scale 414 | vertex.pose.position.y = self.end_p[1] * self.scale 415 | 416 | self.marker_pub.publish(vertex) 417 | 418 | def draw_rtroot(self, root): 419 | if root is None: 420 | return 421 | 422 | vertex = Marker() 423 | vertex.header.frame_id = "map" 424 | vertex.header.stamp = rospy.rostime.Time.now() 425 | vertex.type = Marker.CUBE 426 | vertex.action = Marker.ADD 427 | 428 | vertex.ns = "rt_root" 429 | vertex.id = 1 430 | 431 | vertex.scale.x = vertex.scale.y = vertex.scale.z = 0.1 432 | vertex.color.g = 1.0 433 | vertex.color.r = 0.7 434 | vertex.color.a = 1 435 | 436 | vertex.pose.position.x = root.pos[0] * self.scale 437 | vertex.pose.position.y = root.pos[1] * self.scale 438 | vertex.pose.position.z = root.pos[2] * self.scale 439 | if self.has_b_state: 440 | vertex.pose.position.z = int(root.b_state) 441 | 442 | self.marker_pub.publish(vertex) 443 | 444 | def drawSample(self, x_rand, near_range): 445 | vertex = Marker() 446 | vertex.header.frame_id = "map" 447 | vertex.header.stamp = rospy.rostime.Time.now() 448 | vertex.type = Marker.CYLINDER 449 | vertex.action = Marker.ADD 450 | 451 | vertex.ns = "sample" 452 | vertex.id = 2 453 | 454 | vertex.scale.x = vertex.scale.y = vertex.scale.z = 0.1 455 | vertex.color.b = 0.2 456 | vertex.color.g = 1.0 457 | vertex.color.r = 1.0 458 | vertex.color.a = 1 459 | 460 | vertex.pose.position.x = x_rand[0] * self.scale 461 | vertex.pose.position.y = x_rand[1] * self.scale 462 | 463 | self.marker_pub.publish(vertex) 464 | 465 | circle = Marker() 466 | circle.header.frame_id = "map" 467 | circle.header.stamp = rospy.rostime.Time.now() 468 | circle.type = Marker.SPHERE 469 | circle.action = Marker.ADD 470 | 471 | circle.ns = "near_range" 472 | circle.id = 1 473 | 474 | circle.scale.x = circle.scale.y = near_range * self.scale 475 | circle.scale.z = 0.05 476 | circle.color.b = circle.color.g = circle.color.r = 0.2 477 | circle.color.a = 0.2 478 | 479 | circle.pose.position.x = vertex.pose.position.x 480 | circle.pose.position.y = vertex.pose.position.y 481 | 482 | self.marker_pub.publish(circle) 483 | 484 | def drawFinalPath(self, path): 485 | # if len(path)<2: 486 | # return 487 | 488 | fcs_vis = Marker() 489 | fcs_vis.header.frame_id = "map" 490 | fcs_vis.header.stamp = rospy.rostime.Time.now() 491 | fcs_vis.type = Marker.LINE_STRIP 492 | fcs_vis.action = Marker.ADD 493 | fcs_vis.ns = "path" 494 | fcs_vis.id = 6 495 | 496 | fcs_vis.scale.x = 0.8 497 | fcs_vis.color.g = 0.9 498 | fcs_vis.color.b = 0.5 499 | fcs_vis.color.a = 0.2 500 | fcs_vis.pose.orientation.w = 1 501 | 502 | for p1 in path: 503 | pt1 = Point() 504 | pt1.x = p1.pos[0] * self.scale 505 | pt1.y = p1.pos[1] * self.scale 506 | pt1.z = p1.pos[2] * self.scale 507 | 508 | if self.has_b_state: 509 | pt1.z = int(p1.b_state) 510 | 511 | fcs_vis.points.append(pt1) 512 | 513 | self.marker_pub.publish(fcs_vis) 514 | 515 | def drawPrefixPath(self, path, motion_bias=None): 516 | if len(path) < 2: 517 | return 518 | 519 | fcs_vis = Marker() 520 | fcs_vis.header.frame_id = "map" 521 | fcs_vis.header.stamp = rospy.rostime.Time.now() 522 | fcs_vis.type = Marker.LINE_STRIP 523 | fcs_vis.action = Marker.ADD 524 | fcs_vis.ns = "path" 525 | fcs_vis.id = 6 526 | 527 | fcs_vis.scale.x = 0.8 528 | fcs_vis.color.g = 0.9 529 | fcs_vis.color.b = 0.5 530 | fcs_vis.color.a = 0.2 531 | fcs_vis.pose.orientation.w = 1 532 | 533 | for p1 in path: 534 | pt1 = Point() 535 | pt1.x = p1[0][0][0] + motion_bias[0] 536 | pt1.y = p1[0][0][1] + motion_bias[1] 537 | pt1.z = p1[0][0][2] + motion_bias[1] 538 | 539 | if self.has_b_state: 540 | pt1.z = int(p1[1]) 541 | 542 | fcs_vis.points.append(pt1) 543 | 544 | self.marker_pub.publish(fcs_vis) 545 | 546 | def draw_rbt(self, rbt_pos, cur_heading, fov_range, b_state=None): 547 | rob_vis = Marker() 548 | rob_vis.header.frame_id = "map" 549 | rob_vis.header.stamp = rospy.rostime.Time.now() 550 | rob_vis.type = Marker.CUBE 551 | rob_vis.action = Marker.ADD 552 | 553 | rob_vis.ns = "rbt" 554 | rob_vis.id = 1 555 | 556 | rob_vis.scale.x = rob_vis.scale.y = rob_vis.scale.z = 0.1 557 | 558 | rob_vis.color.r = 1 559 | rob_vis.color.g = 0.2 560 | rob_vis.color.b = 1 561 | rob_vis.color.a = 0.8 562 | 563 | rob_vis.scale.x = 0.5 564 | rob_vis.scale.y = 0.3 565 | rob_vis.scale.z = 0.2 566 | rob_vis.pose.position.x = rbt_pos[0] * self.scale 567 | rob_vis.pose.position.y = rbt_pos[1] * self.scale 568 | rob_vis.pose.position.z = rbt_pos[2] * self.scale 569 | if b_state: 570 | rob_vis.pose.position.z = int(b_state) 571 | yaw = np.arctan2(cur_heading[1], cur_heading[0]) 572 | quat = quaternion_from_euler(0, 0, yaw) 573 | rob_vis.pose.orientation.x = quat[0] 574 | rob_vis.pose.orientation.y = quat[1] 575 | rob_vis.pose.orientation.z = quat[2] 576 | rob_vis.pose.orientation.w = quat[3] 577 | self.marker_pub.publish(rob_vis) 578 | 579 | circle = Marker() 580 | circle.header.frame_id = "map" 581 | circle.header.stamp = rospy.rostime.Time.now() 582 | circle.type = Marker.CUBE 583 | circle.action = Marker.ADD 584 | 585 | circle.ns = "fov_range" 586 | circle.id = 1 587 | 588 | circle.scale.x = circle.scale.y = fov_range * self.scale 589 | circle.scale.z = 0.1 590 | circle.color.b = circle.color.g = 0.2 591 | circle.color.r = 0.5 592 | circle.color.a = 0.1 593 | 594 | circle.pose.position.x = rob_vis.pose.position.x 595 | circle.pose.position.y = rob_vis.pose.position.y 596 | self.marker_pub.publish(circle) 597 | 598 | def draw_leaves(self, node_ls): 599 | 600 | vertex = Marker() 601 | vertex.header.frame_id = "map" 602 | vertex.header.stamp = rospy.rostime.Time.now() 603 | vertex.type = Marker.POINTS 604 | vertex.action = Marker.ADD 605 | 606 | vertex.ns = "leaves" 607 | vertex.id = 4 608 | 609 | vertex.scale.x = vertex.scale.y = vertex.scale.z = 0.05 610 | 611 | vertex.color.b = 1.0 612 | vertex.color.a = 1 613 | 614 | for nd in node_ls: 615 | pt1 = Point() 616 | pt1.x = nd.pos[0] * self.scale 617 | pt1.y = nd.pos[1] * self.scale 618 | pt1.z = nd.pos[2] * self.scale 619 | 620 | if self.has_b_state: 621 | pt1.z = int(nd.b_state) 622 | 623 | vertex.points.append(pt1) 624 | 625 | self.marker_pub.publish(vertex) 626 | 627 | def draw_dyn_obs(self): 628 | vertex = Marker() 629 | vertex.header.frame_id = "map" 630 | vertex.header.stamp = rospy.rostime.Time.now() 631 | vertex.type = Marker.CUBE 632 | vertex.action = Marker.ADD 633 | 634 | vertex.ns = "dyn_obs" 635 | vertex.id = 2 636 | 637 | vertex.scale.x = (self.workspace.do_ur[0] - self.workspace.do_ll[0]) * self.scale 638 | vertex.scale.y = (self.workspace.do_ur[1] - self.workspace.do_ll[1]) * self.scale 639 | vertex.scale.z = 0.8 640 | vertex.color.b = 0.5 641 | vertex.color.g = 0.5 642 | vertex.color.r = 0.8 643 | vertex.color.a = 0.8 644 | 645 | vertex.pose.position.x = self.workspace.dynamic_obs_center[0] * self.scale 646 | vertex.pose.position.y = self.workspace.dynamic_obs_center[1] * self.scale 647 | 648 | self.marker_pub.publish(vertex) 649 | 650 | 651 | if __name__ == '__main__': 652 | from workspace import Workspace3D 653 | 654 | rospy.init_node('visualizer', anonymous=False) 655 | wksp = Workspace3D() 656 | rviz_mgr = rviz_maker(None, None, wksp, scale_factor=1, has_b_state= False) 657 | 658 | # ws_ext_coords = getAABB(wksp.workspace_ll, ws_ur) 659 | # ws_ext_coords.append(wksp.workspace_ll) 660 | 661 | while not rospy.is_shutdown(): 662 | rviz_mgr.populateWorkspace(rviz_mgr.workspace.regions, False) 663 | rviz_mgr.populateWorkspace(rviz_mgr.workspace.obstacles, True) 664 | rviz_mgr.draw_dyn_obs() 665 | # rviz_mgr.populateExt(ws_ext_coords) 666 | 667 | rospy.sleep(0.1) 668 | 669 | # rospy.spin() -------------------------------------------------------------------------------- /LTLMP/src/utils/workspace.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import random 3 | 4 | import sys 5 | sys.path.append("/home/joseph/yzchen_ws/task_planning/AI-plan/pybullet-planning/") 6 | sys.path.append("/home/joseph/yzchen_ws/task_planning/ltl_mq_opensource/src/pybullet-planning") 7 | sys.path.append("/home/joseph/yzchen_ws/task_planning/ltl_mq_opensource/src/LTLMP/src/utils/") 8 | 9 | from pyb_plan_utils import AABB 10 | from misc_func import setup_pybullet 11 | 12 | 13 | def calc_ur_given_ll(ll_coords, reg_side): 14 | ur_coords = [] 15 | for ll_crd in ll_coords: 16 | ur_crd = [ll_crd[i] + reg_side for i in range(len(ll_crd))] 17 | ur_coords.append(ur_crd) 18 | 19 | return ur_coords 20 | 21 | def calc_reg_center(ll_coord, reg_len): 22 | return (ll_coord[0]+ 0.5*reg_len, ll_coord[1]+ 0.5*reg_len) 23 | 24 | 25 | 26 | 27 | class Workspace3D(object): 28 | def __init__(self): 29 | self.length = 7.6 30 | self.width = 6.2 31 | self.height = 2.2 32 | self.workspace_range = (self.length, self.width, self.height) 33 | self.workspace_ll = (-3.4, -3.0, 0.) 34 | self.workspace_ur = (self.workspace_ll[0]+ self.length, self.workspace_ll[1]+ self.width, self.workspace_ll[2] + self.height) 35 | self.ws_AABB = AABB(self.workspace_ll, self.workspace_ur) 36 | reg_side = 1.2 37 | 38 | # regions 39 | reg_ll_coords = [(2.0, 1.0, 0.0), (-1.0, -3.0, 1.), (-3.0, 0.0, 0.0)] 40 | reg_ur_coords = calc_ur_given_ll(reg_ll_coords, reg_side) 41 | self.regions = {'l1': AABB(reg_ll_coords[0], reg_ur_coords[0]), 42 | 'l2': AABB(reg_ll_coords[1], reg_ur_coords[1]), 43 | 'l3': AABB(reg_ll_coords[2], reg_ur_coords[2])} 44 | 45 | self.reg_set = {k for k in self.regions} 46 | self.properties = {'l1': 'grassland', 'l2': 'pond', 'l3': 'grassland'} 47 | 48 | self.reg_centers = { 49 | 'l1': tuple((ll + ur) / 2 for ll, ur in zip(reg_ll_coords[0], reg_ur_coords[0])), 50 | 'l2':tuple((ll + ur) / 2 for ll, ur in zip(reg_ll_coords[1], reg_ur_coords[1])), 51 | 'l3': tuple((ll + ur) / 2 for ll, ur in zip(reg_ll_coords[2], reg_ur_coords[2])), 52 | } 53 | 54 | # obstacles 55 | obs_ll_coords = [(-3.4, -3.0, 0.), (2.0, -3.0, 0.), (-1.0, 1.0, 0)] 56 | obs_ur_coords = [(-2.0, -2.0, 1.0), (4.0, -1.5, 1.0), (0.0, 3.0, 0.2)] 57 | self.obstacles = {'o1': AABB(obs_ll_coords[0], obs_ur_coords[0]), 58 | 'o2': AABB(obs_ll_coords[1], obs_ur_coords[1]), 59 | 'o3': AABB(obs_ll_coords[2], obs_ur_coords[2])} 60 | 61 | # dynamic obstacle 62 | self.do_ll, self.do_ur = (-50, -30,10), (-15, -20, 10) 63 | self.dyn_obs = {'d1': AABB(self.do_ll, self.do_ur) } 64 | self.dynamic_obs_center = ((self.do_ur[0] +self.do_ll[0])* 0.5, (self.do_ur[1] +self.do_ll[1])* 0.5, (self.do_ur[2] +self.do_ll[2])* 0.5) 65 | 66 | 67 | 68 | 69 | if __name__ == '__main__': 70 | wksp = Workspace3D() 71 | setup_pybullet(wksp, use_gui=False) 72 | # wksp.test_scene() 73 | -------------------------------------------------------------------------------- /figure/coverpage.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JINXER000/multiple-task-temporal-logic-planning/16bc21cac81b733efd0e657791ac492d2f0696fc/figure/coverpage.png -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | The software for IROS2023 paper: An interactive system for multiple-task linear temporal logic path planning. 2 | 3 | 4 | 5 | 6 | The supplementary video can be viewed here: 7 | 8 |

9 | 12 |

13 | 14 | 15 | # functionality 16 | - [x] Sampling-based LTL path planning 17 | - [x] Task switching 18 | - [ ] Human-machine interface 19 | 20 | # install 21 | ## 3rd party dependancies 22 | ``` 23 | pip install numpy networkx itertools matplotlib ply pybullet 24 | ``` 25 | ## ltl2dfa 26 | ``` 27 | sudo apt-get install flex -y 28 | sudo apt-get install bison -yx 29 | sudo apt-get install byacc -y 30 | git clone https://github.com/whitemech/LTLf2DFA.git 31 | cd ltlf2dfa 32 | pip install . 33 | ``` 34 | ## ltl2ba 35 | follow $(PROJECT_PATH)/src/formula_parser/install_ltl2ba/README.txt. 36 | 37 | 38 | # Folder configuration 39 | Replace folder name in workspace.py and gazebo_HMI_plugin.py. A quick way is to search '/home/joseph/yzchen_ws'. 40 | 41 | 42 | # Experiment with the whole system 43 | 44 | ## [optional when doing simulation] launch a gazebo environment 45 | ## run modules like localization, mapping, and motion planning 46 | ``` 47 | ./start nndp 48 | ``` 49 | ## launch HMI 50 | ``` 51 | cd /home/joseph/yzchen_ws/task_planning/ltl_interface/ 52 | source devel/setup.bash 53 | rqt 54 | ``` 55 | Then open the plugin in rqt: 'LTL task planning' 56 | ## launch the teleoperation service 57 | ``` 58 | cd /home/joseph/yzchen_ws/task_planning/ltl_interface/ 59 | source devel/setup.bash 60 | python /home/joseph/yzchen_ws/task_planning/ltl_interface/src/remote_server/src/remote_launch_service.py 61 | ``` 62 | ## launch the LTL planner 63 | ``` 64 | python gazebo_HMI_plugin.py 65 | ``` 66 | ## Get ready for the task (e.g., taking off, do system selection in HMI) 67 | ## Input the LTL specification by voice or text 68 | 69 | # credit 70 | The body of code in pyb_plan_utils.py comes from https://github.com/caelan/pybullet-planning.git. --------------------------------------------------------------------------------