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

add car

This commit is contained in:
Your Name 2020-12-15 00:07:07 +08:00
parent e0ce58a1a9
commit 8b17c7712d
30 changed files with 3341 additions and 506 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
View File

@ -3,7 +3,7 @@
<div id="sidebar"><a href="./README.en.md" target="_blank"><font color=#0000FF size=5px >[ENGLISH]<font></center><a></div>
### 介绍
这是基于PX4和ROS的无人机仿真平台(目前模拟器使用Gazebo，与Airsim的连接正在开发中)。目前支持多旋翼飞行器（包含四轴和六轴）、固定翼飞行器、可垂直起降固定翼飞行器（包含quadplane，tailsitter和tiltrotor）和无人车。在XTDrone上验证过的算法，可以方便地部署到真实无人机上。
这是基于PX4和ROS的无人机仿真平台(目前模拟器使用Gazebo，与Airsim的连接正在开发中)。目前支持多旋翼飞行器（包含四轴和六轴）、固定翼飞行器、复合翼飞行器（包含quadplane，tailsitter和tiltrotor）和无人车。在XTDrone上验证过的算法，可以方便地部署到真实无人机上。
<img src="./image/vehicles.png" width="640" />
@ -59,7 +59,7 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio
<img src="./image/planes.gif" width="640" height="368" />
10. VTOL
10. 复合翼
<img src="./image/vtols.gif" width="640" height="368" />

4
communication/rover_communication.py
View File

@ -125,12 +125,12 @@ class Communication:
def cmd_vel_flu_callback(self, msg):
self.coordinate_frame=8
self.motion_type=1
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw=0)
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.angular.z,vz=msg.linear.z,yaw=0)
def cmd_vel_enu_callback(self, msg):
self.coordinate_frame=1
self.motion_type=1
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw=0)
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.angular.z,vz=msg.linear.z,yaw=0)
def cmd_callback(self, msg):
if msg.data == '':

2
control/keyboard/rover_keyboard_control.py
View File

@ -163,7 +163,7 @@ if __name__=="__main__":
elif angle < -MAX_ANGLE:
angle = -MAX_ANGLE
twist.linear.x = forward; twist.linear.y = angle
twist.linear.x = forward; twist.angular.z = angle
for i in range(rover_num):
if ctrl_leader:

8
control/rover_self_driving.py
View File

@ -9,14 +9,14 @@ def lane_mid_error_callback(msg):
global twist
if msg.data == 1000:
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.angular.z = 0.0
else:
if abs(msg.data) > 20:
twist.linear.y = - Kp * msg.data
twist.angular.z = - Kp * msg.data
else:
twist.linear.y = 0.0
twist.angular.z = 0.0
twist.linear.x = Vx * (1 - abs(twist.linear.y))
twist.linear.x = Vx * (1 - abs(twist.angular.z))
if __name__ == "__main__":

501
coordination/launch_generator/multi_vehicle.launch
View File

@ -18,8 +18,8 @@
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- iris_0 -->
<group ns="iris_0">
<!-- rover_0 -->
<group ns="rover_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
@ -32,8 +32,8 @@
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="vehicle" value="rover"/>
<arg name="sdf" value="rover_with_lidar_stereo"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
@ -47,498 +47,5 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_1 -->
<group ns="iris_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="1"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:24541@localhost:34581"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<arg name="y" value="3"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18571"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_2 -->
<group ns="iris_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="2"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:24542@localhost:34582"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="3"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18572"/>
<arg name="mavlink_tcp_port" value="4562"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_3 -->
<group ns="iris_3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="3"/>
<arg name="ID_in_group" value="3"/>
<arg name="fcu_url" default="udp://:24543@localhost:34583"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18573"/>
<arg name="mavlink_tcp_port" value="4563"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_4 -->
<group ns="iris_4">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="4"/>
<arg name="ID_in_group" value="4"/>
<arg name="fcu_url" default="udp://:24544@localhost:34584"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18574"/>
<arg name="mavlink_tcp_port" value="4564"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_5 -->
<group ns="iris_5">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="5"/>
<arg name="ID_in_group" value="5"/>
<arg name="fcu_url" default="udp://:24545@localhost:34585"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18575"/>
<arg name="mavlink_tcp_port" value="4565"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_6 -->
<group ns="iris_6">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="6"/>
<arg name="ID_in_group" value="6"/>
<arg name="fcu_url" default="udp://:24546@localhost:34586"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18576"/>
<arg name="mavlink_tcp_port" value="4566"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_7 -->
<group ns="iris_7">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="7"/>
<arg name="ID_in_group" value="7"/>
<arg name="fcu_url" default="udp://:24547@localhost:34587"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18577"/>
<arg name="mavlink_tcp_port" value="4567"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_8 -->
<group ns="iris_8">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="8"/>
<arg name="ID_in_group" value="8"/>
<arg name="fcu_url" default="udp://:24548@localhost:34588"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18578"/>
<arg name="mavlink_tcp_port" value="4568"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_9 -->
<group ns="iris_9">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="9"/>
<arg name="ID_in_group" value="9"/>
<arg name="fcu_url" default="udp://:24549@localhost:34589"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="12"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18579"/>
<arg name="mavlink_tcp_port" value="4569"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_10 -->
<group ns="iris_10">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="10"/>
<arg name="ID_in_group" value="10"/>
<arg name="fcu_url" default="udp://:24550@localhost:34590"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<arg name="y" value="12"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18580"/>
<arg name="mavlink_tcp_port" value="4570"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_11 -->
<group ns="iris_11">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="11"/>
<arg name="ID_in_group" value="11"/>
<arg name="fcu_url" default="udp://:24551@localhost:34591"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="12"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18581"/>
<arg name="mavlink_tcp_port" value="4571"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_12 -->
<group ns="iris_12">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="12"/>
<arg name="ID_in_group" value="12"/>
<arg name="fcu_url" default="udp://:24552@localhost:34592"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="15"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18582"/>
<arg name="mavlink_tcp_port" value="4572"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_13 -->
<group ns="iris_13">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="13"/>
<arg name="ID_in_group" value="13"/>
<arg name="fcu_url" default="udp://:24553@localhost:34593"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<arg name="y" value="15"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18583"/>
<arg name="mavlink_tcp_port" value="4573"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_14 -->
<group ns="iris_14">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="14"/>
<arg name="ID_in_group" value="14"/>
<arg name="fcu_url" default="udp://:24554@localhost:34594"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="15"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18584"/>
<arg name="mavlink_tcp_port" value="4574"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_15 -->
<group ns="iris_15">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="15"/>
<arg name="ID_in_group" value="15"/>
<arg name="fcu_url" default="udp://:24555@localhost:34595"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="18"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18585"/>
<arg name="mavlink_tcp_port" value="4575"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_16 -->
<group ns="iris_16">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="16"/>
<arg name="ID_in_group" value="16"/>
<arg name="fcu_url" default="udp://:24556@localhost:34596"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<arg name="y" value="18"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18586"/>
<arg name="mavlink_tcp_port" value="4576"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_17 -->
<group ns="iris_17">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="17"/>
<arg name="ID_in_group" value="17"/>
<arg name="fcu_url" default="udp://:24557@localhost:34597"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="18"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18587"/>
<arg name="mavlink_tcp_port" value="4577"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->

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

Binary file not shown.

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

Binary file not shown.

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

@ -0,0 +1,44 @@
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~ Normal file
View File

@ -0,0 +1,44 @@
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 Executable file
View File

@ -0,0 +1,47 @@
#!/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 Executable file
View File

@ -0,0 +1,45 @@
#!/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 Executable file
View File

@ -0,0 +1,21 @@
#!/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~ Executable file
View File

@ -0,0 +1,20 @@
#!/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 Executable file
View File

@ -0,0 +1,30 @@
<?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 Normal file
View File

@ -0,0 +1,59 @@
<?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 Executable file
View File

@ -0,0 +1,5 @@
#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 Normal file
View File

@ -0,0 +1,6 @@
<?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 Normal file
View File

@ -0,0 +1,819 @@
<?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 Normal file
View File

@ -0,0 +1,62 @@
<?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 Normal file
View File

@ -0,0 +1,4 @@
<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 Normal file
View File

@ -0,0 +1,83 @@
<!-------------------------->
<!--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 Normal file
View File

@ -0,0 +1,397 @@
<!-------------------------->
<!--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 Normal file
View File

@ -0,0 +1,276 @@
<!-------------------------->
<!--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 Normal file
View File

@ -0,0 +1,456 @@
// 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 Normal file
View File

@ -0,0 +1,59 @@
<?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>