add contributors

2021-10-03 10:37:32 +08:00 · 2021-10-03 10:37:32 +08:00 · 972d06dc31
parent ba08835670
commit 972d06dc31
7 changed files with 325 additions and 13 deletions
--- a/README.en.md
+++ b/README.en.md
@ -95,13 +95,13 @@ USV

 - Founders: Kun Xiao, Shaochang Tan
 - Adviser: Xiangke Wang
- Developers: Kun Xiao, Shaochang Tan, Guanzheng Wang, Lan Ma, Qipeng Wang, Xinyu Hu, Ruoqiao Guan, Wenxin Hu,  Feng Yi, Jiarun Yan, Yi Bao, Keyan Chen, Gao Chen
+- Developers: Kun Xiao, Shaochang Tan, Guanzheng Wang, Lan Ma, Qipeng Wang, Xinyu Hu, Ruoqiao Guan, Wenxin Hu, Feng Yi, Jiarun Yan, Yi Bao

 ### Contributers

 Sincerely thank you for your contribution to XTDrone.

-Changhao Sun, Ying Nie, Fanjie Kong, Chaoran Li, Xudong Li, Huaqing Zhang, Zihan Lin, Yao He
+Keyan Chen, Jiangwei Xu, Yongguang Lu, Gao Chen, Changhao Sun, Ying Nie, Fanjie Kong, Chaoran Li, Xudong Li, Huaqing Zhang, Zihan Lin, Yao He

 ### Business cooperation

--- a/README.md
+++ b/README.md
@ -95,7 +95,7 @@ K. Xiao, S. Tan, G. Wang, X. An, X. Wang and X. Wang, "XTDrone: A Customizable M

 - 创立者：肖昆，谭劭昌
 - 指导老师：王祥科
- 开发团队：肖昆，谭劭昌，王冠政，马澜，王齐鹏，胡新雨，管若乔，胡文信，易丰，颜佳润，鲍毅，陈科研，陈皋
+- 开发团队：肖昆，谭劭昌，王冠政，马澜，王齐鹏，胡新雨，管若乔，胡文信，易丰，颜佳润，鲍毅

 ### 加入我们

@ -105,7 +105,7 @@ K. Xiao, S. Tan, G. Wang, X. An, X. Wang and X. Wang, "XTDrone: A Customizable M

 非常感谢你们为XTDrone的贡献

-孙长浩，聂莹，孔凡杰，李超然，李旭东，张华卿，林梓涵，何瑶 
+陈科研，许江伟，卢永光，陈皋，孙长浩，聂莹，孔凡杰，李超然，李旭东，张华卿，林梓涵，何瑶 

 ### 中国机器人大赛无人机挑战赛仿真组

--- a/communication/multirotor_communication.py
+++ b/communication/multirotor_communication.py
@ -1,4 +1,5 @@
 import rospy
+import math
 from mavros_msgs.msg import State, PositionTarget
 from mavros_msgs.srv import CommandBool, SetMode
 from geometry_msgs.msg import PoseStamped, Pose, Twist
@ -60,9 +61,9 @@ class Communication:
        while not rospy.is_shutdown():
            self.target_motion_pub.publish(self.target_motion)
            
-            if (self.flight_mode is "LAND") and (self.current_position.z < 0.15):
-                if(self.disarm()):
-                    self.flight_mode = "DISARMED"
+            # if (self.flight_mode is "LAND") and (self.current_position.z < 0.15):
+            #     if(self.disarm()):
+            #         self.arm_state = "DISARMED"
                    
            try:
                response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')
@ -109,7 +110,7 @@ class Communication:
        if(self.motion_type == 1):
            target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
                            + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
-                            + PositionTarget.IGNORE_YAW
+                            + PositionTarget.IGNORE_YAW_RATE
        if(self.motion_type == 2):
            target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
                            + PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
@ -134,7 +135,7 @@ class Communication:
        if self.hover_flag == 0:
            self.coordinate_frame = 8
            self.motion_type = 1     
-            self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)       
+            self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw = -math.pi/2)       
 
    def cmd_vel_enu_callback(self, msg):
        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
--- a/control/dev/arm/arm.py
+++ b/control/dev/arm/arm.py
@ -0,0 +1,47 @@
+# -*- encoding: utf-8 -*-
+import socket
+import time
+import kinematics
+import LeArm
+import sys
+IP = '192.168.149.2' #填写服务器端的IP地址
+port = 40005 #端口号必须一致
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+last_target_x = 0
+last_target_y = 0
+last_target_z = 0
+try:
+    s.connect((IP,port))
+except Exception as e:
+    print('server not find or not open')
+    sys.exit(0)
+LeArm.setServo(1, 500, 20)
+LeArm.setServo(3, 800, 20)
+LeArm.setServo(4, 900, 20)
+LeArm.setServo(5, 800, 20)
+while True:
+    trigger = "Ready"
+    s.sendall(trigger.encode())
+    data = s.recv(1024)
+    data = data.decode()
+    print('recieved:',data)
+    target = data.split(',')
+    target_x = int(float(target[0]))
+    target_y = int(float(target[1]))
+    target_z = int(float(target[2]))
+    if (target_x-last_target_x)**2+(target_y-last_target_y)**2+(target_z-last_target_z)**2 < 90000:
+        continue
+    LeArm.setServo(1, 500, 20)
+    LeArm.setServo(3, 800, 20)
+    LeArm.setServo(4, 900, 20)
+    LeArm.setServo(5, 800, 20)
+    time.sleep(0.33)
+    kinematics.ki_move(target_x,target_y,target_z,20)
+    time.sleep(0.33)
+    LeArm.setServo(1, 1300, 20)
+    time.sleep(0.5)
+    last_target_x = target_x
+    last_target_y = target_y
+    last_target_z = target_z
+s.close()
+
--- a/control/dev/arm/arm.rviz
+++ b/control/dev/arm/arm.rviz
@ -0,0 +1,173 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 274
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        camera_link:
+          Value: true
+        d400_aligned_depth_to_color_frame:
+          Value: true
+        d400_color_frame:
+          Value: true
+        d400_color_optical_frame:
+          Value: true
+        d400_depth_frame:
+          Value: true
+        d400_depth_optical_frame:
+          Value: true
+        d400_link:
+          Value: true
+        t265_link:
+          Value: true
+        t265_odom_frame:
+          Value: true
+        t265_pose_frame:
+          Value: true
+        tag_3:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        camera_link:
+          tag_3:
+            {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /tag_detections_image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: camera_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000035300000398fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000019d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001ca000001f50000001600ffffff000000010000010f00000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002700000398000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002d10000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 24
--- a/control/dev/arm/arm_control.py
+++ b/control/dev/arm/arm_control.py
@ -0,0 +1,95 @@
+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+
+import rospy
+from tf2_ros import TransformListener, Buffer
+from geometry_msgs.msg import Twist, PoseStamped, TransformStamped
+from pyquaternion import Quaternion
+import socket           
+import time
+import sys
+
+def local_pose_callback(data):
+    global local_pose, z_cnt 
+    if abs(local_pose.pose.position.z - data.pose.position.z) < 0.005:
+        z_cnt = z_cnt + 1
+    else:
+        z_cnt = 0
+    local_pose = data
+
+if __name__ == '__main__':
+    vehicle_type = sys.argv[1]
+    vehicle_id = sys.argv[2]
+    rospy.init_node(vehicle_type+'_'+vehicle_id+'_arm_control')
+    cmd_vel_enu = Twist()   
+    local_pose = PoseStamped()
+    time_cnt = 0
+    z_cnt = 0
+    rospy.Subscriber(vehicle_type+'_'+vehicle_id+"/mavros/vision_pose/pose", PoseStamped, local_pose_callback)
+    cmd_vel_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd_vel_flu', Twist, queue_size=2)
+    tfBuffer = Buffer()
+    tflistener = TransformListener(tfBuffer)
+    rate = rospy.Rate(20)
+
+    IP = "192.168.149.2"
+    port = 40005
+    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    s.bind((IP,port))
+    s.listen(1)
+    print('listen at port :',port)
+    conn,addr = s.accept()
+    print('connected by',addr)
+
+
+    Kp_xy = 0.5
+    land_vel = 0.3
+
+    arm_x_bias = -0.0434
+    arm_y_bias = 0.2155
+    arm_z_bias = 0.02
+    
+    while not rospy.is_shutdown():
+        try:
+            tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_0', rospy.Time(0))
+        except:
+            try:
+                tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_1', rospy.Time(0))
+            except:
+                try:
+                    tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_2', rospy.Time(0))
+                except:
+                    try:
+                        tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_3', rospy.Time(0))
+                    except:
+                        try:
+                            tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_4', rospy.Time(0))
+                        except:
+                            try:
+                                tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_5', rospy.Time(0))
+                            except:
+                                 rate.sleep()
+                                 continue
+        # print(z_cnt)
+        if z_cnt < 20:
+            cmd_vel_enu.linear.x = Kp_xy * (tfstamped.transform.translation.y)
+            cmd_vel_enu.linear.y = Kp_xy* (tfstamped.transform.translation.x)
+            cmd_vel_enu.linear.z  =  - land_vel
+            cmd_vel_pub.publish(cmd_vel_enu)
+        else:
+            data = conn.recv(1024)
+            data = data.decode()
+            if not data:
+                continue
+            target_x = (tfstamped.transform.translation.x + arm_x_bias) * 1e4 
+            target_y = (tfstamped.transform.translation.y + arm_y_bias) * 1e4 
+            target_z = (tfstamped.transform.translation.z + arm_z_bias) * 1e4
+            if data == 'Ready':
+                print('recieved message:',data)
+                send =str(target_x) +',' + str(target_y) + ',' + str(target_z)
+                conn.sendall(send.encode())
+                time.sleep(1)
+            data = 'Not ready'
+            rate.sleep()
+
+    conn.close()
+    s.close()
--- a/sitl_config/worlds/robocup.world
+++ b/sitl_config/worlds/robocup.world
@ -4037,10 +4037,6 @@
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
-<<<<<<< HEAD
-=======
-
->>>>>>> f5abb49c166678aab83abf9f9a2d0649e4f4eead
      <model name='sidewalk_10'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <scale>1 1 1</scale>