From 897559e9fc80ee7fe9980817daa090821f3c876c Mon Sep 17 00:00:00 2001 From: Robin Shaun Date: Wed, 20 May 2020 13:30:28 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0yolo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- communication/multirotor_communication.py | 34 +- communication/plane_communication.py | 15 +- communication/rover_communication.py | 22 +- communication/vtol_communication.py | 34 +- control/multirotor_keyboard_control.py | 2 +- control/plane_keyboard_control.py | 4 +- control/rover_keyboard_control.py | 2 +- control/rover_self_driving.py | 7 +- control/vtol_keyboard_multi_control.py | 4 +- control/yolo_human_tracking.py | 50 +- coordination/launch/launch_head_1.11 | 20 - coordination/launch/multi_vehicle.launch | 196 ---- .../{launch => launch_generator}/generator.py | 0 .../launch_generator/multi_vehicle.launch | 859 +++++++++++++++++- .../darknet_ros/config/px4_tracking.yaml | 2 +- 15 files changed, 934 insertions(+), 317 deletions(-) delete mode 100644 coordination/launch/launch_head_1.11 delete mode 100644 coordination/launch/multi_vehicle.launch rename coordination/{launch => launch_generator}/generator.py (100%) diff --git a/communication/multirotor_communication.py b/communication/multirotor_communication.py index 14d89c0..3742337 100644 --- a/communication/multirotor_communication.py +++ b/communication/multirotor_communication.py @@ -102,17 +102,31 @@ class Communication: target_raw_pose = PositionTarget() target_raw_pose.coordinate_frame = self.coordinate_frame - target_raw_pose.position.x = x - target_raw_pose.position.y = y - target_raw_pose.position.z = z + if self.coordinate_frame == 1: + target_raw_pose.position.x = x + target_raw_pose.position.y = y + target_raw_pose.position.z = z - target_raw_pose.velocity.x = vx - target_raw_pose.velocity.y = vy - target_raw_pose.velocity.z = vz - - target_raw_pose.acceleration_or_force.x = afx - target_raw_pose.acceleration_or_force.y = afy - target_raw_pose.acceleration_or_force.z = afz + target_raw_pose.velocity.x = vx + target_raw_pose.velocity.y = vy + target_raw_pose.velocity.z = vz + + target_raw_pose.acceleration_or_force.x = afx + target_raw_pose.acceleration_or_force.y = afy + target_raw_pose.acceleration_or_force.z = afz + + else: + target_raw_pose.position.x = -y + target_raw_pose.position.y = x + target_raw_pose.position.z = z + + target_raw_pose.velocity.x = -vy + target_raw_pose.velocity.y = vx + target_raw_pose.velocity.z = vz + + target_raw_pose.acceleration_or_force.x = afx + target_raw_pose.acceleration_or_force.y = afy + target_raw_pose.acceleration_or_force.z = afz target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate diff --git a/communication/plane_communication.py b/communication/plane_communication.py index 608dc06..6d7abb2 100644 --- a/communication/plane_communication.py +++ b/communication/plane_communication.py @@ -84,11 +84,16 @@ class Communication: def construct_target(self, x=0, y=0, z=0): target_raw_pose = PositionTarget() - target_raw_pose.coordinate_frame = 1 + target_raw_pose.coordinate_frame = self.coordinate_frame - target_raw_pose.position.x = x - target_raw_pose.position.y = y - target_raw_pose.position.z = z + if self.coordinate_frame == 1: + target_raw_pose.position.x = x + target_raw_pose.position.y = y + target_raw_pose.position.z = z + else: + target_raw_pose.position.x = -y + target_raw_pose.position.y = x + target_raw_pose.position.z = z if self.mission == 'takeoff': target_raw_pose.type_mask = 4096 @@ -102,9 +107,11 @@ class Communication: return target_raw_pose def cmd_pose_flu_callback(self, msg): + self.coordinate_frame = 9 self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z) def cmd_pose_enu_callback(self, msg): + self.coordinate_frame = 1 self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z) def cmd_callback(self, msg): diff --git a/communication/rover_communication.py b/communication/rover_communication.py index e7ea795..17ee327 100644 --- a/communication/rover_communication.py +++ b/communication/rover_communication.py @@ -92,13 +92,23 @@ class Communication: target_raw_pose = PositionTarget() target_raw_pose.coordinate_frame = self.coordinate_frame - target_raw_pose.position.x = x - target_raw_pose.position.y = y - target_raw_pose.position.z = z + if self.coordinate_frame == 1: + target_raw_pose.position.x = x + target_raw_pose.position.y = y + target_raw_pose.position.z = z - target_raw_pose.velocity.x = vx - target_raw_pose.velocity.y = vy - target_raw_pose.velocity.z = vz + target_raw_pose.velocity.x = vx + target_raw_pose.velocity.y = vy + target_raw_pose.velocity.z = vz + + else: + target_raw_pose.position.x = -y + target_raw_pose.position.y = x + target_raw_pose.position.z = z + + target_raw_pose.velocity.x = -vy + target_raw_pose.velocity.y = vx + target_raw_pose.velocity.z = vz target_raw_pose.yaw = yaw diff --git a/communication/vtol_communication.py b/communication/vtol_communication.py index ed0a05e..1082439 100644 --- a/communication/vtol_communication.py +++ b/communication/vtol_communication.py @@ -105,9 +105,14 @@ class Communication: target_raw_pose = PositionTarget() target_raw_pose.coordinate_frame = self.coordinate_frame - target_raw_pose.position.x = x - target_raw_pose.position.y = y - target_raw_pose.position.z = z + if self.coordinate_frame == 1: + target_raw_pose.position.x = x + target_raw_pose.position.y = y + target_raw_pose.position.z = z + else: + target_raw_pose.position.x = -y + target_raw_pose.position.y = x + target_raw_pose.position.z = z if self.transition_state == 'plane': if self.plane_mission == 'takeoff': target_raw_pose.type_mask = 4096 @@ -118,13 +123,22 @@ class Communication: else: target_raw_pose.type_mask = 16384 else: - target_raw_pose.velocity.x = vx - target_raw_pose.velocity.y = vy - target_raw_pose.velocity.z = vz - - target_raw_pose.acceleration_or_force.x = afx - target_raw_pose.acceleration_or_force.y = afy - target_raw_pose.acceleration_or_force.z = afz + if self.coordinate_frame == 1: + target_raw_pose.velocity.x = vx + target_raw_pose.velocity.y = vy + target_raw_pose.velocity.z = vz + + target_raw_pose.acceleration_or_force.x = afx + target_raw_pose.acceleration_or_force.y = afy + target_raw_pose.acceleration_or_force.z = afz + else: + target_raw_pose.velocity.x = -vy + target_raw_pose.velocity.y = vx + target_raw_pose.velocity.z = vz + + target_raw_pose.acceleration_or_force.x = -afy + target_raw_pose.acceleration_or_force.y = afx + target_raw_pose.acceleration_or_force.z = afz target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate diff --git a/control/multirotor_keyboard_control.py b/control/multirotor_keyboard_control.py index a58e29e..bbc603f 100755 --- a/control/multirotor_keyboard_control.py +++ b/control/multirotor_keyboard_control.py @@ -249,7 +249,7 @@ if __name__=="__main__": elif angular < -MAX_ANG_VEL: angular = - MAX_ANG_VEL - twist.linear.x = -leftward; twist.linear.y = forward ; twist.linear.z = upward + twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upward twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = angular for i in range(multirotor_num): diff --git a/control/plane_keyboard_control.py b/control/plane_keyboard_control.py index dd5c431..ac46185 100755 --- a/control/plane_keyboard_control.py +++ b/control/plane_keyboard_control.py @@ -90,7 +90,7 @@ if __name__=="__main__": multi_cmd_vel_flu_pub = [None]*plane_num multi_cmd_pub = [None]*plane_num for i in range(plane_num): - multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/plane_'+str(i)+'/cmd_pose_enu', Pose, queue_size=10) + multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/plane_'+str(i)+'/cmd_pose_flu', Pose, queue_size=10) multi_cmd_pub[i] = rospy.Publisher('/xtdrone/plane_'+str(i)+'/cmd',String,queue_size=10) leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_pose", Pose, queue_size=10) leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10) @@ -202,7 +202,7 @@ if __name__=="__main__": elif angular < -MAX_ANG_VEL: angular = - MAX_ANG_VEL - pose.position.x = leftward; pose.position.y = forward; pose.position.z = upward + pose.position.x = forward; pose.position.y = leftward; pose.position.z = upward pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = angular diff --git a/control/rover_keyboard_control.py b/control/rover_keyboard_control.py index 2b5838c..d484842 100755 --- a/control/rover_keyboard_control.py +++ b/control/rover_keyboard_control.py @@ -163,7 +163,7 @@ if __name__=="__main__": elif angle < -MAX_ANGLE: angle = -MAX_ANGLE - twist.linear.x = -angle; twist.linear.y = forward + twist.linear.x = forward; twist.linear.y = angle for i in range(rover_num): if ctrl_leader: diff --git a/control/rover_self_driving.py b/control/rover_self_driving.py index 080bc33..cd7ef4e 100644 --- a/control/rover_self_driving.py +++ b/control/rover_self_driving.py @@ -12,10 +12,11 @@ def lane_mid_error_callback(msg): twist.linear.y = 0.0 else: if abs(msg.data) > 20: - twist.linear.x = Kp * msg.data + twist.linear.y = - Kp * msg.data else: - twist.linear.x = 0.0 - twist.linear.y = Vx * (1 - twist.linear.x) + twist.linear.y = 0.0 + + twist.linear.x = Vx * (1 - twist.linear.x) if __name__ == "__main__": diff --git a/control/vtol_keyboard_multi_control.py b/control/vtol_keyboard_multi_control.py index 6d1c6c7..47cafe7 100755 --- a/control/vtol_keyboard_multi_control.py +++ b/control/vtol_keyboard_multi_control.py @@ -265,12 +265,12 @@ if __name__=="__main__": if transition_state == 'plane': - pose.position.x = -leftward; pose.position.y = forward; pose.position.z = upward + pose.position.x = forward; pose.position.y = leftward; pose.position.z = upward pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = angular else: - twist.linear.x = - leftward; twist.linear.y = forward; twist.linear.z = upward + twist.linear.x = forward; twist.linear.y = leftward; twist.linear.z = upward twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = angular diff --git a/control/yolo_human_tracking.py b/control/yolo_human_tracking.py index cd758e0..b733245 100644 --- a/control/yolo_human_tracking.py +++ b/control/yolo_human_tracking.py @@ -1,40 +1,44 @@ import rospy from geometry_msgs.msg import Twist +from std_msgs.msg import String import sys sys.path.append('/home/robin/catkin_ws/devel/lib/python2.7/dist-packages') from darknet_ros_msgs.msg import BoundingBoxes import time import math -twist = Twist() + def darknet_callback(data): + global twist, cmd for target in data.bounding_boxes: - print(target.id) if(target.id==0): + print('find human') x_error=y_center-(target.ymax+target.ymin)/2 y_error=x_center-(target.xmax+target.xmin)/2 twist.linear.x = Kp_linear*x_error - twist.linear.y = 0.0 - twist.linear.z = 0.0 - twist.angular.x = 0.0 - twist.angular.y = 0.0 twist.angular.z = Kp_angular*math.atan(y_error/x_error) + cmd = '' else: twist.linear.x = 0.0 - twist.linear.y = 0.0 - twist.linear.z = 0.0 - twist.angular.x = 0.0 - twist.angular.y = 0.0 - twist.angular.z = 0.0 - -Kp_linear=0.05 -Kp_angular=0.2/math.pi -x_center=752/2 -y_center=480/2 -rospy.init_node('yolo_human_tracking') -rospy.Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes, darknet_callback) -pub = rospy.Publisher('/xtdrone/cmd_vel_flu', Twist, queue_size=10) -rate = rospy.Rate(60) -while(True): - rate.sleep() - pub.publish(twist) + twist.angular.z = 0.0 + cmd = 'HOVER' + + +if __name__ == "__main__": + twist = Twist() + cmd = String() + Kp_linear=0.05 + Kp_angular=0.2/math.pi + x_center=752/2 + y_center=480/2 + vehicle_type = sys.argv[1] + rospy.init_node('yolo_human_tracking') + rospy.Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes, darknet_callback) + vel_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_0/cmd_vel_flu', Twist, queue_size=2) + cmd_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_0/cmd', String, queue_size=2) + rate = rospy.Rate(60) + while(True): + rate.sleep() + vel_pub.publish(twist) + cmd_pub.publish(cmd) + diff --git a/coordination/launch/launch_head_1.11 b/coordination/launch/launch_head_1.11 deleted file mode 100644 index 403fd8a..0000000 --- a/coordination/launch/launch_head_1.11 +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/coordination/launch/multi_vehicle.launch b/coordination/launch/multi_vehicle.launch deleted file mode 100644 index 1457ba1..0000000 --- a/coordination/launch/multi_vehicle.launch +++ /dev/null @@ -1,196 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/coordination/launch/generator.py b/coordination/launch_generator/generator.py similarity index 100% rename from coordination/launch/generator.py rename to coordination/launch_generator/generator.py diff --git a/coordination/launch_generator/multi_vehicle.launch b/coordination/launch_generator/multi_vehicle.launch index 1cde451..23d9564 100644 --- a/coordination/launch_generator/multi_vehicle.launch +++ b/coordination/launch_generator/multi_vehicle.launch @@ -21,21 +21,21 @@ - + - + - + - - - + + + @@ -50,21 +50,21 @@ - + - + - - + + - - - + + + @@ -79,21 +79,108 @@ - + - + - - + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -108,21 +195,21 @@ - + - + - - + + - - + + @@ -137,21 +224,21 @@ - + - + - - + + - - + + @@ -166,21 +253,717 @@ - + - + - - + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/object_detection_and_tracking/YOLO/darknet_ros/darknet_ros/config/px4_tracking.yaml b/sensing/object_detection_and_tracking/YOLO/darknet_ros/darknet_ros/config/px4_tracking.yaml index dcbca04..bda7631 100644 --- a/sensing/object_detection_and_tracking/YOLO/darknet_ros/darknet_ros/config/px4_tracking.yaml +++ b/sensing/object_detection_and_tracking/YOLO/darknet_ros/darknet_ros/config/px4_tracking.yaml @@ -1,7 +1,7 @@ subscribers: camera_reading: - topic: /stereo/camera/left/image_raw + topic: /iris_0/stereo/camera/left/image_raw queue_size: 1 actions: