diff --git a/lrs_exec/scenario_handler.py b/lrs_exec/scenario_handler.py index 8bcbacc04b92b3a279714a8d22b4d423035e4e7b..5537de2ec48359d7a19806a137895efe5cff8fcc 100644 --- a/lrs_exec/scenario_handler.py +++ b/lrs_exec/scenario_handler.py @@ -45,13 +45,19 @@ class ScenarioHandler(Node): self.declare_parameter("fake", False) self.fake = self.get_parameter('fake').get_parameter_value().bool_value + self.declare_parameter("drop", False) + self.drop_flag = self.get_parameter('drop').get_parameter_value().bool_value + + self.declare_parameter("search", False) + self.search_flag = self.get_parameter('search').get_parameter_value().bool_value + self.real_salient_points = [] self.salient_points = {} self.handled_salient_points = {} self.packages = {} - self.have_dropped = False - self.have_added_search = False + self.have_dropped = True + self.have_added_search = True self.n_packages_in_bucket = {} self.n_object_detections = 0 @@ -202,7 +208,7 @@ class ScenarioHandler(Node): if self.fake: self.check_fake() - self.check_restart_failed() + ## self.check_restart_failed() n_medicine_pkgs = -1 if "medicine" in self.packages: @@ -370,6 +376,12 @@ def main(args=None): #node.add_search_to_queue() #return + if node.search_flag: + node.add_search_to_queue () + + if node.drop_flag: + node.add_to_drop_queue(26.0, 1.0, 20) + try: print('Hi from scenario handler. SPINNING!') executor.spin() diff --git a/tmux/op.tmux b/tmux/op.tmux index 240f4084d99bb97f226ecf493728b035a25f90f3..bdf6cbc768d5d3352044d75d9c5aa6455739514c 100755 --- a/tmux/op.tmux +++ b/tmux/op.tmux @@ -100,10 +100,10 @@ def main_unit(ns): if options.spot: ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=deliver -p team_command:=set-team -p args:=\\\"spot0\\\"') ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=search -p team_command:=set-team -p args:=\\\"dji5\\\"') - ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=drop -p team_command:=set-team -p args:=\\\"dji11\\\"') + ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=drop -p team_command:=set-team -p args:=\\\"dji17\\\"') else: ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=searchsurface -p team_command:=set-team -p args:=\\\"Piraya14 Manta\\\"') - ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=searchair -p team_command:=set-team -p args:=\\\"dji5\\\"') + ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=searchair -p team_command:=set-team -p args:=\\\"dji8\\\"') ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=examine -p team_command:=set-team -p args:=\\\"MiniUSV1 MiniUSV2 MiniUSV3 MiniUSV4\\\"') ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=sup -p team_command:=set-team -p args:=\\\"expand\\\"')