jasder
/
XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add ugv

This commit is contained in:
Your Name 2020-12-24 22:14:01 +08:00
parent 0b9313b385
commit 124510ad2a
321 changed files with 482 additions and 74996 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
communication/multi_vehicle_communication.sh
View File

@ -1,7 +1,7 @@
#!/bin/bash
iris_num=0
typhoon_h480_num=2
solo_num=2
iris_num=9
typhoon_h480_num=0
solo_num=0
plane_num=0
rover_num=0
standard_vtol_num=0

5
control/gcs/receive.py
View File

@ -57,8 +57,9 @@ class Ros2Gui(QThread):
+'\n'+str(self.multirotor_type[id])+' vel:\n'+'uav'+str(id)+':\n'+'x:'+"%s"%(self.local_vel[id].twist.linear.x)+' '+'y:'\
+"%s"%(self.local_vel[id].twist.linear.y)+' '+'z:'+"%s"%(self.local_vel[id].twist.linear.z)+'\n'
self.text_all = self.text_all + self.text[id]
self.plot_all[id][0].append(self.local_pose[id].pose.position.x*self.time_map_x)
self.plot_all[id][1].append(self.local_pose[id].pose.position.y*self.time_map_y)
if self.n > 0:
self.plot_all[id][0].append(self.local_pose[id].pose.position.x*self.time_map_x)
self.plot_all[id][1].append(self.local_pose[id].pose.position.y*self.time_map_y)
self.plot_array.emit(self.plot_all)
self.update_text.emit(self.text_all)

82
control/gcs/return.py
View File

@ -25,20 +25,24 @@ class ReturnHome:
self.Kp = 0.5
self.Kpy = 1
self.Kpvel = 1
self.z = 8.0 # height
self.z = 12.0 # height
self.velxy_max = 4
self.velz_max = 3
self.angz_max = 3
self.bias = [[-10, 32.7],[100,33.5],[75,30],[60,-3],[-30,-40.5],[45,-15]]
self.target_position = Twist()
self.target_yaw = 0.0
self.last_yaw = 0.0
self.tracking_id = tracking()
self.arrive_count = 0
self.safe_dis = 0.3
self.safe_height = 0.5
self.cmd = ''
self.last_ugv0_pose = Point()
self.current_ugv0_pose = Point()
self.last_ugv1_pose = Point()
self.current_ugv1_pose = Point()
self.situation_flag = 0
self.change_task_flag = False
#variables of rostopic
rospy.init_node('uav'+str(self.id))
self.local_pose_sub = rospy.Subscriber(self.uav_type + '_' + str(self.id) + "/mavros/local_position/pose",
@ -50,8 +54,8 @@ class ReturnHome:
def local_pose_callback(self, msg):
self.local_pose = msg
self.uav_current_pose = self.local_pose.pose.position
self.uav_current_pose.x = self.uav_current_pose.x + self.bais[self.id][0]
self.uav_current_pose.y = self.uav_current_pose.y + self.bais[self.id][1]
self.uav_current_pose.x = self.uav_current_pose.x+self.bias[self.id][0]
self.uav_current_pose.y = self.uav_current_pose.y+self.bias[self.id][1]
self.uav_current_pose.z = self.uav_current_pose.z
# change Quaternion to TF:
x = self.local_pose.pose.orientation.x
@ -72,8 +76,8 @@ class ReturnHome:
def following_local_pose_callback(self, msg, id):
self.following_local_pose[id] = msg
self.following_local_pose[id].pose.position.x = self.following_local_pose[id].pose.position.x + self.bais[id][0]
self.following_local_pose[id].pose.position.y = self.following_local_pose[id].pose.position.y + self.bais[id][1]
self.following_local_pose[id].pose.position.x = self.following_local_pose[id].pose.position.x+self.bias[id][0]
self.following_local_pose[id].pose.position.y = self.following_local_pose[id].pose.position.y+self.bias[id][1]
self.following_local_pose[id].pose.position.z = self.following_local_pose[id].pose.position.z
def loop(self):
@ -82,7 +86,6 @@ class ReturnHome:
for i in range(self.uav_num):
if not i == self.id:
self.following_local_pose_sub[i] = rospy.Subscriber(self.uav_type + '_' + str(i) + "/mavros/local_position/pose", PoseStamped, self.following_local_pose_callback, i)
self.auction_sub[i] = rospy.Subscriber(self.uav_type+'_'+str(i)+'/auction',auction,self.auction_callback,i)
while not rospy.is_shutdown():
self.count += 1
self.velxy_max = 4.0
@ -120,11 +123,12 @@ class ReturnHome:
self.change_task_flag = False
#if not self.avoid_start_flag:
self.return_home()
print 'flag222222'
distance_tar_cur = self.VectNorm3(self.target_position.linear, self.uav_current_pose)
if distance_tar_cur < 1:
if distance_tar_cur < 0.5:
self.arrive_count += 1
if self.arrive_count > 3:
if self.arrive_count > 5:
self.arrive_point = True
self.arrive_count = 0
else:
@ -139,7 +143,6 @@ class ReturnHome:
self.situation_flag = 1
self.arrive_point = False
elif (self.situation_flag == 1) and self.arrive_point:
#self.avoid_start_flag = False
self.situation_flag = 2
# self.start_yolo_pub.publish('gogogo')
self.arrive_point = False
@ -147,11 +150,14 @@ class ReturnHome:
if self.situation_flag == 2:
self.velz_max = 1
if self.uav_current_pose.z < 2.0:
if self.uav_current_pose.z < 2.2:
self.target_position.linear.x = self.uav_current_pose.x
self.target_position.linear.y = self.uav_current_pose.y
if self.uav_current_pose.z < 1.9:
self.cmd = 'DISARM'
self.get_control_vel()
self.obstacle_avoid()
# self.obstacle_avoid()
self.vel_enu_pub.publish(self.uav_vel)
self.cmd_pub.publish(self.cmd)
@ -172,13 +178,9 @@ class ReturnHome:
self.uav_vel.linear.x = 0.0
self.uav_vel.linear.y = 0.0
if self.uav_vel.linear.z > self.velz_max:
self.uav_vel.linear.z = self.velz_max
elif self.uav_vel.linear.z < - self.velz_max:
self.uav_vel.linear.z = - self.velz_max
'''
turnover_flag = False
self.target_yaw = self.pos2ang(self.target_position.linear.x, self.target_position.linear.y, self.uav_current_pose.x, self.uav_current_pose.y)
mid_yaw = self.target_yaw - self.uav_current_yaw
if mid_yaw > math.pi:
@ -194,35 +196,39 @@ class ReturnHome:
self.uav_vel.linear.y = uav_vel_total * math.sin(mid_yaw)
self.uav_vel.linear.z = self.Kp * (self.target_position.linear.z - self.uav_current_pose.z)
if self.uav_vel.linear.z > self.velz_max:
self.uav_vel.linear.z = self.velz_max
elif self.uav_vel.linear.z < - self.velz_max:
self.uav_vel.linear.z = - self.velz_max
def init_point(self):
if self.id == 0: # middle circle 3
self.target_position.linear.x = 0.0 # ugv0 -0.3
self.target_position.linear.y = 0.0
self.target_position.linear.x = self.current_ugv0_pose.x # ugv0 -0.3
self.target_position.linear.y = self.current_ugv0_pose.y-0.2
elif self.id == 5: # middle circle 2
self.target_position.linear.x = 0.0 # ugv0 0.5
self.target_position.linear.y = 3.0
elif self.id == 1: # middle circle 2
self.target_position.linear.x = self.current_ugv0_pose.x # ugv0 0.5
self.target_position.linear.y = self.current_ugv0_pose.y+0.6
elif self.id == 2: # outer loop 0
self.target_position.linear.x = 0.0 #ugv0 1.3
self.target_position.linear.y = 6.0
self.target_position.linear.x = self.current_ugv0_pose.x #ugv0 1.3
self.target_position.linear.y = self.current_ugv0_pose.y+1.3
elif self.id == 1: # outer loop 4
self.target_position.linear.x = 3.0 #ugv1 -0.3
self.target_position.linear.y = 0.0
elif self.id == 3: # outer loop 4
self.target_position.linear.x = self.current_ugv1_pose.x #ugv1 -0.3
self.target_position.linear.y = self.current_ugv1_pose.y-0.2
elif self.id == 4: # outer loop 1
self.target_position.linear.x = 3.0
self.target_position.linear.y = 3.0
self.target_position.linear.x = self.current_ugv1_pose.x
self.target_position.linear.y = self.current_ugv1_pose.y+0.6
elif self.id == 3: # outer loop 5
self.target_position.linear.x = 3.0
self.target_position.linear.y = 6.0
elif self.id == 5: # outer loop 5
self.target_position.linear.x = self.current_ugv1_pose.x
self.target_position.linear.y = self.current_ugv1_pose.y+1.3
def init_point(self):
self.target_position.linear.z = 1.5
def return_home(self):
self.target_position.linear.z = 0.0
# ji jian bi zhang
def obstacle_avoid(self):
@ -241,11 +247,11 @@ class ReturnHome:
if self.following_local_pose[self.avo_id[j]].pose.position.z > self.uav_current_pose.z:
heigher_num = heigher_num + 1
if heigher_num == 0:
self.target_position.linear.z = self.z + self.safe_height
self.target_position.linear.z = self.target_position.linear.z + self.safe_height
else:
self.target_position.linear.z = self.z - self.safe_height * heigher_num
self.target_position.linear.z = self.target_position.linear.z - self.safe_height * heigher_num
else:
self.target_position.linear.z = self.z
self.target_position.linear.z = self.target_position.linear.z
def pos2ang(self, xa, ya, xb, yb): #([xb,yb] to [xa, ya])
if not xa-xb == 0:

10
control/gcs/return.sh Normal file
View File

@ -0,0 +1,10 @@
#!/bin/bash
python return.py 1 &
python return.py 2 &
python return.py 3 &
python return.py 4 &
python return.py 5 &
python return.py 0

49
control/gcs/send.py
View File

@ -77,13 +77,13 @@ class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow):
for k in range(self.multirotor_num[i]):
if i == 7:
self.multi_cmd_vel_flu_pub[counnnt] = rospy.Publisher('/ugv_' + str(k) + '/cmd_vel', Twist, queue_size=10)
self.multi_cmd_pub[counnnt] = rospy.Publisher('/ugv_' + str(k) + + '/cmd', String,queue_size=10)
self.multi_cmd_pub[counnnt] = rospy.Publisher('/ugv_' + str(k) + '/cmd', String,queue_size=10)
else:
self.multi_cmd_vel_flu_pub[counnnt] = rospy.Publisher('/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd_vel_flu', Twist, queue_size=10)
self.multi_cmd_pub[counnnt] = rospy.Publisher('/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd', String,queue_size=10)
counnnt += 1
self.leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
self.leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
self.leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
self.leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
else:
self.multi_cmd_accel_flu_pub = [None] * self.multi_num
@ -176,21 +176,29 @@ class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow):
if multirotor_get_control[i]:
self.twist[i].angular.z = orientation
last_orientation[i] = self.twist[i].angular.z
if self.q_ctrl_leader.empty():
self.ctrl_leader = last_ctrl_leader
else:
self.ctrl_leader = self.q_ctrl_leader.get()
last_ctrl_leader = self.ctrl_leader
if self.q_cmd.empty():
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.cmd[i] = ''
else:
cmd = self.q_cmd.get()
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.cmd[i] = cmd
print(self.cmd[i])
if self.q_ctrl_leader.empty():
self.ctrl_leader = last_ctrl_leader
else:
self.ctrl_leader = self.q_ctrl_leader.get()
last_ctrl_leader = self.ctrl_leader
if self.ctrl_leader:
for i in range(self.multi_num):
if i == 1:
self.cmd[i] = cmd
else:
self.cmd[i] = ''
else:
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.cmd[i] = cmd
print(self.cmd[i])
if self.q_cmd_vel_mask.empty():
self.cmd_vel_mask = last_cmd_vel_mask
@ -204,15 +212,15 @@ class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow):
if check_stop_flag:
for i in range(self.multi_num):
self.cmd[i] = 'AUTO.RTL'
if self.ctrl_leader:
for i in range(self.multi_num):
if multirotor_get_control[i]:
if self.control_type == 'vel':
self.leader_cmd_vel_flu_pub.publish(self.twist[i])
else:
self.leader_cmd_accel_flu_pub.publish(self.twist[i])
self.leader_cmd_pub.publish(self.cmd[i])
break
if self.control_type == 'vel':
self.leader_cmd_vel_flu_pub.publish(self.twist[1])
else:
self.leader_cmd_accel_flu_pub.publish(self.twist[1])
self.leader_cmd_pub.publish(self.cmd[1])
print self.cmd[1]
else:
for i in range(self.multi_num):
if not self.cmd_vel_mask:
@ -221,6 +229,7 @@ class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow):
else:
self.multi_cmd_accel_flu_pub[i].publish(self.twist[i])
self.multi_cmd_pub[i].publish(self.cmd[i])
# print self.cmd[0]
else:
print 'shut down!'
rate.sleep()

6
control/gcs/stop_return.sh Normal file
View File

@ -0,0 +1,6 @@
kill -9 $(ps -ef|grep xtd_ui.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep send.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep receive.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep main.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep plotcanvas.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

12
control/gcs/xtd_ui.py
View File

@ -17,7 +17,7 @@ from plotcanvas import PlotCanvas
class Ui_MainWindow(object):
def __init__(self):
self.multirotor_type = ['iris','typhoon_h480','solo','tailsitter','quadplane','plane','tiltrotor','rotor']
self.multirotor_type = ['iris','typhoon_h480','solo','tailsitter','quadplane','plane','tiltrotor','ugv']
self.multirotor_num = [1, 0, 0, 0, 0, 0, 0, 0]
self.multi_type_num = 8
self.multirotor_select=[]
@ -792,7 +792,7 @@ class Ui_MainWindow(object):
self.checkBox_quadplane.setText(_translate("MainWindow", "quadplane"))
self.checkBox_plane.setText(_translate("MainWindow", "plane"))
self.checkBox_tiltrotor.setText(_translate("MainWindow", "tiltrotor"))
self.checkBox_rotor.setText(_translate("MainWindow", "rotor"))
self.checkBox_rotor.setText(_translate("MainWindow", "ugv"))
self.label_control_board_4.setText(_translate("MainWindow", "vehicle states"))
self.more_topics.setText(_translate("MainWindow", "more topics"))
self.label_website.setToolTip(_translate("MainWindow", "XTDrone wiki"))
@ -825,7 +825,7 @@ class Ui_MainWindow(object):
self.box_plane_num.setToolTip(_translate("MainWindow", "maximum:+-10.00"))
self.label_tiltrotor.setText(_translate("MainWindow", "tiltrotor:"))
self.box_tiltrotor_num.setToolTip(_translate("MainWindow", "maximum:+-10.00"))
self.label_rotor.setText(_translate("MainWindow", "rotor:"))
self.label_rotor.setText(_translate("MainWindow", "ugv:"))
self.box_rotor_num.setToolTip(_translate("MainWindow", "maximum:+-10.00"))
self.button_world.setText(_translate("MainWindow", "world"))
self.button_waypoint.setText(_translate("MainWindow", "way point"))
@ -854,6 +854,7 @@ class Ui_MainWindow(object):
self.box_orientation.setReadOnly(True)
self.box_command.currentIndexChanged.connect(self.get_command)
self.box_formation.currentIndexChanged.connect(self.get_formation)
self.box_formation.setVisible(False)
self.comboBox_maps.currentIndexChanged.connect(self.initplot)
self.checkBox_iris.setCheckable(False)
self.checkBox_iris.stateChanged.connect(self.get_uav_control)
@ -930,8 +931,9 @@ class Ui_MainWindow(object):
if self.multirotor_num[i] > 0.5:
self.multirotor_select.append(i)
self.checkBox_rotor.setCheckable(True)
if self.multirotor_num[i] != 6 and self.multirotor_num[i] != 9 and self.multirotor_num[i] != 18:
self.box_formation.setVisible(False)
if self.multirotor_num[i] != 6 or self.multirotor_num[i] != 9 or self.multirotor_num[i] != 18:
self.box_formation.setVisible(True)
self.control_type = str(self.comboBox_controltype.currentText())
self.button_control.setEnabled(True)

12
control/keyboard/rover_keyboard_control.py → control/keyboard/rover_keyboard_controlller.py Executable file → Normal file
View File

@ -86,10 +86,10 @@ if __name__=="__main__":
multi_cmd_vel_flu_pub = [None]*rover_num
multi_cmd_pub = [None]*rover_num
for i in range(rover_num):
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/ugv_'+str(i)+'/cmd_vel', Twist, queue_size=10)
multi_cmd_pub[i] = rospy.Publisher('/ugv_'+str(i)+'/cmd',String,queue_size=10)
leader_cmd_vel_pub = rospy.Publisher("/ugv/leader/cmd_vel", Twist, queue_size=10)
leader_cmd_pub = rospy.Publisher("/ugv/leader_cmd", String, queue_size=10)
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/rover_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/rover_'+str(i)+'/cmd',String,queue_size=10)
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_vel", Twist, queue_size=10)
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
cmd= String()
twist = Twist()
@ -163,7 +163,7 @@ if __name__=="__main__":
elif angle < -MAX_ANGLE:
angle = -MAX_ANGLE
twist.linear.x = forward; twist.angular.z = angle
twist.linear.x = forward; twist.linear.y = angle
for i in range(rover_num):
if ctrl_leader:
@ -177,4 +177,4 @@ if __name__=="__main__":
cmd = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

164
control/keyboard/ugv_keyboard_control.py Executable file
View File

@ -0,0 +1,164 @@
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
import tty, termios
from std_msgs.msg import String
MAX_LIN_VEL = 20
MAX_ANGLE = 1
LIN_VEL_STEP_SIZE = 0.1
ANGLE_STEP_SIZE = 0.03
cmd_vel_mask = False
ctrl_leader = False
msg2all = """
Control Your XTDrone!
To all drones (press g to control the leader)
---------------------------
1 2 3 4 5 6 7 8 9 0
w r t y i
a s d g j k l
x v b n ,
w/x : increase/decrease forward velocity
a/d : increase/decrease steering angle
i/, : no use
j/l : no use
r : no use
t/y : no use
v/n : no use
b : no use
s/k : stop
0~9 : extendable mission(eg.different formation configuration)
this will mask the keyboard control
g : control the leader
CTRL-C to quit
"""
msg2leader = """
Control Your XTDrone!
To the leader (press g to control all drones)
---------------------------
1 2 3 4 5 6 7 8 9 0
w r t y i
a s d g j k l
x v b n ,
w/x : increase/decrease forward velocity
a/d : increase/decrease steering angle
i/, : no use
j/l : no use
r : no use
t/y : no use
v/n : no use
b : no use
s/k : stop
0~9 : extendable mission(eg.different formation configuration)
this will mask the keyboard control
g : control all drones
CTRL-C to quit
"""
def getKey():
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def print_msg():
if ctrl_leader:
print(msg2leader)
else:
print(msg2all)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
ugv_num = int(sys.argv[1])
rospy.init_node('ugv_keyboard_multi_control')
multi_cmd_vel_flu_pub = [None]*ugv_num
multi_cmd_pub = [None]*ugv_num
for i in range(ugv_num):
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/ugv_'+str(i)+'/cmd_vel', Twist, queue_size=10)
multi_cmd_pub[i] = rospy.Publisher('/ugv_'+str(i)+'/cmd',String,queue_size=10)
leader_cmd_vel_pub = rospy.Publisher("/ugv/leader/cmd_vel", Twist, queue_size=10)
leader_cmd_pub = rospy.Publisher("/ugv/leader_cmd", String, queue_size=10)
cmd= String()
twist = Twist()
forward = 0.0
angle = 0.0
print_msg()
while(1):
key = getKey()
if key == 'w' :
forward = forward + LIN_VEL_STEP_SIZE
print_msg()
print("currently:\t forward vel %.2f\t steering angle %.2f " % (forward, angle))
elif key == 'x' :
forward = forward - LIN_VEL_STEP_SIZE
print_msg()
print("currently:\t forward vel %.2f\t steering angle %.2f " % (forward, angle))
elif key == 'a' :
angle = angle + ANGLE_STEP_SIZE
print_msg()
print("currently:\t forward vel %.2f\t steering angle %.2f " % (forward, angle))
elif key == 'd' :
angle = angle - ANGLE_STEP_SIZE
print_msg()
print("currently:\t forward vel %.2f\t steering angle %.2f " % (forward, angle))
elif key == 'g':
ctrl_leader = not ctrl_leader
print_msg()
elif key == 's' :
cmd_vel_mask = False
forward = 0.0
angle = 0.0
print_msg()
print("currently:\t forward vel %.2f\t steering angle %.2f " % (forward, angle))
else:
for i in range(10):
if key == str(i):
cmd = 'mission'+key
print_msg()
print(cmd)
cmd_vel_mask = True
if (key == '\x03'):
break
if forward > MAX_LIN_VEL:
forward = MAX_LIN_VEL
elif forward < -MAX_LIN_VEL:
forward = -MAX_LIN_VEL
if angle > MAX_ANGLE:
angle = MAX_ANGLE
elif angle < -MAX_ANGLE:
angle = -MAX_ANGLE
twist.linear.x = forward; twist.angular.z = angle
for i in range(ugv_num):
if ctrl_leader:
leader_cmd_vel_pub.publish(twist)
leader_cmd_pub.publish(cmd)
else:
if not cmd_vel_mask:
multi_cmd_vel_flu_pub[i].publish(twist)
multi_cmd_pub[i].publish(cmd)
cmd = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

5
control/rover_self_driving.py
View File

@ -27,7 +27,4 @@ if __name__ == "__main__":
rate = rospy.Rate(50)
while not rospy.is_shutdown():
cmd_vel_flu_pub.publish(twist)
rate.sleep()
rate.sleep()

35
control/ugv_self_driving.py Normal file
View File

@ -0,0 +1,35 @@
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int16
import sys
Kp = 0.002
Vx = 5
def lane_mid_error_callback(msg):
global twist
if msg.data == 1000:
twist.linear.x = 0.0
twist.angular.z = 0.0
else:
if abs(msg.data) > 20:
twist.angular.z = - Kp * msg.data
else:
twist.angular.z = 0.0
twist.linear.x = Vx * (1 - abs(twist.angular.z))
if __name__ == "__main__":
ugv_num = sys.argv[1]
rospy.init_node('ugv_self_driving_'+ugv_num)
cmd_vel_flu_pub = rospy.Publisher('/ugv_'+ugv_num+'/cmd_vel', Twist, queue_size=2)
lane_mid_error_sub = rospy.Subscriber("/ugv_"+ugv_num+"/lane_mid_error",Int16,callback=lane_mid_error_callback)
twist = Twist()
rate = rospy.Rate(50)
while not rospy.is_shutdown():
cmd_vel_flu_pub.publish(twist)
rate.sleep()

0
sensing/lane_detection/lane_detection.py → sensing/lane_detection/lane_detection_rover.py
View File

128
sensing/lane_detection/lane_detection_ugv.py Normal file
View File

@ -0,0 +1,128 @@
import numpy as np
import cv2
import math
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Int16
import matplotlib.pyplot as plt
from cv_bridge import CvBridge
import sys
bridge = CvBridge()
def region_of_interest(img, vertices):
mask = np.zeros_like(img)
match_mask_color = 255
cv2.fillPoly(mask, vertices, match_mask_color)
masked_image = cv2.bitwise_and(img, mask)
return masked_image
def draw_lines(img, lines, color=[255, 0, 0], thickness=3):
line_img = np.zeros(
(
img.shape[0],
img.shape[1],
3
),
dtype=np.uint8
)
img = np.copy(img)
if lines is None:
return
for line in lines:
for x1, y1, x2, y2 in line:
cv2.line(line_img, (x1, y1), (x2, y2), color, thickness)
img = cv2.addWeighted(img, 0.8, line_img, 1.0, 0.0)
return img
def yellow_dectection(image):
# create hsv
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower = np.uint8([ 30, 150, 101])
upper = np.uint8([ 100, 255, 255])
yellow_mask = cv2.inRange(hsv, lower, upper)
result = cv2.bitwise_and(image, image,mask=yellow_mask)
return result
def pipeline(image):
image = yellow_dectection(image)
height = image.shape[0]
width = image.shape[1]
gray_image = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
cannyed_image = cv2.Canny(gray_image, 100, 200)
lines = cv2.HoughLinesP(
cannyed_image,
rho=6,
theta=np.pi / 60,
threshold=200,
lines=np.array([]),
minLineLength=50,
maxLineGap=50
)
if lines is None:
img_ros = bridge.cv2_to_imgmsg(image, "rgb8")
return img_ros, 0
left_line_x = []
left_line_y = []
right_line_x = []
right_line_y = []
for line in lines:
for x1, y1, x2, y2 in line:
slope = float(y2 - y1) / float(x2 - x1)
if slope <= 0:
left_line_x.extend([x1, x2])
left_line_y.extend([y1, y2])
else:
right_line_x.extend([x1, x2])
right_line_y.extend([y1, y2])
min_y = int(image.shape[0]* 1 / 2)
max_y = int(image.shape[0]* 3 / 5)
if not left_line_x == []:
poly_left = np.poly1d(np.polyfit(left_line_y,left_line_x,deg=1))
left_x_start = int(poly_left(max_y))
left_x_end = int(poly_left(min_y))
if not right_line_x == []:
poly_right = np.poly1d(np.polyfit(right_line_y,right_line_x,deg=1))
right_x_start = int(poly_right(max_y))
right_x_end = int(poly_right(min_y))
if not left_line_x == [] and not right_line_x == []:
mid_x_error = (left_x_end+right_x_end)/2.0-width/2
line_image = draw_lines(image,[[[left_x_start, max_y, left_x_end, min_y],[right_x_start, max_y, right_x_end,min_y],]],thickness=5,)
img_ros = bridge.cv2_to_imgmsg(line_image, "rgb8")
else:
mid_x_error = 1000
img_ros = bridge.cv2_to_imgmsg(image, "rgb8")
return img_ros, mid_x_error
def img_callback(msg):
global img_processed, lane_mid_error
cv_image = bridge.imgmsg_to_cv2(msg, "rgb8")
img_processed, lane_mid_error = pipeline(cv_image)
img_processed.header.stamp = rospy.Time.now()
img_processed_pub.publish(img_processed)
if __name__ == "__main__":
ugv_num = sys.argv[1]
rospy.init_node("lane_detection_"+ugv_num)
img_sub = rospy.Subscriber("/ugv_"+ugv_num+"/triclops/triclops/left/image", Image, callback=img_callback)
img_processed_pub = rospy.Publisher("/ugv_"+ugv_num+"/image_lane", Image,queue_size=2)
lane_mid_error_pub = rospy.Publisher("/ugv_"+ugv_num+"/lane_mid_error",Int16,queue_size=2)
img_processed = Image()
lane_mid_error = Int16()
rate = rospy.Rate(50)
while not rospy.is_shutdown():
lane_mid_error_pub.publish(lane_mid_error)
rate.sleep()

BIN
sitl_config/car/colorado/Assets/colorado.blend
View File

Binary file not shown.

BIN
sitl_config/car/colorado/Assets/colorado.blend1
View File

Binary file not shown.

359
sitl_config/car/colorado/Assets/colorado_body.dae
View File

File diff suppressed because one or more lines are too long

110
sitl_config/car/colorado/Assets/colorado_left_suspension.dae
View File

File diff suppressed because one or more lines are too long

136
sitl_config/car/colorado/Assets/colorado_left_wheel.dae
View File

File diff suppressed because one or more lines are too long

110
sitl_config/car/colorado/Assets/colorado_right_suspension.dae
View File

File diff suppressed because one or more lines are too long

136
sitl_config/car/colorado/Assets/colorado_wheel.dae
View File

File diff suppressed because one or more lines are too long

44
sitl_config/car/colorado/CMakeLists.txt
View File

@ -1,44 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(colorado)
SET(CMAKE_CXX_FLAGS "-std=c++0x")
find_package(catkin REQUIRED COMPONENTS
gazebo_plugins
gazebo_ros
roscpp
std_msgs
geometry_msgs
dynamic_reconfigure
)
find_package(Boost REQUIRED COMPONENTS
thread
system
)
find_package(gazebo REQUIRED)
generate_dynamic_reconfigure_options(cfg/colorado.cfg)
link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
catkin_package(
CATKIN_DEPENDS
geometry_msgs
roscpp
std_msgs
#dynamic_reconfigure
)
include_directories( ${catkin_INCLUDE_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_library(colorado_driving_plugin SHARED src/colorado_driving_plugin.cc)
target_link_libraries(colorado_driving_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
add_dependencies(colorado_driving_plugin ${PROJECT_NAME}_gencfg)

44
sitl_config/car/colorado/CMakeLists.txt~
View File

@ -1,44 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(colorado)
SET(CMAKE_CXX_FLAGS "-std=c++0x")
find_package(catkin REQUIRED COMPONENTS
gazebo_plugins
gazebo_ros
roscpp
std_msgs
geometry_msgs
dynamic_reconfigure
)
find_package(Boost REQUIRED COMPONENTS
thread
system
)
find_package(gazebo REQUIRED)
generate_dynamic_reconfigure_options(cfg/colorado_model.cfg)
link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
catkin_package(
CATKIN_DEPENDS
geometry_msgs
roscpp
std_msgs
#dynamic_reconfigure
)
include_directories( ${catkin_INCLUDE_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_library(colorado_driving_plugin SHARED src/colorado_driving_plugin.cc)
target_link_libraries(colorado_driving_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
add_dependencies(colorado_driving_plugin ${PROJECT_NAME}_gencfg)

47
sitl_config/car/colorado/Scripts/CMD_VEL_Direct_Control.py
View File

@ -1,47 +0,0 @@
#!/usr/bin/env python
import roslib
import rospy
# import socket
# UDP_IP = "127.0.0.1"
# UDP_PORT = 5005
# MESSAGE = ""
# print "UDP target IP:", UDP_IP
# print "UDP target port:", UDP_PORT
# print "message:", MESSAGE
# sock = socket.socket(socket.AF_INET, # Internet
# socket.SOCK_DGRAM) # UDP
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64
pubt = rospy.Publisher('/colorado/Driving/Throttle', Float64, queue_size=10)
pubs = rospy.Publisher('/colorado/Driving/Steering', Float64, queue_size=10)
Throttle=0
Steer=0
rospy.init_node('cmd_vel_listener')
rate = rospy.Rate(30)
def callback(msg):
Throttle = msg.linear.x
Steer = msg.angular.z
# rospy.loginfo(Throttle)
pubt.publish(Float64(Throttle))
pubs.publish(Float64(-Steer))
# MESSAGE ="%f %f" % (Throttle, -Steer)
# sock.sendto(MESSAGE, (UDP_IP, UDP_PORT))
def listener():
rospy.Subscriber("/xtdrone/rover_0/cmd_vel_flu", Twist, callback)
rate.sleep()
rospy.spin()
if __name__ == '__main__':
try:
listener()
except rospy.ROSInterruptException:
pass

45
sitl_config/car/colorado/Scripts/transform_broadcaster.py
View File

@ -1,45 +0,0 @@
#!/usr/bin/env python
import roslib
import rospy
import tf
from gazebo_msgs.msg import LinkStates
def command_callback(msg):
names = msg.name
if 'colorado::body' in names:
index = names.index('colorado::body')
pos_x = msg.pose[index].position.x
pos_y = msg.pose[index].position.y
pos_z = msg.pose[index].position.z
ori_x = msg.pose[index].orientation.x
ori_y = msg.pose[index].orientation.y
ori_z = msg.pose[index].orientation.z
ori_w = msg.pose[index].orientation.w
br = tf.TransformBroadcaster()
br.sendTransform( (pos_x, pos_y, pos_z),
(ori_x, ori_y ,ori_z, ori_w),
rospy.Time.now(),
"body",
"world_gazebo")
else:
print 'colorado::body dosn\'t exist in the Gazebo'
rospy.init_node('world_to_body_TF_pub')
def main():
rate = rospy.Rate(100)
while not rospy.is_shutdown():
msg = rospy.wait_for_message("/gazebo/link_states", LinkStates)
command_callback(msg)
rate.sleep()
main()

21
sitl_config/car/colorado/cfg/colorado.cfg
View File

@ -1,21 +0,0 @@
#!/usr/bin/env python
PACKAGE = "colorado"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("Steer_control_P", double_t, 0, "Porptional", 50000, 0, 50000)
gen.add("Steer_control_I", double_t, 0, "Integral", 100, 0, 1000)
gen.add("Steer_control_D", double_t, 0, "Differential", 800, 0, 10000)
#Engine power, Velocity damping and steering speed:
gen.add("Power", double_t, 0, "Power multiplier", 2, 0, 10000)
gen.add("Damping", double_t, 0, "Velocity Damping", 12, 0, 1000)
gen.add("Friction", double_t, 0, "Wheel Dynamic Friction", 100, 0, 1000)
gen.add("Steering", double_t, 0, "Streeing speed", 2, 0, 1000)
#Suspension system values:
gen.add("Spring", double_t, 0, "Suspension Spring", 28000, 0, 100000)
gen.add("Damper", double_t, 0, "Suspesion Damping", 4000, -5000, 5000)
gen.add("Target", double_t, 0, "Suspesion Target", 0.11, -2, 2)
exit(gen.generate(PACKAGE, "colorado_reconfig_node", "colorado"))

20
sitl_config/car/colorado/cfg/colorado.cfg~
View File

@ -1,20 +0,0 @@
#!/usr/bin/env python
PACKAGE = "colorado"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("Steer_control_P", double_t, 0, "Porptional", 10000, 0, 50000)
gen.add("Steer_control_I", double_t, 0, "Porptional", 0.0, 0, 1000)
gen.add("Steer_control_D", double_t, 0, "Porptional", 1000, 0, 10000)
gen.add("Power", double_t, 0, "Power multiplier", 4, 0, 1000)
gen.add("Damping", double_t, 0, "Velocity Damping", 100, 0, 1000)
gen.add("Steering", double_t, 0, "streeing power", 0.001, 0, 1)
gen.add("Command_Linear_Noise", double_t, 0, "Porptional", 0.0, 0, 1)
gen.add("Command_Angular_Noise", double_t, 0, "Porptional", 0.0, 0, 1)
exit(gen.generate(PACKAGE, "colorado_reconfig_node", "colorado"))

30
sitl_config/car/colorado/launch/colorado_empty_world.launch
View File

@ -1,30 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="physics" default="ode"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="robot_models" default="$(find colorado)/sdf_models"/>
<!-- setting gazebo path for platform and sensors models -->
<env name="GAZEBO_MODEL_PATH" value="$(arg robot_models)" />
<!-- We resume the logic in empty_world.launch -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="physics" value="$(arg physics)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="world_name" value="$(find colorado)/worlds/colorado_empty.world"/>
</include>
</launch>

59
sitl_config/car/colorado/package.xml
View File

@ -1,59 +0,0 @@
<?xml version="1.0"?>
<package>
<name>colorado</name>
<version>0.0.0</version>
<description>The colorado package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dany@todo.todo">dany</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/oshkosh_model</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

5
sitl_config/car/colorado/sdf_models/colorado/BuildSDFModelFromAssets.sh
View File

@ -1,5 +0,0 @@
#This script merges all the sdf_assets into the final model.sdf
rm -f model.sdf
cat sdf_assets/Body.xml sdf_assets/Suspension.xml sdf_assets/Steering.xml sdf_assets/Wheels.xml sdf_assets/Ender.xml > model.sdf
echo model.sdf built!

6
sitl_config/car/colorado/sdf_models/colorado/model.config
View File

@ -1,6 +0,0 @@
<?xml version="1.0"?>
<model>
<name>colorado</name>
<version>1.0</version>
<sdf version="1.4">model.sdf</sdf>
</model>

819
sitl_config/car/colorado/sdf_models/colorado/model.sdf
View File

@ -1,819 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<!-- -*- mode: XML -*- -->
<sdf version='1.4'>
<model name='colorado'>
<link name='body'>
<pose>0 0 0 0 0 0</pose>
<gravity>1</gravity>
<self_collide>1</self_collide>
<inertial>
<pose>0.00 0.00 1 0 0 0</pose>
<mass>2000</mass>
<inertia>
<ixx>500</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>2000</iyy>
<iyz>0.00</iyz>
<izz>2000</izz>
</inertia>
</inertial>
<collision name='body_collision_base_mid'>
<pose>-0.17975 0 1.20715 0 0 0</pose>
<geometry>
<box>
<size>1.934 2.392 1.438</size>
</box>
</geometry>
</collision>
<collision name='body_collision_base_hood'>
<pose>1.65953 0 1.20715 0 0.09472 0</pose>
<geometry>
<box>
<size>1.622 2.414 0.1</size>
</box>
</geometry>
</collision>
<collision name='body_collision_base_back'>
<pose>-2.59851 0 1.08547 0 0 0</pose>
<geometry>
<box>
<size>0.158 2.46 0.591</size>
</box>
</geometry>
</collision>
<collision name='body_collision_base_back_top'>
<pose>-1.93823 0 1.56472 0 -0.33794 0</pose>
<geometry>
<box>
<size>1.384 2.460 0.269</size>
</box>
</geometry>
</collision>
<visual name='body_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://../Assets/colorado_body.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<!--Suspension setup-->
<!--Front left wheel-->
<link name="suspension_main_left_1">
<pose>1.6 0.27504 0.65450 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
</link>
<link name="suspension_holder_left_1">
<pose>1.6 0.3666 0.8654 1.09558 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 -0.25 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<link name="suspension_left_1">
<pose>1.6 0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.1</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name="spring_left_1" type="revolute">
<parent>body</parent>
<child>suspension_main_left_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_joint_left_1" type="revolute">
<parent>body</parent>
<child>suspension_holder_left_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_wheel_joint_left_1" type="revolute">
<parent>suspension_holder_left_1</parent>
<child>suspension_left_1</child>
<pose>0.0 0.0 0.21 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
</axis><!--Front left wheel-->
</joint>
<joint name="suspension_main_wheel_joint_left_1" type="revolute">
<parent>suspension_main_left_1</parent>
<child>suspension_left_1</child>
<pose>0.0 0.0 -0.11 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<!--Back left wheel-->
<link name="suspension_main_left_2">
<pose>-1.6 0.27504 0.65450 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
</link>
<link name="suspension_holder_left_2">
<pose>-1.6 0.3666 0.8654 1.09558 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 -0.25 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<link name="suspension_left_2">
<pose>-1.6 0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.1</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name="spring_left_2" type="revolute">
<parent>body</parent>
<child>suspension_main_left_2</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_joint_left_2" type="revolute">
<parent>body</parent>
<child>suspension_holder_left_2</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_holder_wheel_joint_left_2" type="revolute">
<parent>suspension_holder_left_2</parent>
<child>suspension_left_2</child>
<pose>0.0 0.0 0.21 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
</axis>
</joint>
<joint name="suspension_main_wheel_joint_left_2" type="revolute">
<parent>suspension_main_left_2</parent>
<child>suspension_left_2</child>
<pose>0.0 0.0 -0.11 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<!--Front right wheel-->
<link name="suspension_main_right_1">
<pose>1.6 -0.27504 0.65450 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
</link>
<link name="suspension_holder_right_1">
<pose>1.6 -0.3666 0.8654 -1.09558 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 -0.25 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<link name="suspension_right_1">
<pose>1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.1</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name="spring_right_1" type="revolute">
<parent>body</parent>
<child>suspension_main_right_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>-1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_joint_right_1" type="revolute">
<parent>body</parent>
<child>suspension_holder_right_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_wheel_joint_right_1" type="revolute">
<parent>suspension_holder_right_1</parent>
<child>suspension_right_1</child>
<pose>0.0 0.0 0.21 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
</axis>
</joint>
<joint name="suspension_main_wheel_joint_right_1" type="revolute">
<parent>suspension_main_right_1</parent>
<child>suspension_right_1</child>
<pose>0.0 0.0 -0.11 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<!--Back right wheel-->
<link name="suspension_main_right_2">
<pose>-1.6 -0.27504 0.65450 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
</link>
<link name="suspension_holder_right_2">
<pose>-1.6 -0.3666 0.8654 -1.09558 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 -0.25 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<link name="suspension_right_2">
<pose>-1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.1</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name="spring_right_2" type="revolute">
<parent>body</parent>
<child>suspension_main_right_2</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>-1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_joint_right_2" type="revolute">
<parent>body</parent>
<child>suspension_holder_right_2</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_wheel_joint_right_2" type="revolute">
<parent>suspension_holder_right_2</parent>
<child>suspension_right_2</child>
<pose>0.0 0.0 0.21 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
</axis>
</joint>
<joint name="suspension_main_wheel_joint_right_2" type="revolute">
<parent>suspension_main_right_2</parent>
<child>suspension_right_2</child>
<pose>0.0 0.0 -0.11 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<!--End of suspension setup-->
<!--steering setup-->
<link name="steering_left_1">
<pose>1.6 0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 3.14159265359</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>0.3</length>
</cylinder>
</geometry>
<material><ambient>0.2 0.2 0.2 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<joint name="steering_joint_left_1" type="revolute">
<parent>suspension_left_1</parent>
<child>steering_left_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<dynamics>
<damping>1</damping>
</dynamics>
<limit>
<lower>-0.61</lower>
<upper>0.61</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="steering_right_1">
<pose>1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>0.3</length>
</cylinder>
</geometry>
<material><ambient>0.2 0.2 0.2 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<joint name="steering_joint_right_1" type="revolute">
<parent>suspension_right_1</parent>
<child>steering_right_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<dynamics>
<damping>1</damping>
</dynamics>
<limit>
<lower>-0.61</lower>
<upper>0.61</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<!--End of steering setup-->
<!--wheels Setup-->
<link name='left_wheel_1'>
<pose>1.6 0.9 0.391 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>80</mass>
<inertia>
<ixx>5.9</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.88</iyy>
<iyz>0</iyz>
<izz>5.9</izz>
</inertia>
</inertial>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.1</length>
<radius>0.388</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
<radius>0.387</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.24</length>
<radius>0.385</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.254</length>
<radius>0.38</radius>
</cylinder>
</geometry>
</collision>
<visual name='left_wheel'>
<pose>0 0 0 0 0 3.1469265</pose>
<geometry>
<mesh>
<uri>model://../Assets/colorado_wheel.dae</uri>
</mesh>
</geometry>
</visual>
<gravity>1</gravity>
<self_collide>0</self_collide>
</link>
<joint name='left_wheel_1' type='revolute'>
<child>left_wheel_1</child>
<parent>steering_left_1</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<link name='left_wheel_2'>
<pose>-1.6 0.9 0.391 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>80</mass>
<inertia>
<ixx>5.9</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.88</iyy>
<iyz>0</iyz>
<izz>5.9</izz>
</inertia>
</inertial>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.1</length>
<radius>0.388</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
<radius>0.387</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.24</length>
<radius>0.385</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.254</length>
<radius>0.38</radius>
</cylinder>
</geometry>
</collision>
<visual name='left_wheel'>
<pose>0 0 0 0 0 3.1469265</pose>
<geometry>
<mesh>
<uri>model://../Assets/colorado_wheel.dae</uri>
</mesh>
</geometry>
</visual>
<gravity>1</gravity>
<self_collide>0</self_collide>
</link>
<joint name='left_wheel_2' type='revolute'>
<child>left_wheel_2</child>
<parent>suspension_left_2</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<link name='right_wheel_1'>
<pose>1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>80</mass>
<inertia>
<ixx>5.9</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.88</iyy>
<iyz>0</iyz>
<izz>5.9</izz>
</inertia>
</inertial>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.1</length>
<radius>0.388</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
<radius>0.387</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.24</length>
<radius>0.385</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.254</length>
<radius>0.38</radius>
</cylinder>
</geometry>
</collision>
<visual name='right_wheel'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://../Assets/colorado_wheel.dae</uri>
</mesh>
</geometry>
</visual>
<gravity>1</gravity>
<self_collide>0</self_collide>
</link>
<joint name='right_wheel_1' type='revolute'>
<child>right_wheel_1</child>
<parent>steering_right_1</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<link name='right_wheel_2'>
<pose>-1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>80</mass>
<inertia>
<ixx>5.9</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.88</iyy>
<iyz>0</iyz>
<izz>5.9</izz>
</inertia>
</inertial>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.1</length>
<radius>0.388</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
<radius>0.387</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.24</length>
<radius>0.385</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.254</length>
<radius>0.38</radius>
</cylinder>
</geometry>
</collision>
<visual name='right_wheel'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://../Assets/colorado_wheel.dae</uri>
</mesh>
</geometry>
</visual>
<gravity>1</gravity>
<self_collide>0</self_collide>
</link>
<joint name='right_wheel_2' type='revolute'>
<child>right_wheel_2</child>
<parent>suspension_right_2</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<!--End of wheels setup-->
<!--<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so"><robotNamespace>/Sahar</robotNamespace><jointName>left_wheel_1, </jointName><updateRate>50.0</updateRate><alwaysOn>true</alwaysOn></plugin>-->
</model>
</sdf>

62
sitl_config/car/colorado/sdf_models/colorado/sdf_assets/Body.xml
View File

@ -1,62 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<!-- -*- mode: XML -*- -->
<sdf version='1.4'>
<model name='colorado'>
<link name='body'>
<pose>0 0 0 0 0 0</pose>
<gravity>1</gravity>
<self_collide>1</self_collide>
<inertial>
<pose>0.00 0.00 1 0 0 0</pose>
<mass>2000</mass>
<inertia>
<ixx>500</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>2000</iyy>
<iyz>0.00</iyz>
<izz>2000</izz>
</inertia>
</inertial>
<collision name='body_collision_base_mid'>
<pose>-0.17975 0 1.20715 0 0 0</pose>
<geometry>
<box>
<size>1.934 2.392 1.438</size>
</box>
</geometry>
</collision>
<collision name='body_collision_base_hood'>
<pose>1.65953 0 1.20715 0 0.09472 0</pose>
<geometry>
<box>
<size>1.622 2.414 0.1</size>
</box>
</geometry>
</collision>
<collision name='body_collision_base_back'>
<pose>-2.59851 0 1.08547 0 0 0</pose>
<geometry>
<box>
<size>0.158 2.46 0.591</size>
</box>
</geometry>
</collision>
<collision name='body_collision_base_back_top'>
<pose>-1.93823 0 1.56472 0 -0.33794 0</pose>
<geometry>
<box>
<size>1.384 2.460 0.269</size>
</box>
</geometry>
</collision>
<visual name='body_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://../Assets/colorado_body.dae</uri>
</mesh>
</geometry>
</visual>
</link>

4
sitl_config/car/colorado/sdf_models/colorado/sdf_assets/Ender.xml
View File

@ -1,4 +0,0 @@
<plugin name="colorado_driving_plugin" filename="libcolorado_driving_plugin.so"/>
<!--<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so"><robotNamespace>/Sahar</robotNamespace><jointName>left_wheel_1, </jointName><updateRate>50.0</updateRate><alwaysOn>true</alwaysOn></plugin>-->
</model>
</sdf>

83
sitl_config/car/colorado/sdf_models/colorado/sdf_assets/Steering.xml
View File

@ -1,83 +0,0 @@
<!-------------------------->
<!--steering setup-->
<link name="steering_left_1">
<pose>1.6 0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 3.14159265359</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>0.3</length>
</cylinder>
</geometry>
<material><ambient>0.2 0.2 0.2 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<joint name="steering_joint_left_1" type="revolute">
<parent>suspension_left_1</parent>
<child>steering_left_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<dynamics>
<damping>1</damping>
</dynamics>
<limit>
<lower>-0.61</lower>
<upper>0.61</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="steering_right_1">
<pose>1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>0.3</length>
</cylinder>
</geometry>
<material><ambient>0.2 0.2 0.2 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<joint name="steering_joint_right_1" type="revolute">
<parent>suspension_right_1</parent>
<child>steering_right_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<dynamics>
<damping>1</damping>
</dynamics>
<limit>
<lower>-0.61</lower>
<upper>0.61</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<!--End of steering setup-->
<!-------------------------->

397
sitl_config/car/colorado/sdf_models/colorado/sdf_assets/Suspension.xml
View File

@ -1,397 +0,0 @@
<!-------------------------->
<!--Suspension setup-->
<!--Front left wheel-->
<link name="suspension_main_left_1">
<pose>1.6 0.27504 0.65450 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
</link>
<link name="suspension_holder_left_1">
<pose>1.6 0.3666 0.8654 1.09558 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 -0.25 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<link name="suspension_left_1">
<pose>1.6 0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.1</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name="spring_left_1" type="revolute">
<parent>body</parent>
<child>suspension_main_left_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_joint_left_1" type="revolute">
<parent>body</parent>
<child>suspension_holder_left_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_wheel_joint_left_1" type="revolute">
<parent>suspension_holder_left_1</parent>
<child>suspension_left_1</child>
<pose>0.0 0.0 0.21 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
</axis><!--Front left wheel-->
</joint>
<joint name="suspension_main_wheel_joint_left_1" type="revolute">
<parent>suspension_main_left_1</parent>
<child>suspension_left_1</child>
<pose>0.0 0.0 -0.11 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<!--Back left wheel-->
<link name="suspension_main_left_2">
<pose>-1.6 0.27504 0.65450 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
</link>
<link name="suspension_holder_left_2">
<pose>-1.6 0.3666 0.8654 1.09558 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 -0.25 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<link name="suspension_left_2">
<pose>-1.6 0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.1</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name="spring_left_2" type="revolute">
<parent>body</parent>
<child>suspension_main_left_2</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_joint_left_2" type="revolute">
<parent>body</parent>
<child>suspension_holder_left_2</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_holder_wheel_joint_left_2" type="revolute">
<parent>suspension_holder_left_2</parent>
<child>suspension_left_2</child>
<pose>0.0 0.0 0.21 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
</axis>
</joint>
<joint name="suspension_main_wheel_joint_left_2" type="revolute">
<parent>suspension_main_left_2</parent>
<child>suspension_left_2</child>
<pose>0.0 0.0 -0.11 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<!--Front right wheel-->
<link name="suspension_main_right_1">
<pose>1.6 -0.27504 0.65450 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
</link>
<link name="suspension_holder_right_1">
<pose>1.6 -0.3666 0.8654 -1.09558 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 -0.25 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<link name="suspension_right_1">
<pose>1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.1</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name="spring_right_1" type="revolute">
<parent>body</parent>
<child>suspension_main_right_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>-1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_joint_right_1" type="revolute">
<parent>body</parent>
<child>suspension_holder_right_1</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_wheel_joint_right_1" type="revolute">
<parent>suspension_holder_right_1</parent>
<child>suspension_right_1</child>
<pose>0.0 0.0 0.21 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
</axis>
</joint>
<joint name="suspension_main_wheel_joint_right_1" type="revolute">
<parent>suspension_main_right_1</parent>
<child>suspension_right_1</child>
<pose>0.0 0.0 -0.11 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<!--Back right wheel-->
<link name="suspension_main_right_2">
<pose>-1.6 -0.27504 0.65450 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
</link>
<link name="suspension_holder_right_2">
<pose>-1.6 -0.3666 0.8654 -1.09558 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 -0.25 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material><ambient>0.1 0.1 0.1 1</ambient><diffuse>0.1 0.1 0.1 1</diffuse><specular>1 1 1 1</specular><emissive>0 0 0 1</emissive></material>
</visual>
</link>
<link name="suspension_right_2">
<pose>-1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
<mass>50</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.1</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name="spring_right_2" type="revolute">
<parent>body</parent>
<child>suspension_main_right_2</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>-1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_joint_right_2" type="revolute">
<parent>body</parent>
<child>suspension_holder_right_2</child>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<joint name="suspension_holder_wheel_joint_right_2" type="revolute">
<parent>suspension_holder_right_2</parent>
<child>suspension_right_2</child>
<pose>0.0 0.0 0.21 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
</axis>
</joint>
<joint name="suspension_main_wheel_joint_right_2" type="revolute">
<parent>suspension_main_right_2</parent>
<child>suspension_right_2</child>
<pose>0.0 0.0 -0.11 0.0 0.0 0.0</pose>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<!--End of suspension setup-->

276
sitl_config/car/colorado/sdf_models/colorado/sdf_assets/Wheels.xml
View File

@ -1,276 +0,0 @@
<!-------------------------->
<!--wheels Setup-->
<link name='left_wheel_1'>
<pose>1.6 0.9 0.391 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>80</mass>
<inertia>
<ixx>5.9</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.88</iyy>
<iyz>0</iyz>
<izz>5.9</izz>
</inertia>
</inertial>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.1</length>
<radius>0.388</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
<radius>0.387</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.24</length>
<radius>0.385</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.254</length>
<radius>0.38</radius>
</cylinder>
</geometry>
</collision>
<visual name='left_wheel'>
<pose>0 0 0 0 0 3.1469265</pose>
<geometry>
<mesh>
<uri>model://../Assets/colorado_wheel.dae</uri>
</mesh>
</geometry>
</visual>
<gravity>1</gravity>
<self_collide>0</self_collide>
</link>
<joint name='left_wheel_1' type='revolute'>
<child>left_wheel_1</child>
<parent>steering_left_1</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<link name='left_wheel_2'>
<pose>-1.6 0.9 0.391 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>80</mass>
<inertia>
<ixx>5.9</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.88</iyy>
<iyz>0</iyz>
<izz>5.9</izz>
</inertia>
</inertial>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.1</length>
<radius>0.388</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
<radius>0.387</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.24</length>
<radius>0.385</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.254</length>
<radius>0.38</radius>
</cylinder>
</geometry>
</collision>
<visual name='left_wheel'>
<pose>0 0 0 0 0 3.1469265</pose>
<geometry>
<mesh>
<uri>model://../Assets/colorado_wheel.dae</uri>
</mesh>
</geometry>
</visual>
<gravity>1</gravity>
<self_collide>0</self_collide>
</link>
<joint name='left_wheel_2' type='revolute'>
<child>left_wheel_2</child>
<parent>suspension_left_2</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<link name='right_wheel_1'>
<pose>1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>80</mass>
<inertia>
<ixx>5.9</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.88</iyy>
<iyz>0</iyz>
<izz>5.9</izz>
</inertia>
</inertial>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.1</length>
<radius>0.388</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
<radius>0.387</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.24</length>
<radius>0.385</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.254</length>
<radius>0.38</radius>
</cylinder>
</geometry>
</collision>
<visual name='right_wheel'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://../Assets/colorado_wheel.dae</uri>
</mesh>
</geometry>
</visual>
<gravity>1</gravity>
<self_collide>0</self_collide>
</link>
<joint name='right_wheel_1' type='revolute'>
<child>right_wheel_1</child>
<parent>steering_right_1</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<link name='right_wheel_2'>
<pose>-1.6 -0.9 0.391 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>80</mass>
<inertia>
<ixx>5.9</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.88</iyy>
<iyz>0</iyz>
<izz>5.9</izz>
</inertia>
</inertial>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.1</length>
<radius>0.388</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
<radius>0.387</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.24</length>
<radius>0.385</radius>
</cylinder>
</geometry>
</collision>
<collision name='wheel_col'>
<pose>0 0 0 1.57079633 0 0</pose>
<geometry>
<cylinder>
<length>0.254</length>
<radius>0.38</radius>
</cylinder>
</geometry>
</collision>
<visual name='right_wheel'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://../Assets/colorado_wheel.dae</uri>
</mesh>
</geometry>
</visual>
<gravity>1</gravity>
<self_collide>0</self_collide>
</link>
<joint name='right_wheel_2' type='revolute'>
<child>right_wheel_2</child>
<parent>suspension_right_2</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<!--End of wheels setup-->
<!-------------------------->

456
sitl_config/car/colorado/src/colorado_driving_plugin.cc
View File

@ -1,456 +0,0 @@
// Written By : Yossi Cohen
#define MY_GAZEBO_VER 5
// If the plugin is not defined then define it
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <random>
// Gazebo Libraries
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo_config.h>
// Ignition
#include <ignition/math.hh>
// ROS Communication
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "std_msgs/Bool.h"
// Boost Thread Mutex
#include <boost/thread/mutex.hpp>
// Dynamic Configuration
#include <dynamic_reconfigure/server.h>
#include <colorado/coloradoConfig.h>
#include <boost/bind.hpp> // Boost Bind
#include <tf/transform_broadcaster.h>
// Maximum time delay before a "no command" behaviour is initiated.
#define command_MAX_DELAY 0.3
#define PI 3.14159265359
#define VehicleLength 3.5932
#define VehicleWidth 1.966
#define WheelRadius 0.497
#define HP 190 //190 HP @3400 rpm=142KW @3400 rpm & 515 NM @1300
#define POWER 142
#define TRANSMISSIONS 4
#define IDLE_RPM 550
//#define MY_GAZEBO_VER 5
namespace gazebo
{
class coloradoDrivingPlugin : public ModelPlugin
{
double deltaSimTime = 0.001;
/// Constructor
public:
coloradoDrivingPlugin() {}
/// The load function is called by Gazebo when the plugin is inserted into simulation
/// \param[in] _model A pointer to the model that this plugin is attached to.
/// \param[in] _sdf A pointer to the plugin's SDF element.
public:
void Load(physics::ModelPtr _model, sdf::ElementPtr /*_sdf*/) // we are not using the pointer to the sdf file so its commanted as an option
{
std::cout << "My major GAZEBO VER = [" << GAZEBO_MAJOR_VERSION << "]" << std::endl;
this->model = _model;
// Store the pointers to the joints
this->left_wheel_1 = this->model->GetJoint("left_wheel_1");
this->left_wheel_2 = this->model->GetJoint("left_wheel_2");
this->right_wheel_1 = this->model->GetJoint("right_wheel_1");
this->right_wheel_2 = this->model->GetJoint("right_wheel_2");
this->streer_joint_left_1 = this->model->GetJoint("steering_joint_left_1");
this->streer_joint_right_1 = this->model->GetJoint("steering_joint_right_1");
this->spring_left_1 = this->model->GetJoint("spring_left_1");
this->spring_right_1 = this->model->GetJoint("spring_right_1");
this->spring_left_2 = this->model->GetJoint("spring_left_2");
this->spring_right_2 = this->model->GetJoint("spring_right_2");
// Starting Timers
Throttle_command_timer.Start();
Angular_command_timer.Start();
Breaking_command_timer.Start();
this->Ros_nh = new ros::NodeHandle("coloradoDrivingPlugin_node");
// Subscribe to the topic, and register a callback
Steering_rate_sub = this->Ros_nh->subscribe("colorado/Driving/Steering", 60, &coloradoDrivingPlugin::On_Steering_command, this);
Velocity_rate_sub = this->Ros_nh->subscribe("colorado/Driving/Throttle", 60, &coloradoDrivingPlugin::On_Throttle_command, this);
Breaking_sub = this->Ros_nh->subscribe("colorado/Driving/Break", 60, &coloradoDrivingPlugin::On_Break_command, this);
platform_hb_pub_ = this->Ros_nh->advertise<std_msgs::Bool>("colorado/ConnctionStatus", 60);
platform_Speedometer_pub = this->Ros_nh->advertise<std_msgs::Float64>("colorado/Speedometer", 60);
// Listen to the update event. This event is broadcast every simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&coloradoDrivingPlugin::OnUpdate, this, _1));
std::cout << "Setting up dynamic config" << std::endl;
this->model_reconfiguration_server = new dynamic_reconfigure::Server<colorado::coloradoConfig>(*(this->Ros_nh));
this->model_reconfiguration_server->setCallback(boost::bind(&coloradoDrivingPlugin::dynamic_Reconfiguration_callback, this, _1, _2));
std::cout << "Dynamic configuration is set up" << std::endl;
Steering_Request = 0;
Throttle_command = 0;
std::cout << "Driving Plugin Loaded" << std::endl;
}
public:
void dynamic_Reconfiguration_callback(colorado::coloradoConfig &config, uint32_t level)
{
control_P = config.Steer_control_P;
control_I = config.Steer_control_I;
control_D = config.Steer_control_D;
steeringSpeed = config.Steering;
damping = config.Damping;
friction = config.Friction;
power = config.Power;
suspenSpring = config.Spring;
SuspenDamp = config.Damper;
SuspenTarget = config.Target;
}
// Called by the world update start event, This function is the event that will be called every update
public:
void OnUpdate(const common::UpdateInfo &simInfo) // we are not using the pointer to the info so its commanted as an option
{
deltaSimTime = simInfo.simTime.Double() - simTime.Double();
simTime = simInfo.simTime;
// std::cout << "update function started"<<std::endl;
// std::cout << "command_timer = " << command_timer.GetElapsed().Float() << std::endl;
// Applying effort to the wheels , brakes if no message income
if (Throttle_command_timer.GetElapsed().Float() > command_MAX_DELAY)
{
// Applies 0 throtle
Throttle_command = 0;
}
if (Breaking_command_timer.GetElapsed().Float() > command_MAX_DELAY)
{
// Brakes
BreakPedal = 1;
}
if (Angular_command_timer.GetElapsed().Float() > command_MAX_DELAY)
{
// Restores straight direction.
Steering_Request = 0;
}
// std::cout << "Applying efforts"<<std::endl;
EngineCalculations();
apply_efforts();
apply_steering();
ApplySuspension();
BreakPedal = 0;
breaker();
// std::cout << this->spring_right_1->GetAngle(0).Radian() << std::endl;
SpeedMsg.data = Speed * 3.6;
platform_Speedometer_pub.publish(SpeedMsg);
connection.data = true;
platform_hb_pub_.publish(connection);
tf::Transform transform;
transform.setOrigin( tf::Vector3(model->WorldPose().Pos().X(), model->WorldPose().Pos().Y(), model->WorldPose().Pos().Z()));
transform.setRotation(tf::Quaternion(model->WorldPose().Rot().X(),model->WorldPose().Rot().Y(),model->WorldPose().Rot().Z(),model->WorldPose().Rot().W()));
TF_Broadcast(transform, "WORLD", model->GetName(), simInfo.simTime);
}
void TF_Broadcast(tf::Transform transform, std::string frame_id, std::string child_frame_id, common::Time time)
{
static tf::TransformBroadcaster br;
tf::StampedTransform st(transform, ros::Time::now()/*(time.sec, time.nsec)*/, frame_id, child_frame_id);
br.sendTransform(st);
}
void EngineCalculations()
{
ThrottlePedal = ThrottlePedal + deltaSimTime * 5 * (Throttle_command - ThrottlePedal); //move the gas pedal smoothly
if (Throttle_command < 0 && Speed > 5)
ThrottlePedal = 0;
CurrentRPM = fabs(Speed) * GearRatio[CurrentGear] * 3.1 / (WheelRadius * 2 * PI / 60) + IDLE_RPM; //Calculating RPM from wheel speed. 3.1 being the Final Drive Gear ratio.
if (CurrentGear < TRANSMISSIONS && Speed * 3.6 > ShiftSpeed[CurrentGear]) //simulating Torque drop in transmission
{
CurrentGear++;
ShiftTime = simTime.Double() + 0.25;
}
else if (CurrentGear > 1 && Speed * 3.6 < ShiftSpeed[CurrentGear - 1] - 10) //simulating Torque drop in transmission
{
CurrentGear--;
ShiftTime = simTime.Double() + 0.25;
}
int indexRPM = ((int)CurrentRPM) / 600;
double interpolatedEngineTorque = 400 + 0.1620123 * CurrentRPM - 0.00005657748 * CurrentRPM * CurrentRPM; //An extracted function from the TorqueRPM600 array
// double interpolatedEngineTorque=(TorqueRPM600[indexRPM]+TorqueRPM600[indexRPM+1]*fmod(CurrentRPM,600)/600)/(1+fmod(CurrentRPM,600)/600); Deprecated interpolation code
Torque = ThrottlePedal * interpolatedEngineTorque * GearRatio[CurrentGear] * power;
if (simTime < ShiftTime) //simulating Torque drop in transmission
Torque *= 0.5;
EngineLoad = Torque;
// std::cout << CurrentRPM << " RPM at Gear " << CurrentGear << " Speed " << Speed * 3.6 << " Engine Torque " << EngineLoad << std::endl;
}
void apply_efforts()
{
double WheelTorque = Torque;
// std::cout << " Controlling wheels"<< std::endl;
wheel_controller(this->left_wheel_1, WheelTorque);
wheel_controller(this->left_wheel_2, WheelTorque);
wheel_controller(this->right_wheel_1, WheelTorque);
wheel_controller(this->right_wheel_2, WheelTorque);
// std::cout << " Controlling Steering"<< std::endl;
}
void wheel_controller(physics::JointPtr wheelJoint, double Torque2)
{
WheelPower = Torque2;
double wheelOmega = wheelJoint->GetVelocity(0);
if (abs(wheelOmega > 1))
jointforce = WheelPower - damping * wheelOmega - friction * wheelOmega / fabs(wheelOmega);
else
jointforce = WheelPower - damping * wheelOmega;
wheelJoint->SetForce(0, jointforce);
if (wheelJoint == right_wheel_2)
{
wheelsSpeedSum = wheelsSpeedSum + wheelOmega;
Speed = wheelsSpeedSum * WheelRadius / 4;
wheelsSpeedSum = 0;
}
else
wheelsSpeedSum = wheelsSpeedSum + wheelOmega;
}
void apply_steering()
{
double ThetaAckerman = 0;
double ThetaOuter = 0;
if (Steering_Request > 0) //turning right
{
ThetaAckerman = atan(1 / ((1 / (tan(Steering_Request)) + (VehicleWidth / VehicleLength))));
steer_controller(this->streer_joint_left_1, Steering_Request);
steer_controller(this->streer_joint_right_1, ThetaAckerman);
}
else if (Steering_Request < 0) //turning left
{
ThetaAckerman = atan(1 / ((1 / (tan(-Steering_Request)) + (VehicleWidth / VehicleLength))));
steer_controller(this->streer_joint_left_1, -ThetaAckerman);
steer_controller(this->streer_joint_right_1, Steering_Request);
}
else
{
steer_controller(this->streer_joint_left_1, 0);
steer_controller(this->streer_joint_right_1, 0);
}
// std::cout << ThetaAckerman << std::endl;
}
void steer_controller(physics::JointPtr steer_joint, double Angle)
{
// std::cout << " getting angle"<< std::endl;
double currentWheelAngle = steer_joint->Position(0);
double steeringOmega = steer_joint->GetVelocity(0);
if (steer_joint == this->streer_joint_left_1)
{
DesiredAngle = DesiredAngle + steeringSpeed * deltaSimTime * (Angle - DesiredAngle);
if (fabs(Angle - DesiredAngle)<0.01)DesiredAngle=Angle;
IerL+=DesiredAngleR - currentWheelAngle;
double jointforce = control_P * (DesiredAngle - currentWheelAngle)+control_I*IerL - control_D * (steeringOmega);
steer_joint->SetForce(0, jointforce);
// std::cout << currentWheelAngle<< std::endl;
}
else
{
DesiredAngleR = DesiredAngleR + steeringSpeed * deltaSimTime * (Angle - DesiredAngleR);
if (fabs(Angle - DesiredAngleR)<0.01)DesiredAngleR=Angle;
IerR+=DesiredAngleR - currentWheelAngle;
double jointforce = control_P * (DesiredAngleR - currentWheelAngle)+control_I*IerR - control_D * (steeringOmega);
steer_joint->SetForce(0, jointforce);
}
// std::cout << "efforting"<< std::endl;
// this->jointController->SetJointPosition(steer_joint, Angle*0.61);
}
void breaker()
{
// std::cout << " getting angle"<< std::endl;
if (BreakPedal >= 0.09 && !Breaks)
{
TempDamping = damping;
damping = 10000 * BreakPedal * BreakPedal;
Breaks = true;
std::cout << "Break on " << damping << std::endl;
}
else if (BreakPedal == 0 && Breaks)
{
damping = TempDamping;
Breaks = false;
std::cout << "Breaks off " << damping << std::endl;
}
if (BreakPedal >= 0.09 && Breaks)
damping = 10000 * BreakPedal * BreakPedal;
// std::cout << "efforting"<< std::endl;
// this->jointController->SetJointPosition(steer_joint, Angle*0.61);
}
void ApplySuspension()
{
Suspension(spring_left_1);
Suspension(spring_left_2);
Suspension(spring_right_1);
Suspension(spring_right_2);
}
void Suspension(physics::JointPtr Suspension)
{ //The function to control the suspension of the Vehicle
double SpringForce = -(Suspension->Position(0) + SuspenTarget) * suspenSpring - Suspension->GetVelocity(0) * SuspenDamp;
Suspension->SetForce(0, SpringForce);
}
// The subscriber callback , each time data is published to the subscriber this function is being called and recieves the data in pointer msg
void On_Throttle_command(const std_msgs::Float64ConstPtr &msg)
{
Throttle_command_mutex.lock();
// Recieving referance velocity
if (msg->data > 1)
{
Throttle_command = 1;
}
else if (msg->data < -1)
{
Throttle_command = -1;
}
else
{
Throttle_command = msg->data;
}
// Reseting timer every time LLC publishes message
#if GAZEBO_MAJOR_VERSION >= 5
Throttle_command_timer.Reset();
#endif
Throttle_command_timer.Start();
Throttle_command_mutex.unlock();
}
// The subscriber callback , each time data is published to the subscriber this function is being called and recieves the data in pointer msg
void On_Steering_command(const std_msgs::Float64ConstPtr &msg)
{
Angular_command_mutex.lock();
// Recieving referance steering angle
if (msg->data > 1)
{
Steering_Request = 1;
}
else if (msg->data < -1)
{
Steering_Request = -1;
}
else
{
Steering_Request = msg->data;
}
Steering_Request = -Steering_Request;
// Reseting timer every time LLC publishes message
#if GAZEBO_MAJOR_VERSION >= 5
Angular_command_timer.Reset();
#endif
Angular_command_timer.Start();
Angular_command_mutex.unlock();
}
void On_Break_command(const std_msgs::Float64ConstPtr &msg)
{
Breaking_command_mutex.lock();
// Recieving referance velocity
if (msg->data >= 1)
BreakPedal = 1;
else if (msg->data >= 0.09)
BreakPedal = msg->data;
else
BreakPedal = 0;
// Reseting timer every message
#if GAZEBO_MAJOR_VERSION >= 5
Breaking_command_timer.Reset();
#endif
Breaking_command_timer.Start();
Breaking_command_mutex.unlock();
}
// Defining private Pointer to model
physics::ModelPtr model;
// Defining private Pointer to joints
physics::JointPtr right_wheel_1;
physics::JointPtr right_wheel_2;
physics::JointPtr left_wheel_1;
physics::JointPtr left_wheel_2;
physics::JointPtr streer_joint_left_1;
physics::JointPtr streer_joint_right_1;
physics::JointPtr spring_left_1;
physics::JointPtr spring_right_1;
physics::JointPtr spring_left_2;
physics::JointPtr spring_right_2;
// Defining private Pointer to the update event connection
event::ConnectionPtr updateConnection;
// Defining private Ros Node Handle
ros::NodeHandle *Ros_nh;
// Defining private Ros Subscribers
ros::Subscriber Steering_rate_sub;
ros::Subscriber Velocity_rate_sub;
ros::Subscriber Breaking_sub;
// Defining private Ros Publishers
ros::Publisher platform_hb_pub_;
ros::Publisher platform_Speedometer_pub;
std_msgs::Bool connection;
std_msgs::Float64 SpeedMsg;
// Defining private Timers
common::Timer Throttle_command_timer;
common::Timer Angular_command_timer;
common::Timer Breaking_command_timer;
common::Time simTime;
// Defining private Mutex
boost::mutex Angular_command_mutex;
boost::mutex Throttle_command_mutex;
boost::mutex Breaking_command_mutex;
//helper vars
float Throttle_command = 0;
float ThrottlePedal = 0;
float Steering_Request = 0;
float BreakPedal = 0;
double friction = 0;
double WheelPower = 0;
double DesiredAngle = 0;
double DesiredAngleR = 0;
double wheelsSpeedSum = 0;
double CurrentRPM = 0;
double ShiftTime = 0;
double EngineLoad = 0;
int CurrentGear = 1;
double Torque = 0;
double jointforce = 0;
float tempTime = 0;
float Speed = 0;
bool Breaks = false;
double IerL=0;
double IerR=0;
double ShiftSpeed[TRANSMISSIONS] = {0, 20, 40, 65};
double GearRatio[TRANSMISSIONS + 1] = {0, 3.2, 2.5, 1.5, 0.8};
double TorqueRPM600[8] = {0, 350, 515, 500, 450, 300, 200, 200};
double PowerRPM600[8] = {0, 10, 60, 80, 120, 142, 120, 120};
//Dynamic Configuration Definitions
dynamic_reconfigure::Server<colorado::coloradoConfig> *model_reconfiguration_server;
double control_P, control_I, control_D, steeringSpeed,
damping, power, TempDamping, suspenSpring, SuspenDamp, SuspenTarget;
};
// Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
GZ_REGISTER_MODEL_PLUGIN(coloradoDrivingPlugin)
}
// 4*(0.8654*50+0.65450*50+0.48924*50+0.48924*50)+0.48924*80*4-0.48924*50*2+2000 is 0.863347 which is the CoM height.
// LeftRearWheelLoc = (1.79660 1.04 0.48924)

59
sitl_config/car/colorado/worlds/colorado_empty.world
View File

@ -1,59 +0,0 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://sun</uri>
</include>
<scene>
<sky>
<time>10</time>
<clouds>
<speed>5</speed>
<direction>1.14</direction>
<humidity>0.3</humidity>
</clouds>
</sky>
</scene>
<gui>
<camera name="camera_pose">
<pose>80 -60 60 0 0.5 2</pose>
</camera>
</gui>
<!-- <physics type="bullet">
<gravity>0 0 -9.81</gravity>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
<bullet>
<solver>
<type>sequencial_impulse</type>
<iters>150</iters>
<sor>1.3</sor>
</solver>
<constraints>
<cfm>0.0000</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>0</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</bullet>
</physics>-->
<include>
<uri>model://ground_plane</uri>
<name>ground_plane</name>
<pose>0 0 0 0 0 0</pose>
</include>
<include>
<uri>model://colorado</uri>
<name>colorado1</name>
<pose>0 0 2 0 0 0</pose>
<plugin name="colorado1_driving_plugin" filename="libcolorado_driving_plugin.so"/>
</include>
<include>
<uri>model://colorado</uri>
<name>colorado2</name>
<pose>0 5 2 0 0 0</pose>
<plugin name="colorado2_driving_plugin" filename="libcolorado_driving_plugin.so"/>
</include>
</world>
</sdf>

41
sitl_config/launch/outdoor3_ugv.launch Normal file
View File

@ -0,0 +1,41 @@
<launch>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="obstaclestopper" default="false"/>
<param name="use_sim_time" value="true"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find mavlink_sitl_gazebo)/worlds/outdoor3.world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<group ns="ugv_0">
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle1-3.xacro' roboname:='ugv_0'" />
<include file="$(find catvehicle)/launch/catvehicle.launch">
<arg name="robot_name" value="ugv_0"/>
<arg name="init_pose" value="-x 0 -y 0 -z 0 -R 0 -P 0 -Y -0.6"/>
<arg name="config_file" value="catvehicle_control.yaml"/>
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
</include>
</group>
<group ns="ugv_1">
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle4-6.xacro' roboname:='ugv_1'" />
<include file="$(find catvehicle)/launch/catvehicle.launch">
<arg name="robot_name" value="ugv_1"/>
<arg name="init_pose" value="-x -5 -y 0 -z 0 -R 0 -P 0 -Y -0.6"/>
<arg name="config_file" value="catvehicle_control.yaml"/>
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
</include>
</group>
</launch>

1
sitl_config/ugv/catvehicle Submodule

@ -0,0 +1 @@
Subproject commit 384c35895d8349ac7806f1922ef08015d738e167

0
ugv/cmdvel2gazebo/CMakeLists.txt → sitl_config/ugv/cmdvel2gazebo/CMakeLists.txt
View File

0
ugv/cmdvel2gazebo/MW_custom_RTOS_header.h → sitl_config/ugv/cmdvel2gazebo/MW_custom_RTOS_header.h
View File

0
ugv/cmdvel2gazebo/cmdvel2gazebo.cpp → sitl_config/ugv/cmdvel2gazebo/cmdvel2gazebo.cpp
View File

0
ugv/cmdvel2gazebo/cmdvel2gazebo.h → sitl_config/ugv/cmdvel2gazebo/cmdvel2gazebo.h
View File

0
ugv/cmdvel2gazebo/cmdvel2gazebo_data.cpp → sitl_config/ugv/cmdvel2gazebo/cmdvel2gazebo_data.cpp
View File

0
ugv/cmdvel2gazebo/cmdvel2gazebo_private.h → sitl_config/ugv/cmdvel2gazebo/cmdvel2gazebo_private.h
View File

0
ugv/cmdvel2gazebo/cmdvel2gazebo_types.h → sitl_config/ugv/cmdvel2gazebo/cmdvel2gazebo_types.h
View File

0
ugv/cmdvel2gazebo/defines.txt → sitl_config/ugv/cmdvel2gazebo/defines.txt
View File

0
ugv/cmdvel2gazebo/ert_main.cpp → sitl_config/ugv/cmdvel2gazebo/ert_main.cpp
View File

0
ugv/cmdvel2gazebo/linuxinitialize.cpp → sitl_config/ugv/cmdvel2gazebo/linuxinitialize.cpp
View File

0
ugv/cmdvel2gazebo/linuxinitialize.h → sitl_config/ugv/cmdvel2gazebo/linuxinitialize.h
View File

0
ugv/cmdvel2gazebo/package.xml → sitl_config/ugv/cmdvel2gazebo/package.xml
View File

0
ugv/cmdvel2gazebo/rtGetInf.cpp → sitl_config/ugv/cmdvel2gazebo/rtGetInf.cpp
View File

0
ugv/cmdvel2gazebo/rtGetInf.h → sitl_config/ugv/cmdvel2gazebo/rtGetInf.h
View File

0
ugv/cmdvel2gazebo/rtGetNaN.cpp → sitl_config/ugv/cmdvel2gazebo/rtGetNaN.cpp
View File

0
ugv/cmdvel2gazebo/rtGetNaN.h → sitl_config/ugv/cmdvel2gazebo/rtGetNaN.h
View File

0
ugv/cmdvel2gazebo/rt_defines.h → sitl_config/ugv/cmdvel2gazebo/rt_defines.h
View File

0
ugv/cmdvel2gazebo/rt_nonfinite.cpp → sitl_config/ugv/cmdvel2gazebo/rt_nonfinite.cpp
View File

0
ugv/cmdvel2gazebo/rt_nonfinite.h → sitl_config/ugv/cmdvel2gazebo/rt_nonfinite.h
View File

0
ugv/cmdvel2gazebo/rtmodel.h → sitl_config/ugv/cmdvel2gazebo/rtmodel.h
View File

0
ugv/cmdvel2gazebo/rtw_continuous.h → sitl_config/ugv/cmdvel2gazebo/rtw_continuous.h
View File

0
ugv/cmdvel2gazebo/rtw_solver.h → sitl_config/ugv/cmdvel2gazebo/rtw_solver.h
View File

0
ugv/cmdvel2gazebo/rtwtypes.h → sitl_config/ugv/cmdvel2gazebo/rtwtypes.h
View File

0
ugv/cmdvel2gazebo/slTestResult.h → sitl_config/ugv/cmdvel2gazebo/slTestResult.h
View File

0
ugv/cmdvel2gazebo/slTestTypes.h → sitl_config/ugv/cmdvel2gazebo/slTestTypes.h
View File

0
ugv/cmdvel2gazebo/slros_busmsg_conversion.cpp → sitl_config/ugv/cmdvel2gazebo/slros_busmsg_conversion.cpp
View File

0
ugv/cmdvel2gazebo/slros_busmsg_conversion.h → sitl_config/ugv/cmdvel2gazebo/slros_busmsg_conversion.h
View File

0
ugv/cmdvel2gazebo/slros_generic.h → sitl_config/ugv/cmdvel2gazebo/slros_generic.h
View File

0
ugv/cmdvel2gazebo/slros_generic_param.cpp → sitl_config/ugv/cmdvel2gazebo/slros_generic_param.cpp
View File

0
ugv/cmdvel2gazebo/slros_generic_param.h → sitl_config/ugv/cmdvel2gazebo/slros_generic_param.h
View File

0
ugv/cmdvel2gazebo/slros_generic_pubsub.h → sitl_config/ugv/cmdvel2gazebo/slros_generic_pubsub.h
View File

0
ugv/cmdvel2gazebo/slros_initialize.cpp → sitl_config/ugv/cmdvel2gazebo/slros_initialize.cpp
View File

0
ugv/cmdvel2gazebo/slros_initialize.h → sitl_config/ugv/cmdvel2gazebo/slros_initialize.h
View File

0
ugv/cmdvel2gazebo/slros_msgconvert_utils.h → sitl_config/ugv/cmdvel2gazebo/slros_msgconvert_utils.h
View File

0
ugv/obstaclestopper/.gitignore → sitl_config/ugv/obstaclestopper/.gitignore vendored
View File

0
ugv/obstaclestopper/CMakeLists.txt → sitl_config/ugv/obstaclestopper/CMakeLists.txt
View File

0
ugv/obstaclestopper/MW_custom_RTOS_header.h → sitl_config/ugv/obstaclestopper/MW_custom_RTOS_header.h
View File

0
ugv/obstaclestopper/defines.txt → sitl_config/ugv/obstaclestopper/defines.txt
View File

0
ugv/obstaclestopper/ert_main.cpp → sitl_config/ugv/obstaclestopper/ert_main.cpp
View File

0
ugv/obstaclestopper/linuxinitialize.cpp → sitl_config/ugv/obstaclestopper/linuxinitialize.cpp
View File

0
ugv/obstaclestopper/linuxinitialize.h → sitl_config/ugv/obstaclestopper/linuxinitialize.h
View File

0
ugv/obstaclestopper/obstacleStopper.cpp → sitl_config/ugv/obstaclestopper/obstacleStopper.cpp
View File

0
ugv/obstaclestopper/obstacleStopper.h → sitl_config/ugv/obstaclestopper/obstacleStopper.h
View File

0
ugv/obstaclestopper/obstacleStopper_data.cpp → sitl_config/ugv/obstaclestopper/obstacleStopper_data.cpp
View File

0
ugv/obstaclestopper/obstacleStopper_private.h → sitl_config/ugv/obstaclestopper/obstacleStopper_private.h
View File

0
ugv/obstaclestopper/obstacleStopper_types.h → sitl_config/ugv/obstaclestopper/obstacleStopper_types.h
View File

0
ugv/obstaclestopper/package.xml → sitl_config/ugv/obstaclestopper/package.xml
View File

0
ugv/obstaclestopper/rtGetInf.cpp → sitl_config/ugv/obstaclestopper/rtGetInf.cpp
View File

0
ugv/obstaclestopper/rtGetInf.h → sitl_config/ugv/obstaclestopper/rtGetInf.h
View File

0
ugv/obstaclestopper/rtGetNaN.cpp → sitl_config/ugv/obstaclestopper/rtGetNaN.cpp
View File

0
ugv/obstaclestopper/rtGetNaN.h → sitl_config/ugv/obstaclestopper/rtGetNaN.h
View File

0
ugv/obstaclestopper/rt_nonfinite.cpp → sitl_config/ugv/obstaclestopper/rt_nonfinite.cpp
View File

0
ugv/obstaclestopper/rt_nonfinite.h → sitl_config/ugv/obstaclestopper/rt_nonfinite.h
View File

0
ugv/obstaclestopper/rtmodel.h → sitl_config/ugv/obstaclestopper/rtmodel.h
View File

0
ugv/obstaclestopper/rtwtypes.h → sitl_config/ugv/obstaclestopper/rtwtypes.h
View File

0
ugv/obstaclestopper/slros_busmsg_conversion.cpp → sitl_config/ugv/obstaclestopper/slros_busmsg_conversion.cpp
View File

0
ugv/obstaclestopper/slros_busmsg_conversion.h → sitl_config/ugv/obstaclestopper/slros_busmsg_conversion.h
View File

0
ugv/obstaclestopper/slros_generic.h → sitl_config/ugv/obstaclestopper/slros_generic.h
View File

0
ugv/obstaclestopper/slros_generic_param.cpp → sitl_config/ugv/obstaclestopper/slros_generic_param.cpp
View File

0
ugv/obstaclestopper/slros_generic_param.h → sitl_config/ugv/obstaclestopper/slros_generic_param.h
View File

Some files were not shown because too many files have changed in this diff Show More