forked from xtdrone/XTDrone
add contributors
This commit is contained in:
parent
ba08835670
commit
972d06dc31
|
@ -95,13 +95,13 @@ USV
|
||||||
|
|
||||||
- Founders: Kun Xiao, Shaochang Tan
|
- Founders: Kun Xiao, Shaochang Tan
|
||||||
- Adviser: Xiangke Wang
|
- 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
|
### Contributers
|
||||||
|
|
||||||
Sincerely thank you for your contribution to XTDrone.
|
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
|
### Business cooperation
|
||||||
|
|
||||||
|
|
|
@ -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的贡献
|
非常感谢你们为XTDrone的贡献
|
||||||
|
|
||||||
孙长浩,聂莹,孔凡杰,李超然,李旭东,张华卿,林梓涵,何瑶
|
陈科研,许江伟,卢永光,陈皋,孙长浩,聂莹,孔凡杰,李超然,李旭东,张华卿,林梓涵,何瑶
|
||||||
|
|
||||||
### 中国机器人大赛无人机挑战赛仿真组
|
### 中国机器人大赛无人机挑战赛仿真组
|
||||||
|
|
||||||
|
|
|
@ -1,4 +1,5 @@
|
||||||
import rospy
|
import rospy
|
||||||
|
import math
|
||||||
from mavros_msgs.msg import State, PositionTarget
|
from mavros_msgs.msg import State, PositionTarget
|
||||||
from mavros_msgs.srv import CommandBool, SetMode
|
from mavros_msgs.srv import CommandBool, SetMode
|
||||||
from geometry_msgs.msg import PoseStamped, Pose, Twist
|
from geometry_msgs.msg import PoseStamped, Pose, Twist
|
||||||
|
@ -60,9 +61,9 @@ class Communication:
|
||||||
while not rospy.is_shutdown():
|
while not rospy.is_shutdown():
|
||||||
self.target_motion_pub.publish(self.target_motion)
|
self.target_motion_pub.publish(self.target_motion)
|
||||||
|
|
||||||
if (self.flight_mode is "LAND") and (self.current_position.z < 0.15):
|
# if (self.flight_mode is "LAND") and (self.current_position.z < 0.15):
|
||||||
if(self.disarm()):
|
# if(self.disarm()):
|
||||||
self.flight_mode = "DISARMED"
|
# self.arm_state = "DISARMED"
|
||||||
|
|
||||||
try:
|
try:
|
||||||
response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')
|
response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')
|
||||||
|
@ -109,7 +110,7 @@ class Communication:
|
||||||
if(self.motion_type == 1):
|
if(self.motion_type == 1):
|
||||||
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
|
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_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
|
||||||
+ PositionTarget.IGNORE_YAW
|
+ PositionTarget.IGNORE_YAW_RATE
|
||||||
if(self.motion_type == 2):
|
if(self.motion_type == 2):
|
||||||
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
|
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
|
||||||
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
|
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
|
||||||
|
@ -134,7 +135,7 @@ class Communication:
|
||||||
if self.hover_flag == 0:
|
if self.hover_flag == 0:
|
||||||
self.coordinate_frame = 8
|
self.coordinate_frame = 8
|
||||||
self.motion_type = 1
|
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):
|
def cmd_vel_enu_callback(self, msg):
|
||||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||||
|
|
|
@ -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()
|
||||||
|
|
|
@ -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
|
|
@ -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()
|
|
@ -4037,10 +4037,6 @@
|
||||||
<wrench>0 0 0 0 -0 0</wrench>
|
<wrench>0 0 0 0 -0 0</wrench>
|
||||||
</link>
|
</link>
|
||||||
</model>
|
</model>
|
||||||
<<<<<<< HEAD
|
|
||||||
=======
|
|
||||||
|
|
||||||
>>>>>>> f5abb49c166678aab83abf9f9a2d0649e4f4eead
|
|
||||||
<model name='sidewalk_10'>
|
<model name='sidewalk_10'>
|
||||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
|
|
Loading…
Reference in New Issue