From 0f87f7cd4d9aedce5fc582ef2b6a06587b0c5e20 Mon Sep 17 00:00:00 2001 From: Cadart Nicolas Date: Mon, 25 Feb 2019 18:16:30 +0100 Subject: [PATCH] [*] Several changes. Code used to generate POC videos. --- config/rviz_config.rviz | 99 ++++++++++++++++-------------------- launch/ardrone_driver.launch | 2 +- src/flight_manager_node.py | 61 +++++++++++++--------- src/navigation_node.py | 12 ++--- 4 files changed, 87 insertions(+), 87 deletions(-) diff --git a/config/rviz_config.rviz b/config/rviz_config.rviz index 0d5db49..3fb9752 100644 --- a/config/rviz_config.rviz +++ b/config/rviz_config.rviz @@ -1,17 +1,16 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Status1 - /TF1 - - /TF1/Frames1 - - /Image1 - - /Pose1 + - /TF1/Tree1 + - /TF1/Tree1/odom1 + - /TF1/Tree1/odom1/ardrone_base_link1 + - /TF1/Tree1/odom1/ardrone_base_link1/ardrone_base_bottomcam_new1 Splitter Ratio: 0.719444454 - Tree Height: 361 + Tree Height: 457 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -30,7 +29,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Image + SyncSource: Camera Visualization Manager: Class: "" Displays: @@ -52,36 +51,17 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Lines - Line Width: 0.0299999993 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: /rviz/navigation/traj - Unreliable: false - Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: false + ar_marker_3: + Value: true ardrone_base_bottomcam: Value: false + ardrone_base_bottomcam_new: + Value: false ardrone_base_frontcam: Value: false ardrone_base_link: @@ -98,39 +78,48 @@ Visualization Manager: ardrone_base_link: ardrone_base_bottomcam: {} + ardrone_base_bottomcam_new: + ar_marker_3: + {} ardrone_base_frontcam: {} Update Interval: 0 Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /ardrone/bottom/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Class: rviz/Marker Enabled: true Marker Topic: /visualization_marker Name: Marker Namespaces: - {} + basic_shapes: true + main_shapes: true Queue Size: 100 Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /ardrone/bottom_new/image_raw + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Visibility: + Grid: true + Marker: true + Pose Goal: true + TF: true + Value: true + Zoom Factor: 1 - Alpha: 1 Axes Length: 0.200000003 - Axes Radius: 0.0299999993 + Axes Radius: 0.0250000004 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.300000012 Head Radius: 0.100000001 - Name: Pose + Name: Pose Goal Shaft Length: 1 Shaft Radius: 0.0500000007 Shape: Axes @@ -162,35 +151,35 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 5.80267429 + Distance: 3.01293278 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.286439419 - Y: -0.00250601768 - Z: 0.000181674957 + X: 0.656870425 + Y: -0.0427799486 + Z: 0.409981489 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 - Pitch: 0.669797122 + Pitch: 0.424797416 Target Frame: Value: Orbit (rviz) - Yaw: 0.648460448 + Yaw: 2.34346151 Saved: ~ Window Geometry: + Camera: + collapsed: false Displays: collapsed: false Height: 1026 Hide Left Dock: false Hide Right Dock: true - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b200000378fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001f7000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002250000017b0000001600fffffffb0000000a0049006d00610067006501000002aa000000f60000000000000000fb0000000a0049006d00610067006501000002d7000000c90000000000000000fb0000000c00430061006d00650072006100000002c1000000160000000000000000fb0000000a0049006d00610067006501000002d2000000ce0000000000000000000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000378000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000025200000378fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000209000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001af000000bb0000000000000000fb0000000a0049006d00610067006501000002aa000000f60000000000000000fb0000000a0049006d00610067006501000002d7000000c90000000000000000fb0000000c00430061006d0065007200610100000237000001690000001600fffffffb0000000a0049006d00610067006501000002d2000000ce0000000000000000fb0000000c00430061006d00650072006101000002640000013c0000000000000000fb0000000a0049006d006100670065000000027d000001230000000000000000000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000378000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004e70000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/launch/ardrone_driver.launch b/launch/ardrone_driver.launch index d841188..cb14fd4 100644 --- a/launch/ardrone_driver.launch +++ b/launch/ardrone_driver.launch @@ -17,6 +17,6 @@ - + diff --git a/src/flight_manager_node.py b/src/flight_manager_node.py index d34e1d8..137b5d3 100755 --- a/src/flight_manager_node.py +++ b/src/flight_manager_node.py @@ -33,24 +33,25 @@ class STATE(IntEnum): FRAME_WORLD = "odom" # base frame # time parameters -LOOP_RATE = 10. # [Hz] rate of the ROS node loop +LOOP_RATE = 20. # [Hz] rate of the ROS node loop BUNDLE_DETECTION_TIMEOUT = 1. # [s] if a bundle detection is older than this, we go back to FINDING state BUNDLE_FINDING_DISTANCE_FACTOR = 0.10 # [m] how much we increase distance from approximate target position at each step # Flight parameters TAKEOFF_ALLOWED = True # if False, no takeoff order will be sent (Test mode) -FLIGHT_ALTITUDE = 1. # [m] general altitude of flight for the drone +FLIGHT_ALTITUDE = 1.20 # [m] general altitude of flight for the drone FLIGHT_PRECISION = 0.20 # [m] tolerance to reach specified target -LANDING_FACTOR = 0.8 # at each iteration, the drone multiply its distance to target by this factor -LANDING_MIN_ALTITUDE = 0.20 # [m] below this altitude, the drone stops flying and tries to land +LANDING_FACTOR = 0.7 # at each iteration, the drone multiply its distance to target by this factor +LANDING_MIN_ALTITUDE = 0.50 # [m] below this altitude, the drone stops flying and tries to land class FlightManager: def __init__(self): """ Object constructor. """ rospy.loginfo("Waiting for 'ardrone_autonomy' node...") + rospy.on_shutdown(self.on_shutdown) # ROS subscribers and publishers rospy.wait_for_service("/ardrone/setcamchannel") @@ -58,9 +59,9 @@ def __init__(self): self.led_srv = rospy.ServiceProxy("/ardrone/setledanimation", LedAnim) self.takeoff_pub = rospy.Publisher("/ardrone/takeoff", Empty, queue_size=1) self.land_pub = rospy.Publisher("/ardrone/land", Empty, queue_size=1) - self.nav_pub = rospy.Publisher("/pose_goal", NavigationGoal, queue_size=1) + self.nav_pub = rospy.Publisher("/pose_goal", NavigationGoal, queue_size=5) self.command_sub = rospy.Subscriber("/command", ArdroneCommand, self._command_callback, queue_size=1) - self.bundle_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, self._bundle_callback, queue_size=5) + self.bundle_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, self._bundle_callback, queue_size=10) self.navdata_sub = rospy.Subscriber("/ardrone/navdata", Navdata, self._navdata_callback, queue_size=5) # TF management @@ -85,13 +86,11 @@ def __init__(self): self.bundle_pose_received = False # ensure bottom camera is selected + rospy.sleep(rospy.Duration(0.5)) self.cam_srv(1) # send null pose to navigation - nav_target = NavigationGoal() - nav_target.header.frame_id = FRAME_DRONE - nav_target.mode = NavigationGoal.RELATIVE - self.nav_pub.publish(nav_target) + self.send_nav_null_order() rospy.loginfo("Flight manager successfully initialized and ready.") @@ -119,9 +118,6 @@ def run(self): rate.sleep() - # land on exit - self.land_pub.publish() - # ======================= States loops ======================= def off_loop(self): @@ -148,6 +144,13 @@ def reaching_loop(self): """ Fly to approximate target where bundle is supposed to be located. """ + # if bundle detection has been received, target has been found : abort reaching loop + if self.bundle_pose_received and self.command in {ArdroneCommand.TRACK, ArdroneCommand.LAND}: + self.bundle_pose_received = False + self.send_nav_null_order() + self._change_state(STATE.TRACKING) + return + # if new target has been received, send its pose to navigation node if self.target_point_received: self.target_point_received = False @@ -178,11 +181,7 @@ def finding_loop(self): # if new bundle detection has been received, target has been found ! if self.bundle_pose_received: self.bundle_pose_received = False - # send null pose to navigation - nav_target = NavigationGoal() - nav_target.header.frame_id = FRAME_DRONE - nav_target.mode = NavigationGoal.RELATIVE - self.nav_pub.publish(nav_target) + self.send_nav_null_order() # if command was only to find target, reset order and standby if self.command == ArdroneCommand.FIND: self.command = ArdroneCommand.STANDBY @@ -277,8 +276,10 @@ def landing_loop(self): nav_target.header = above_bundle_pose.header nav_target.pose.position = above_bundle_pose.pose.position nav_target.mode = NavigationGoal.ABSOLUTE - self.target_point.point = nav_target.pose.position self.nav_pub.publish(nav_target) + self.target_point.point.x = nav_target.pose.position.x + self.target_point.point.y = nav_target.pose.position.y + self.target_point.point.z = nav_target.pose.position.z + FLIGHT_ALTITUDE - LANDING_FACTOR * distance # if drone is landing or has landed, go to OFF state if self.drone_state in {0, 1, 2, 8}: @@ -311,11 +312,7 @@ def _change_state(self, new_state, warn=False, previous=None): # GREEN when drone is ready and landed if new_state == STATE.OFF: - # send null pose to navigation - nav_target = NavigationGoal() - nav_target.header.frame_id = FRAME_DRONE - nav_target.mode = NavigationGoal.RELATIVE - self.nav_pub.publish(nav_target) + self.send_nav_null_order() self.led_srv(type=8, freq=1.0, duration=5) # BLINK_GREEN_RED when we are looking for target @@ -330,6 +327,18 @@ def _change_state(self, new_state, warn=False, previous=None): # change state self.state = new_state + def send_nav_null_order(self): + """ Send null command to navigation node to disable it """ + nav_target = NavigationGoal() + nav_target.header.frame_id = FRAME_DRONE + nav_target.mode = NavigationGoal.RELATIVE + self.nav_pub.publish(nav_target) + + def on_shutdown(self): + # land on exit + self.send_nav_null_order() + self.land_pub.publish() + # ======================= ROS callbacks ======================= def _bundle_callback(self, msg): @@ -441,4 +450,6 @@ def _command_callback(self, msg): flight_manager = FlightManager() flight_manager.run() except rospy.ROSInterruptException: - print("Shutting down flight manager node...") + pass + + print("Shutting down flight manager node...") diff --git a/src/navigation_node.py b/src/navigation_node.py index 9e8122e..3b49df1 100755 --- a/src/navigation_node.py +++ b/src/navigation_node.py @@ -67,8 +67,8 @@ def __init__(self): 'rot_z': 0.7} # init PID gains - self.pid_gains = {'trans_x': {'P': 0.15, 'I': 0.01, 'D': 0.05}, - 'trans_y': {'P': 0.15, 'I': 0.01, 'D': 0.05}, + self.pid_gains = {'trans_x': {'P': 0.2, 'I': 0.01, 'D': 0.03}, + 'trans_y': {'P': 0.2, 'I': 0.01, 'D': 0.03}, 'trans_z': {'P': 3., 'I': 0.01, 'D': 0.05}, 'rot_z': {'P': 0., 'I': 0., 'D': 0.}} @@ -137,10 +137,10 @@ def update(self): # otherwise, update order with computed PID commands else: # TODO : check units (m/s or mm/s, rad/s or deg/s) - cmd_vel.linear.x = command['trans_x'] # if abs(command['trans_x']) > 0.1 else 0. - cmd_vel.linear.y = command['trans_y'] # if abs(command['trans_y']) > 0.1 else 0. - cmd_vel.linear.z = command['trans_z'] # if abs(command['trans_z']) > 0.1 else 0. - cmd_vel.angular.z = command['rot_z'] # if abs(command['rot_z']) > 0.1 else 0. + cmd_vel.linear.x = command['trans_x'] + cmd_vel.linear.y = command['trans_y'] + cmd_vel.linear.z = command['trans_z'] + cmd_vel.angular.z = command['rot_z'] # publish the command msg to drone self.pub_command.publish(cmd_vel)