def test_safe_wait_till_true(self): class FakeObject: def __init__(self): self.field_a = 1 f = FakeObject() safe_wait_till_true('kwargs["f"].field_a', 1, 2, 0.1, f=f)
def _test_goal_reached(self): # reset self.ros_topic.publishers[self.reset_topic].publish(Empty()) time.sleep(0.1) safe_wait_till_true( 'kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.Running.name, 3, 0.1, ros_topic=self.ros_topic) self.ros_topic.publishers[self.pose_topic].publish(Odometry()) rospy.sleep(0.1) goal_pos = [2, 2, 1] self.ros_topic.publishers[self.pose_topic].publish( get_fake_odometry(*goal_pos)) safe_wait_till_true( 'kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.Terminated.name, 3, 0.1, ros_topic=self.ros_topic) distance = np.sqrt(sum(np.asarray(goal_pos)**2)) self.assertEqual( rospy.get_param( '/world/reward/goal_reached/weights/distance_from_start') * distance, self.ros_topic.topic_values[self.reward_topic].reward) self.assertEqual( rospy.get_param('/world/reward/goal_reached/termination'), self.ros_topic.topic_values[self.reward_topic].termination)
def test_modified_state_frame_visualizer(self): self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) config = { 'output_path': self.output_dir, 'modified_state_publisher_mode': 'CombinedGlobalPoses', 'modified_state_frame_visualizer': True, } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=config, visible=True) # subscribe to modified_state_topic self.frame_topic = '/modified_state_frame' subscribe_topics = [ TopicConfig(topic_name=self.frame_topic, msg_type='Image'), ] # create publishers for modified state self.modified_state_topic = '/modified_state' publish_topics = [ TopicConfig(topic_name=self.modified_state_topic, msg_type='CombinedGlobalPoses'), ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics) # test center view message = get_fake_combined_global_poses(0, 0, 1, 3, 1, 1, 0, 0, 0) rospy.sleep(2) self.ros_topic.publishers[self.modified_state_topic].publish(message) safe_wait_till_true( '"/modified_state_frame" in kwargs["ros_topic"].topic_values.keys()', True, 3, 0.1, ros_topic=self.ros_topic) frame = process_image( self.ros_topic.topic_values['/modified_state_frame']) plt.imshow(frame) plt.show() a = 100
def __init__(self): self.count = 0 rospy.init_node('altitude_control') safe_wait_till_true(f'kwargs["rospy"].has_param("/output_path")', True, 5, 0.1, rospy=rospy) self._output_path = get_output_path() self._logger = get_logger(get_filename_without_extension(__file__), self._output_path) self._reference_height = rospy.get_param('/starting_height', 1) self._rate_fps = 60 self._go_publisher = rospy.Publisher('/fsm/go', Empty, queue_size=10) self._robot = rospy.get_param('/robot/model_name', 'default') self._height = {} self._publishers = {} self._enable_motors_services = {} self._setup()
def _test_start_up(self): # FSM should start in unknown state, waiting for reset # @ startup (before reset) safe_wait_till_true( '"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 5, 0.1, ros_topic=self.ros_topic) self.assertEqual('Unknown', self.ros_topic.topic_values[self.state_topic].data) safe_wait_till_true( '"/fsm/reward" in kwargs["ros_topic"].topic_values.keys()', True, 5, 0.1, ros_topic=self.ros_topic) self.assertEqual(0, self.ros_topic.topic_values[self.reward_topic].reward) self.assertEqual( 'Unknown', self.ros_topic.topic_values[self.reward_topic].termination)
def _test_out_of_time(self): # reset self.ros_topic.publishers[self.reset_topic].publish(Empty()) safe_wait_till_true( 'kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.Running.name, 4, 0.1, ros_topic=self.ros_topic) offset = 3 while self.ros_topic.topic_values[ self.reward_topic].termination == 'NotDone': rospy.sleep(1) self.ros_topic.publishers[self.modified_state_topic].publish( get_fake_modified_state([1, 0, 1, 1, offset, 1, 0, 0, 0])) rospy.sleep(1) iou = 5 # TODO define correct IOU when this is implemented self.assertEqual( rospy.get_param('/world/reward/out_of_time/weights/iou') * iou, self.ros_topic.topic_values[self.reward_topic].reward) self.assertEqual( rospy.get_param('/world/reward/out_of_time/termination'), self.ros_topic.topic_values[self.reward_topic].termination)
def _test_on_collision(self): offset = 3 self.ros_topic.publishers[self.modified_state_topic].publish( get_fake_modified_state([1, 0, 1, 1, offset, 1, 0, 0, 0])) time.sleep( 0.5) # make sure modified state is received before collision self.ros_topic.publishers[self.depth_scan_topic].publish( get_fake_laser_scan([.2] * 360)) safe_wait_till_true( 'kwargs["ros_topic"].topic_values["/fsm/reward"].termination', TerminationType.Failure.name, 4, 0.1, ros_topic=self.ros_topic) self.assertAlmostEqual( rospy.get_param( '/world/reward/on_collision/weights/distance_between_agents') * offset, self.ros_topic.topic_values[self.reward_topic].reward, places=5) self.assertEqual( rospy.get_param('/world/reward/on_collision/termination'), self.ros_topic.topic_values[self.reward_topic].termination)
def test_april_tag_detector_real_bebop(self): self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) self._config = { 'output_path': self.output_dir, 'world_name': 'april_tag', 'robot_name': 'bebop_real', 'gazebo': False, 'april_tag_detector': True, } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=True) # subscribe to command control subscribe_topics = [ TopicConfig(topic_name='/reference_ground_point', msg_type='PointStamped'), ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=[]) safe_wait_till_true( '"/reference_ground_point" in kwargs["ros_topic"].topic_values.keys()', True, 25, 0.1, ros_topic=self.ros_topic) while True: print( f'waypoint: {self.ros_topic.topic_values["/reference_ground_point"]}' ) time.sleep(0.5)
def test_forward_cam_drone(self) -> None: self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) self._config = { 'output_path': self.output_dir, 'world_name': 'april_tag', 'robot_name': 'drone_sim', 'gazebo': True, 'fsm': True, 'fsm_mode': 'TakeOverRun', 'control_mapping': True, 'control_mapping_config': 'mathias_controller', 'altitude_control': True, 'waypoint_indicator': False, 'april_tag_detector': True, 'mathias_controller_with_KF': True, 'mathias_controller_config_file_path_with_extension': f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF.yml', 'starting_height': 0.5, } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=True) # subscribe to command control self.image_topic = rospy.get_param(f'/robot/camera_sensor/topic') subscribe_topics = [ TopicConfig(topic_name=rospy.get_param('/robot/position_sensor/topic'), msg_type=rospy.get_param('/robot/position_sensor/type')), TopicConfig(topic_name='/reference_ground_point', msg_type='PointStamped'), TopicConfig(topic_name='/fsm/state', msg_type='String'), TopicConfig(topic_name=self.image_topic, msg_type='Image') ] publish_topics = [ TopicConfig(topic_name='/fsm/reset', msg_type='Empty') ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics ) self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice) self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice) self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # unpause gazebo to receive messages self._unpause_client.wait_for_service() self._unpause_client.call() safe_wait_till_true('"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 10, 0.1, ros_topic=self.ros_topic) self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data, FsmState.Unknown.name) # publish reset self.ros_topic.publishers['/fsm/reset'].publish(Empty()) # gets fsm in taken over state safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.TakenOver.name, 2, 0.1, ros_topic=self.ros_topic) # altitude control brings drone to starting_height safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.Running.name, 45, 0.1, ros_topic=self.ros_topic) # check pose pose = self.get_pose() self._pause_client.wait_for_service() self._pause_client.call() a = 100
def test_waypoints_tracking_real_bebop_with_KF_with_keyboard(self): self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) self._config = { 'output_path': self.output_dir, 'world_name': 'hexagon', 'robot_name': 'bebop_real', 'gazebo': False, 'fsm': True, 'fsm_mode': 'TakeOverRun', 'control_mapping': True, 'control_mapping_config': 'mathias_controller_keyboard', 'waypoint_indicator': True, 'altitude_control': False, 'mathias_controller_with_KF': True, 'starting_height': 1., 'keyboard': True, 'mathias_controller_config_file_path_with_extension': f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF_real_bebop.yml', } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=True) # subscribe to command control self.visualisation_topic = '/actor/mathias_controller/visualisation' subscribe_topics = [ TopicConfig( topic_name=rospy.get_param('/robot/position_sensor/topic'), msg_type=rospy.get_param('/robot/position_sensor/type')), TopicConfig(topic_name='/fsm/state', msg_type='String'), TopicConfig(topic_name='/waypoint_indicator/current_waypoint', msg_type='Float32MultiArray'), TopicConfig(topic_name=self.visualisation_topic, msg_type='Image') ] publish_topics = [ TopicConfig(topic_name='/fsm/reset', msg_type='Empty'), ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics) safe_wait_till_true( '"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 25, 0.1, ros_topic=self.ros_topic) self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data, FsmState.Unknown.name) while True: # publish reset self.ros_topic.publishers['/fsm/reset'].publish(Empty()) while self.ros_topic.topic_values[ "/fsm/state"].data != FsmState.Running.name: rospy.sleep(0.1) safe_wait_till_true( '"/waypoint_indicator/current_waypoint" in kwargs["ros_topic"].topic_values.keys()', True, 10, 0.1, ros_topic=self.ros_topic) poses = [] waypoints = [] while self.ros_topic.topic_values["/fsm/state"].data != FsmState.Terminated.name and \ self.ros_topic.topic_values["/fsm/state"].data != FsmState.TakenOver.name: rospy.sleep(0.5) pose = self.get_pose() waypoint = self.ros_topic.topic_values[ '/waypoint_indicator/current_waypoint'] poses.append(pose) waypoints.append(waypoint) plt.figure(figsize=(15, 15)) plt.scatter([p[0] for p in poses], [p[1] for p in poses], color='C0', label='xy-pose') plt.scatter([p.data[0] for p in waypoints], [p.data[1] for p in waypoints], color='C1', label='xy-waypoints') plt.legend() plt.xlabel("x [m]") plt.ylabel("y [m]") plt.show()
def test_drone_keyboard_gazebo_with_KF(self): self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) self._config = { 'output_path': self.output_dir, 'world_name': 'empty', 'robot_name': 'drone_sim', 'gazebo': True, 'fsm': True, 'fsm_mode': 'TakeOverRun', 'control_mapping': True, 'control_mapping_config': 'mathias_controller_keyboard', 'altitude_control': False, 'keyboard': True, 'mathias_controller_with_KF': True, 'mathias_controller_config_file_path_with_extension': f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF.yml', 'yaw_or': 0, 'x_pos': 0, 'y_pos': 0 } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=True) # subscribe to command control self.visualisation_topic = '/actor/mathias_controller/visualisation' subscribe_topics = [ TopicConfig( topic_name=rospy.get_param('/robot/position_sensor/topic'), msg_type=rospy.get_param('/robot/position_sensor/type')), TopicConfig(topic_name='/fsm/state', msg_type='String'), TopicConfig(topic_name=self.visualisation_topic, msg_type='Image') ] self._reference_topic = '/reference_pose' self._reference_type = 'PointStamped' publish_topics = [ TopicConfig(topic_name='/fsm/reset', msg_type='Empty'), TopicConfig(topic_name=self._reference_topic, msg_type=self._reference_type), ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics) self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice) self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice) safe_wait_till_true( '"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 30, 0.1, ros_topic=self.ros_topic) measured_data = {} index = 0 while True: # publish reset self.ros_topic.publishers['/fsm/reset'].publish(Empty()) self._unpause_client.wait_for_service() self._unpause_client.call() # index = self.tweak_combined_axis_keyboard(measured_data, index, [0, 0, 0]) index = self.tweak_combined_axis_keyboard(measured_data, index, [3, 1, 0])
def test_drone_keyboard_gazebo_with_KF_waypoints(self): self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) self._config = { 'output_path': self.output_dir, 'world_name': 'gate_test', 'robot_name': 'drone_sim', 'gazebo': True, 'fsm': True, 'fsm_mode': 'TakeOverRun', 'waypoint_indicator': True, 'control_mapping': True, 'control_mapping_config': 'mathias_controller_keyboard', 'altitude_control': False, 'keyboard': True, 'mathias_controller_with_KF': True, 'mathias_controller_config_file_path_with_extension': f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF.yml', 'yaw_or': 0, 'x_pos': 0, 'y_pos': 0 } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=True) # subscribe to command control self.visualisation_topic = '/actor/mathias_controller/visualisation' subscribe_topics = [ TopicConfig( topic_name=rospy.get_param('/robot/position_sensor/topic'), msg_type=rospy.get_param('/robot/position_sensor/type')), TopicConfig(topic_name='/fsm/state', msg_type='String'), TopicConfig(topic_name=self.visualisation_topic, msg_type='Image') ] self._reference_topic = '/reference_pose' self._reference_type = 'PointStamped' publish_topics = [ TopicConfig(topic_name='/fsm/reset', msg_type='Empty'), TopicConfig(topic_name=self._reference_topic, msg_type=self._reference_type), ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics) self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice) self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice) safe_wait_till_true( '"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 30, 0.1, ros_topic=self.ros_topic) measured_data = {} index = 0 while True: measured_data[index] = {'x': [], 'y': [], 'z': []} # publish reset self.ros_topic.publishers['/fsm/reset'].publish(Empty()) self._unpause_client.wait_for_service() self._unpause_client.call() # User flies drone to starting position while self.ros_topic.topic_values[ "/fsm/state"].data == FsmState.TakenOver.name: time.sleep(1) while self.ros_topic.topic_values[ "/fsm/state"].data == FsmState.Running.name: rospy.sleep(0.1) odom = self.ros_topic.topic_values[rospy.get_param( '/robot/position_sensor/topic')] waypoint = self.ros_topic.topic_values[ '/waypoint_indicator/current_waypoint'] measured_data[index]['x'].append(odom.pose.pose.position.x - waypoint.data[0]) measured_data[index]['y'].append(odom.pose.pose.position.y - waypoint.data[1]) measured_data[index]['z'].append(odom.pose.pose.position.z - 1) # see it reaches the goal state: # safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data', # FsmState.Terminated.name, 20, 0.1, ros_topic=self.ros_topic) colors = ['C0', 'C1', 'C2', 'C3', 'C4'] for key in measured_data.keys(): for a in measured_data[key].keys(): if a == 'x': style = '-' elif a == 'y': style = '--' else: style = ':' plt.plot(measured_data[key][a], linestyle=style, color=colors[key % len(colors)], label=f'{key}: {a}') plt.legend() plt.show() index += 1 index %= len(colors)
def tweak_combined_axis_keyboard(self, measured_data, index, point_ref_drone): initial_point_ref_drone = copy.deepcopy(point_ref_drone) # gets fsm in taken over state safe_wait_till_true( 'kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.TakenOver.name, 20, 0.1, ros_topic=self.ros_topic) # once drone is in good starting position invoke 'go' with key m while self.ros_topic.topic_values[ "/fsm/state"].data != FsmState.Running.name: # while taking off, update reference point for PID controller to remain at same height self.ros_topic.publishers[self._reference_topic].publish( PointStamped(header=Header(frame_id="agent"), point=Point(x=point_ref_drone[0], y=point_ref_drone[1], z=point_ref_drone[2]))) last_pose = self.get_pose() rospy.sleep(0.5) measured_data[index] = {'x': [], 'y': [], 'z': [], 'yaw': []} point_ref_global = transform(points=[np.asarray(point_ref_drone)], orientation=R.from_euler( 'XYZ', (0, 0, last_pose[-1]), degrees=False).as_matrix(), translation=np.asarray(last_pose[:3]))[0] # Mathias controller should keep drone in steady pose while self.ros_topic.topic_values[ "/fsm/state"].data != FsmState.TakenOver.name: point_drone_global = self.get_pose() point_ref_drone = Point( x=point_ref_global[0] - point_drone_global[0], y=point_ref_global[1] - point_drone_global[1], z=point_ref_global[2] - point_drone_global[2]) # rotate pose error to rotated frame pose_error_local = transform(points=[point_ref_drone], orientation=R.from_euler( 'XYZ', (0, 0, -last_pose[-1]), degrees=False).as_matrix())[0] measured_data[index]['x'].append(pose_error_local.x) measured_data[index]['y'].append(pose_error_local.y) measured_data[index]['z'].append(pose_error_local.z) # measured_data[index]['yaw'].append(last_pose[-1]) rospy.sleep(0.5) if False and '/bebop/land' in self.ros_topic.publishers.keys(): self.ros_topic.publishers['/bebop/land'].publish(Empty()) colors = ['C0', 'C1', 'C2', 'C3', 'C4'] styles = {'x': '-', 'y': '--', 'z': ':', 'yaw': '-.'} fig = plt.figure(figsize=(15, 15)) for key in measured_data.keys(): for a in measured_data[key].keys(): if len(measured_data[key][a]) != 0: plt.plot(measured_data[key][a], linestyle=styles[a], linewidth=3 if key == index else 1, color=colors[key % len(colors)], label=f'{key}: {a}') plt.legend() #plt.savefig(os.path.join(self.output_dir, 'measured_data.jpg')) plt.show() # print visualisation if it's in ros topic: if self.visualisation_topic in self.ros_topic.topic_values.keys(): frame = process_image( self.ros_topic.topic_values[self.visualisation_topic]) plt.figure(figsize=(15, 20)) plt.imshow(frame) plt.axis('off') plt.tight_layout() plt.savefig( f'{os.environ["HOME"]}/kf_study/pid_tweak_{initial_point_ref_drone[0]}_{initial_point_ref_drone[1]}_{initial_point_ref_drone[2]}.png' ) plt.clf() plt.close() index += 1 index %= len(colors) return index
def test_double_drone(self) -> None: self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) height = 5 self._config = { 'output_path': self.output_dir, 'world_name': 'empty', 'robot_name': 'double_drone_sim', 'gazebo': True, 'fsm': True, 'fsm_mode': 'TakeOverRun', 'control_mapping': False, 'control_mapping_config': 'python_adversarial', 'altitude_control': False, 'ros_expert': False, 'starting_height': height } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=False) # subscribe to command control subscribe_topics = [ TopicConfig(topic_name=rospy.get_param('/robot/tracking_position_sensor/topic'), msg_type=rospy.get_param('/robot/tracking_position_sensor/type')), TopicConfig(topic_name=rospy.get_param('/robot/fleeing_position_sensor/topic'), msg_type=rospy.get_param('/robot/fleeing_position_sensor/type')), TopicConfig(topic_name='/fsm/state', msg_type='String') ] publish_topics = [ TopicConfig(topic_name='/fsm/reset', msg_type='Empty') ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics ) self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice) self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice) self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # unpause gazebo to receive messages self._unpause_client.wait_for_service() self._unpause_client.call() safe_wait_till_true('"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 10, 0.1, ros_topic=self.ros_topic) self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data, FsmState.Unknown.name) # pause again before start self._pause_client.wait_for_service() self._pause_client.call() self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) def set_model_state(name: str): model_state = ModelState() model_state.pose = Pose() model_state.model_name = name model_state.pose.position.x = 0 if 'fleeing' in name: model_state.pose.position.x += 3. # fixed distance model_state.pose.position.y = 0 model_state.pose.position.z = 0 yaw = 0 if 'fleeing' not in name else 3.14 model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, \ model_state.pose.orientation.w = quaternion_from_euler((0, 0, yaw)) self._set_model_state.wait_for_service() self._set_model_state(model_state) for _ in range(3): print(f'round: {_}') for model_name in rospy.get_param('/robot/model_name'): set_model_state(model_name) # publish reset self.ros_topic.publishers['/fsm/reset'].publish(Empty()) self._unpause_client.wait_for_service() self._unpause_client.call() # gets fsm in taken over state safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.TakenOver.name, 2, 0.1, ros_topic=self.ros_topic) # altitude control brings drone to starting_height safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.Running.name, 45, 0.1, ros_topic=self.ros_topic) # check current height for agent in ['tracking', 'fleeing']: z_pos = self.ros_topic.topic_values[rospy.get_param(f'/robot/{agent}_position_sensor/topic')].pose.position.z self.assertLess(abs(z_pos - height), 0.2)
def test_takeoff_double_drone_sim(self): config = { "output_path": self.output_dir, "random_seed": 123, "robot_name": "double_drone_sim", "world_name": "empty", "gazebo": True, } ros_process = RosWrapper(launch_file='load_ros.launch', config=config, visible=False) self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice) self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice) self.ros_topic = TestPublisherSubscriber( subscribe_topics=[ TopicConfig(topic_name=rospy.get_param( '/robot/tracking_position_sensor/topic'), msg_type=rospy.get_param( '/robot/tracking_position_sensor/type')), TopicConfig(topic_name=rospy.get_param( '/robot/fleeing_position_sensor/topic'), msg_type=rospy.get_param( '/robot/fleeing_position_sensor/type')) ], publish_topics=[ TopicConfig(topic_name=rospy.get_param( '/robot/tracking_command_topic'), msg_type='Twist'), TopicConfig( topic_name=rospy.get_param('/robot/fleeing_command_topic'), msg_type='Twist') ]) # unpause gazebo to receive messages self._unpause_client.wait_for_service() self._unpause_client.call() safe_wait_till_true( f'\'{rospy.get_param("/robot/tracking_position_sensor/topic")}\' ' f'in kwargs["ros_topic"].topic_values.keys()', True, 5, 0.1, ros_topic=self.ros_topic) rospy.wait_for_service('/tracking/enable_motors') enable_motors_service_tracking = rospy.ServiceProxy( '/tracking/enable_motors', EnableMotors) rospy.wait_for_service('/fleeing/enable_motors') enable_motors_service_fleeing = rospy.ServiceProxy( '/fleeing/enable_motors', EnableMotors) # set gazebo model state self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) def set_model_state(name: str): model_state = ModelState() model_state.pose = Pose() model_state.model_name = name model_state.pose.position.x = 0 if 'fleeing' in name: model_state.pose.position.x += 3. # fixed distance model_state.pose.position.y = 0 model_state.pose.position.z = 0 yaw = 0 if 'fleeing' not in name else 3.14 model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, \ model_state.pose.orientation.w = quaternion_from_euler((0, 0, yaw)) self._set_model_state.wait_for_service() self._set_model_state(model_state) self._pause_client.wait_for_service() self._pause_client.call() for _ in range(3): for model_name in rospy.get_param('/robot/model_name'): set_model_state(model_name) self._unpause_client.wait_for_service() self._unpause_client.call() # start motors enable_motors_service_tracking.call(True) time.sleep(3) enable_motors_service_fleeing.call(True) # fly up till reference height reference = 1. errors = [1] while np.mean(errors) > 0.1: for agent in ['tracking', 'fleeing']: position = self.ros_topic.topic_values[rospy.get_param( f'/robot/{agent}_position_sensor/topic')].pose.position print(f'{agent}: {position.z}') errors.append(np.abs(reference - position.z)) twist = Twist() twist.linear.z = +0.5 if position.z < reference else -0.5 self.ros_topic.publishers[rospy.get_param( f'/robot/{agent}_command_topic')].publish(twist) while len(errors) > 10: errors.pop(0) time.sleep(0.1) final_error = abs(self.ros_topic.topic_values[rospy.get_param( '/robot/tracking_position_sensor/topic')].pose.position.z - reference) self.assertLess(final_error, 0.2) final_error = abs(self.ros_topic.topic_values[rospy.get_param( '/robot/fleeing_position_sensor/topic')].pose.position.z - reference) self.assertLess(final_error, 0.2) self._pause_client.wait_for_service() self._pause_client(EmptyRequest()) # enable_motors_service_tracking.call(False) # time.sleep(3) # enable_motors_service_fleeing.call(False) ros_process.terminate()
def test_takeoff_drone_sim(self): config = { "output_path": self.output_dir, "random_seed": 123, "robot_name": "drone_sim", "world_name": "empty", "gazebo": True, } ros_process = RosWrapper(launch_file='load_ros.launch', config=config, visible=False) self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice) self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice) self.ros_topic = TestPublisherSubscriber( subscribe_topics=[ TopicConfig( topic_name=rospy.get_param('/robot/position_sensor/topic'), msg_type=rospy.get_param('/robot/position_sensor/type')) ], publish_topics=[ TopicConfig(topic_name=rospy.get_param('/robot/command_topic'), msg_type='Twist') ]) # unpause gazebo to receive messages self._unpause_client.wait_for_service() self._unpause_client.call() safe_wait_till_true( f'\'{rospy.get_param("/robot/position_sensor/topic")}\' ' f'in kwargs["ros_topic"].topic_values.keys()', True, 5, 0.1, ros_topic=self.ros_topic) rospy.wait_for_service('/enable_motors') enable_motors_service = rospy.ServiceProxy('/enable_motors', EnableMotors) enable_motors_service.call(False) self._pause_client.wait_for_service() self._pause_client.call() self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) for _ in range(3): # set gazebo model state model_state = ModelState() model_state.model_name = 'quadrotor' model_state.pose = Pose() self._set_model_state.wait_for_service() self._set_model_state(model_state) self._unpause_client.wait_for_service() self._unpause_client.call() enable_motors_service.call(True) # fly up till reference height reference = 1. errors = [1] while np.mean(errors) > 0.05: position = self.ros_topic.topic_values[rospy.get_param( '/robot/position_sensor/topic')].pose.pose.position print(position.z) errors.append(np.abs(reference - position.z)) if len(errors) > 5: errors.pop(0) twist = Twist() twist.linear.z = +0.5 if position.z < reference else -0.5 self.ros_topic.publishers[rospy.get_param( '/robot/command_topic')].publish(twist) time.sleep(0.1) final_error = abs(self.ros_topic.topic_values[rospy.get_param( '/robot/position_sensor/topic')].pose.pose.position.z - reference) self.assertLess(final_error, 0.1) self._pause_client.wait_for_service() self._pause_client(EmptyRequest()) enable_motors_service.call(False) ros_process.terminate()
def test_april_tag_detector_gazebo_KF(self): self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) self._config = { 'output_path': self.output_dir, 'world_name': 'april_tag', 'robot_name': 'drone_sim_down_cam', 'gazebo': True, 'fsm': True, 'fsm_mode': 'TakeOverRun', 'control_mapping': True, 'control_mapping_config': 'mathias_controller_keyboard', 'april_tag_detector': True, 'altitude_control': False, 'mathias_controller_with_KF': True, 'starting_height': 1., 'keyboard': True, 'mathias_controller_config_file_path_with_extension': f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF.yml', } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=True) # subscribe to command control self.visualisation_topic = '/actor/mathias_controller/visualisation' subscribe_topics = [ TopicConfig( topic_name=rospy.get_param('/robot/position_sensor/topic'), msg_type=rospy.get_param('/robot/position_sensor/type')), TopicConfig(topic_name='/fsm/state', msg_type='String'), TopicConfig(topic_name='/reference_pose', msg_type='PointStamped'), TopicConfig(topic_name=self.visualisation_topic, msg_type='Image') ] publish_topics = [ TopicConfig(topic_name='/fsm/reset', msg_type='Empty'), ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics) safe_wait_till_true( '"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 235, 0.1, ros_topic=self.ros_topic) self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data, FsmState.Unknown.name) index = 0 while True: print(f'start loop: {index} with resetting') # publish reset self.ros_topic.publishers['/fsm/reset'].publish(Empty()) rospy.sleep(0.5) print(f'waiting in overtake state') while self.ros_topic.topic_values[ "/fsm/state"].data != FsmState.Running.name: rospy.sleep(0.5) # safe_wait_till_true('"/reference_ground_point" in kwargs["ros_topic"].topic_values.keys()', # True, 10, 0.1, ros_topic=self.ros_topic) waypoints = [] print(f'waiting in running state') while self.ros_topic.topic_values[ "/fsm/state"].data != FsmState.TakenOver.name: if '/reference_pose' in self.ros_topic.topic_values.keys() \ and '/bebop/odom' in self.ros_topic.topic_values.keys(): odom = self.ros_topic.topic_values[rospy.get_param( '/robot/position_sensor/topic')] point = transform( [self.ros_topic.topic_values['/reference_pose'].point], orientation=odom.pose.pose.orientation, translation=odom.pose.pose.position)[0] waypoints.append(point) rospy.sleep(0.5) if len(waypoints) != 0: plt.clf() fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter([_.x for _ in waypoints], [_.y for _ in waypoints], [_.z for _ in waypoints], label='waypoints') ax.legend() plt.savefig(os.path.join(self.output_dir, f'image_{index}.jpg')) # plt.show() index += 1
def test_single_drone(self) -> None: self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) height = 5 self._config = { 'output_path': self.output_dir, 'world_name': 'empty', 'robot_name': 'drone_sim', 'gazebo': True, 'fsm': True, 'fsm_mode': 'TakeOverRun', 'control_mapping': True, 'control_mapping_config': 'ros_expert', 'altitude_control': True, 'ros_expert': True, 'starting_height': height } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=True) # subscribe to command control subscribe_topics = [ TopicConfig(topic_name=rospy.get_param('/robot/position_sensor/topic'), msg_type=rospy.get_param('/robot/position_sensor/type')), TopicConfig(topic_name='/fsm/state', msg_type='String') ] publish_topics = [ TopicConfig(topic_name='/fsm/reset', msg_type='Empty') ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics ) self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice) self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice) self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # unpause gazebo to receive messages self._unpause_client.wait_for_service() self._unpause_client.call() safe_wait_till_true('"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 10, 0.1, ros_topic=self.ros_topic) self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data, FsmState.Unknown.name) # pause again before start self._pause_client.wait_for_service() self._pause_client.call() self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) for _ in range(3): print(f'round: {_}') # set gazebo model state model_state = ModelState() model_state.model_name = 'quadrotor' model_state.pose = Pose() self._set_model_state.wait_for_service() self._set_model_state(model_state) # publish reset self.ros_topic.publishers['/fsm/reset'].publish(Empty()) self._unpause_client.wait_for_service() self._unpause_client.call() # gets fsm in taken over state safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.TakenOver.name, 2, 0.1, ros_topic=self.ros_topic) # altitude control brings drone to starting_height safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data', FsmState.Running.name, 45, 0.1, ros_topic=self.ros_topic) # check current height z_pos = self.ros_topic.topic_values[rospy.get_param('/robot/position_sensor/topic')].pose.pose.position.z print(f'final_height: {z_pos}') self.assertLess(abs(z_pos - height), 0.2) self._pause_client.wait_for_service() self._pause_client.call()
def test_drone_relative_positioning_real_bebop(self): self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) self._config = { 'output_path': self.output_dir, 'world_name': 'empty', 'robot_name': 'bebop_real', 'gazebo': False, 'fsm': True, 'fsm_mode': 'TakeOverRun', 'control_mapping': True, 'control_mapping_config': 'mathias_controller_keyboard', 'altitude_control': False, 'keyboard': True, 'mathias_controller': True, 'mathias_controller_config_file_path_with_extension': f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_real_bebop.yml' } # spinoff roslaunch self._ros_process = RosWrapper(launch_file='load_ros.launch', config=self._config, visible=True) self.visualisation_topic = None # subscribe to command control subscribe_topics = [ TopicConfig( topic_name=rospy.get_param('/robot/position_sensor/topic'), msg_type=rospy.get_param('/robot/position_sensor/type')), TopicConfig(topic_name='/fsm/state', msg_type='String') ] self._reference_topic = '/reference_pose' self._reference_type = 'PointStamped' publish_topics = [ TopicConfig(topic_name='/fsm/reset', msg_type='Empty'), TopicConfig(topic_name=self._reference_topic, msg_type=self._reference_type), ] self.ros_topic = TestPublisherSubscriber( subscribe_topics=subscribe_topics, publish_topics=publish_topics) safe_wait_till_true( '"/fsm/state" in kwargs["ros_topic"].topic_values.keys()', True, 10, 0.1, ros_topic=self.ros_topic) measured_data = {} index = 0 while True: # publish reset self.ros_topic.publishers['/fsm/reset'].publish(Empty()) # index = self.tweak_steady_pose(measured_data, index) #index = self.tweak_separate_axis_keyboard(measured_data, index, axis=0) index = self.tweak_combined_axis_keyboard( measured_data, index, point_ref_drone=[0.5, 0.5, 0.5])