diff --git a/README.md b/README.md
index deffac9..e777f75 100644
--- a/README.md
+++ b/README.md
@@ -5,24 +5,30 @@
# FSSIM
FSSIM is a vehicle simulator dedicated for Formula Student Driverless Competition. It was developed for autonomous software testing purposes and not for gaming. A version of this simulator was used to predict **lap time of *gotthard* at FSG 2018** trackdrive with **1% accuracy**.
-This simulator is developed and tested on **Ubuntu 16.04 and ROS Kinetic** and both are assumed to be already installed.
+This simulator is developed and tested on **Ubuntu 20.04, Python 3 and ROS Noetic** and the three are assumed to be already installed.
The more extensive tutorial can be found under [Wiki](fssim_doc/index.md)
FSSIM is developed by [Juraj Kabzan](https://www.linkedin.com/in/juraj-kabzan-143698a1/) as part of our work at [AMZ-Driverless](http://driverless.amzracing.ch/).
# How to Run It in your Workspace
-0. Install `sudo apt install ros-kinetic-desktop-full` and `sudo apt install python-catkin-tools`
-1. Clone this repository to an existing **ROS Workspace** initialized with `catkin init`
-2. Run `cd src/fssim` from the workspace.
+0. Install `sudo apt install ros-noetic-desktop-full` and `sudo apt install python3-catkin-tools`
+1. Clone this repository to an existing **ROS Workspace**, run:
+`mkdir ws` ->
+`mkdir ws/src` ->
+`cd ws` ->
+`catkin init` ->
+`cd src` ->
+`git clone https://github.com/Huguet57/fssim-2021.git`
+2. Go to the `src/fssim` folder.
3. Run `./update_dependencies.sh`, you will need to approve multiple packages to be installed
4. Run `catkin build`
-5. Source the workspace
+5. Go into the root of the workspace (in this case the `ws` folder, where the `devel` folder is) and source the workspace `source devel/setup.bash`
6. After successful building, run the simulator with `roslaunch fssim auto_fssim.launch`. RVIZ window will start. NOTE: You might need to untick and tick `FSSIM Track` and `RobotModel` in RVIZ in order to load the STL files. NOTE: This ` [Wrn] [ModelDatabase.cc:339] Getting models from[http://gazebosim.org/models/]. This may take a few seconds.` might take up to a minute when starting for the first time.
7. The terminal will inform you what is happening. The loading time takes around 20 seconds. When `Sending RES GO` will show up in the terminal, you can start controlling the vehicle with `/fssim/cmd` topic.
# Combine it with simple FSD skeleton Framework and drive a lap
-0. Install `sudo apt install ros-kinetic-desktop-full` and `sudo apt install python-catkin-tools`
+0. Install `sudo apt install ros-noetic-desktop-full` and `sudo apt install python3-catkin-tools`
1. [Clone the AMZ skeleton workspace](https://github.com/AMZ-Driverless/fsd_skeleton#setting-up-the-workspace).
2. Run `./update_dependencies.sh -f` from `fsd_skeleton`, you will need to approve multiple packages to be installed
3. Compile with `catkin build`
diff --git a/fssim/CMakeLists.txt b/fssim/CMakeLists.txt
index c751037..e1a181c 100644
--- a/fssim/CMakeLists.txt
+++ b/fssim/CMakeLists.txt
@@ -20,7 +20,7 @@
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
project(fssim)
find_package(catkin REQUIRED COMPONENTS
@@ -41,4 +41,4 @@ catkin_install_python(PROGRAMS scripts/launch.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
catkin_install_python(PROGRAMS scripts/topics_monitor.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
\ No newline at end of file
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/fssim/config/simulation.yaml b/fssim/config/simulation.yaml
index 6c11d46..a7acc46 100644
--- a/fssim/config/simulation.yaml
+++ b/fssim/config/simulation.yaml
@@ -27,7 +27,7 @@ robot_name: gotthard # Name of the robot [string]
kill_after: 6000 # After this time, repetition is stopped [s]
pkg_config_storage: fssim_description # If relative path is given in following config files, the path will be taken
- # wrt to this package
+ # wrt to this packagesendTransform
# Config files for repetitions
repetitions:
- {sensors_config_file: cars/gotthard/config/sensors.yaml, track_name: FSG.sdf, autonomous_stack: }
diff --git a/fssim/config/topics_frequency.yaml b/fssim/config/topics_frequency.yaml
index 2e1cf2e..5cd4b2e 100644
--- a/fssim/config/topics_frequency.yaml
+++ b/fssim/config/topics_frequency.yaml
@@ -28,7 +28,7 @@ topics:
- topic: "/lidar/cones"
expected_frequency: 5
- topic: "/fssim/cmd"
- expected_frequency: 5
+ expected_frequency: 100
diff --git a/fssim/rviz/fssim.rviz b/fssim/rviz/fssim.rviz
index 31d4f0f..1b8aa76 100644
--- a/fssim/rviz/fssim.rviz
+++ b/fssim/rviz/fssim.rviz
@@ -3,9 +3,10 @@ Panels:
Help Height: 93
Name: Displays
Property Tree Widget:
- Expanded: ~
- Splitter Ratio: 0.663888872
- Tree Height: 732
+ Expanded:
+ - /Global Options1
+ Splitter Ratio: 0.550000011920929
+ Tree Height: 460
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -14,17 +15,21 @@ Panels:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
- Splitter Ratio: 0.588679016
+ Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
- Splitter Ratio: 0.492592603
+ Splitter Ratio: 0.49259260296821594
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Lidar Observations
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
@@ -34,7 +39,7 @@ Visualization Manager:
Color: 177; 177; 177
Enabled: true
Line Style:
- Line Width: 0.0299999993
+ Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
@@ -57,7 +62,7 @@ Visualization Manager:
Enabled: true
Length: 1
Name: FSSIM Map
- Radius: 0.100000001
+ Radius: 0.10000000149011612
Reference Frame: fssim_map
Value: true
- Alpha: 1
@@ -136,18 +141,6 @@ Visualization Manager:
Value: true
fssim/vehicle/front_center_lidar_link:
Value: true
- fssim/vehicle/left_front_wheel:
- Value: true
- fssim/vehicle/left_rear_wheel:
- Value: true
- fssim/vehicle/left_steering_hinge:
- Value: true
- fssim/vehicle/right_front_wheel:
- Value: true
- fssim/vehicle/right_rear_wheel:
- Value: true
- fssim/vehicle/right_steering_hinge:
- Value: true
fssim_map:
Value: true
map:
@@ -164,16 +157,6 @@ Visualization Manager:
fssim/vehicle/chassis:
fssim/vehicle/front_center_lidar_link:
{}
- fssim/vehicle/left_rear_wheel:
- {}
- fssim/vehicle/left_steering_hinge:
- fssim/vehicle/left_front_wheel:
- {}
- fssim/vehicle/right_rear_wheel:
- {}
- fssim/vehicle/right_steering_hinge:
- fssim/vehicle/right_front_wheel:
- {}
map:
{}
Update Interval: 0
@@ -279,6 +262,14 @@ Visualization Manager:
{}
Queue Size: 100
Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /fssim/GO_marker
+ Name: GO Signal
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
Enabled: true
Global Options:
Background Color: 255; 255; 255
@@ -294,7 +285,10 @@ Visualization Manager:
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
@@ -304,33 +298,33 @@ Visualization Manager:
Views:
Current:
Class: rviz/ThirdPersonFollower
- Distance: 15.6628675
+ Distance: 15.662867546081543
Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
+ Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 0.198139191
- Y: -0.605263352
- Z: -1.90734863e-06
+ X: 0.19813919067382812
+ Y: -0.605263352394104
+ Z: -1.9073486328125e-06
Focal Shape Fixed Size: true
- Focal Shape Size: 0.0500000007
+ Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
- Near Clip Distance: 0.00999999978
- Pitch: 0.277739823
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.27773982286453247
Target Frame: fssim/vehicle/base_link
Value: ThirdPersonFollower (rviz)
- Yaw: 3.0936656
+ Yaw: 3.093665599822998
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 1028
+ Height: 704
Hide Left Dock: false
Hide Right Dock: true
- QMainWindow State: 000000ff00000000fd00000004000000000000016a0000037afc0200000008fb0000001200530065006c0065006300740069006f006e0000000028000001c50000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001100000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c00000003efc0100000002fb0000000800540069006d00650100000000000003c00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002500000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd00000004000000000000015600000266fc0200000008fb0000001200530065006c0065006300740069006f006e0000000028000001c50000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000266000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001100000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c00000003efc0100000002fb0000000800540069006d00650000000000000003c0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002640000026600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -340,5 +334,5 @@ Window Geometry:
Views:
collapsed: true
Width: 960
- X: 960
- Y: 24
+ X: 406
+ Y: 27
diff --git a/fssim/scripts/automated_res.py b/fssim/scripts/automated_res.py
index 41d4102..8d39219 100755
--- a/fssim/scripts/automated_res.py
+++ b/fssim/scripts/automated_res.py
@@ -64,6 +64,7 @@ def generate_track_model(track_name):
xacro_file = fssim_gazebo + '/models/track/model.xacro'
xacro_out_file = fssim_gazebo + '/models/track/model.config'
command = "xacro --inorder {} track_name:={} > {}".format(xacro_file, track_name, xacro_out_file)
+ rospy.logwarn("%s", command)
xacro_proccess = subprocess.check_output(command, shell = True, stderr = subprocess.STDOUT)
@@ -110,7 +111,7 @@ def __init__(self, arg):
self.pub_initialpose = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size = 1)
with open(arg.config, 'r') as f:
- self.sim_config = yaml.load(f)
+ self.sim_config = yaml.load(f, Loader=yaml.FullLoader)
self.sim_config_id = arg.sim_id
self.checks_is_in_track = self.sim_config["res"]["checks"]["is_in_track"]
@@ -160,7 +161,7 @@ def run(self):
return
# Send initial position if available
- if self.track_checks.track_details is not None:
+ if self.track_checks.track_details != None:
rospy.logwarn("Setting Initial Pose")
pose_init = PoseWithCovarianceStamped()
pose_init.pose.pose.position, pose_init.pose.pose.orientation = self.track_checks.get_track_init_pos()
@@ -174,7 +175,7 @@ def run(self):
t.transform.translation.y = 0.0
t.transform.rotation.w = 1.0
self.br.sendTransform(t)
-
+
# Wait for short time
rospy.sleep(5.0)
@@ -185,7 +186,7 @@ def run(self):
# Launch Recording
raw_rosbag_launch_cmd = self.sim_config['launch_file']['rosbag_record']
gen_rosbag_name = self.statistics.get_rosbag_name(self.output_folder, self.sim_config_id)
- if gen_rosbag_name is not None:
+ if gen_rosbag_name != None:
self.cmd_rosbag = self.launch_file(raw_rosbag_launch_cmd.format(gen_rosbag_name))
# Wait for short time
@@ -198,7 +199,7 @@ def run(self):
is_inside_of_track = self.track_checks.is_car_in_track()
if not is_inside_of_track:
- rospy.logwarn("Car Is not in the track")
+ rospy.logwarn("Car != in the track")
rospy.sleep(0.5)
# Stop sim after max time achieved
@@ -210,6 +211,9 @@ def run(self):
# Terminate simulation is any of conditions are met
stop_simulation = not is_inside_of_track or time_expired or self.ecu.state == EcuState.FINNISHED_DISCIPLINE or request_stop_given_mission
+ # Edit: Do not stop simulation
+ stop_simulation = False
+
if not stop_simulation and not self.sim_health.vehicle_started:
# Log Starting time
@@ -226,7 +230,8 @@ def run(self):
self.sim_health.vehicle_started = True
elif not is_inside_of_track and self.sim_health.vehicle_started or stop_simulation:
-
+
+ """
rospy.logwarn("Sending EMERGENCY STOP")
self.send_res(False)
@@ -236,13 +241,16 @@ def run(self):
# Request shutdown of all commands
self.sim_health.request_shutdown = True
break
+ """
+
+ print("We don't want to stop the simulation")
# Stream health of FSSIM
self.pub_health.publish(self.sim_health)
try:
rate.sleep()
except:
- print "ROSCORE has been killed during sleep"
+ print("ROSCORE has been killed during sleep")
# Write statisctics into a file
self.statistics.write_report(self.sim_config_id, self.ecu.state == EcuState.FINNISHED_DISCIPLINE)
@@ -258,7 +266,7 @@ def run(self):
def sim_time_expired(self):
time_expired = False
repetition_time = rospy.Time.now().to_sec() - self.start_time
- if 'kill_after' in self.sim_config and self.sim_config['kill_after'] is not 0:
+ if 'kill_after' in self.sim_config and self.sim_config['kill_after'] != 0:
if self.sim_health.vehicle_started and repetition_time > self.sim_config['kill_after']:
time_expired = True
rospy.logerr("Simulation Time EXPIRED with: %f", repetition_time)
@@ -274,7 +282,7 @@ def launch_fssim(self, settings, pkg_config_storage = 'fssim_description'):
# type: (dict, str) -> None
# We are sure the config is going to be in fssim_description
- if pkg_config_storage is None:
+ if pkg_config_storage == None:
pkg_config_storage = 'fssim_description'
# Create Track Config
@@ -289,7 +297,7 @@ def launch_fssim(self, settings, pkg_config_storage = 'fssim_description'):
if os.path.isfile(track_detailed_description):
rospy.logwarn("Found detailed description")
with open(track_detailed_description, 'r') as f:
- self.track_details = yaml.load(f)
+ self.track_details = yaml.load(f, Loader=yaml.FullLoader)
self.track_checks.track_details = self.track_details
# Parse Config paths
@@ -348,7 +356,7 @@ def launch_file(self, launch_file):
:return: Command which was launched
:rtype: Command
'''
- if launch_file is not None:
+ if launch_file != None:
process = Command(launch_file)
process.run()
rospy.logwarn("LAUNCHING: \n%s", launch_file)
@@ -398,9 +406,9 @@ def terminate_all_launched_commands(self):
Terminate everything was has been launched
:return: None
'''
- if self.cmd_rosbag is not None:
+ if self.cmd_rosbag != None:
self.cmd_rosbag.ensure_terminated()
- if self.cmd_autonomous_system is not None:
+ if self.cmd_autonomous_system != None:
self.cmd_autonomous_system.ensure_terminated()
self.cmd_fssim.ensure_terminated()
@@ -441,7 +449,7 @@ def start_roscore():
except socket.error:
roscore.run()
time.sleep(1.0) # wait a bit to be sure the roscore is really launched
- print '\033[93m', "ROSCORE STARTED HERE", '\033[0m'
+ print('\033[93m', "ROSCORE STARTED HERE", '\033[0m')
return roscore
@@ -455,7 +463,7 @@ def start_roscore():
parser.add_argument("--output", dest = "output", metavar = "FOLDER", help = "Output YAML file")
parser.add_argument("--id", dest = "sim_id", help = "Config ID in YAML file", default = 0, type = int)
args, unknown = parser.parse_known_args()
- args.output = os.path.abspath(args.output) if args.output is not None else None
+ args.output = os.path.abspath(args.output) if args.output != None else None
roscore = start_roscore()
@@ -471,4 +479,4 @@ def start_roscore():
roscore.ensure_terminated()
- print "THIS REPETITION IS OVER"
+ print("THIS REPETITION IS OVER")
diff --git a/fssim/scripts/launch.py b/fssim/scripts/launch.py
index 1ea67c1..44a21ca 100755
--- a/fssim/scripts/launch.py
+++ b/fssim/scripts/launch.py
@@ -46,7 +46,7 @@ class Launcher:
def __init__(self, args):
with open(args.config, 'r') as f:
- self.config = yaml.load(f)
+ self.config = yaml.load(f, Loader=yaml.FullLoader)
self.args = args
def setup_report_file(self):
@@ -66,16 +66,16 @@ def start(self):
self.setup_report_file()
for i, settings in enumerate(self.config['repetitions']):
- print "STARITNG REPETITION: ", i
+ print("STARITNG REPETITION: ", i)
path = rospkg.RosPack().get_path("fssim") + "/scripts/automated_res.py"
launching_script = "python {} --config {} --id {} --output {} ".format(path, self.args.config, i, self.args.output)
- print launching_script
+ print(launching_script)
self.fssim_cmd = Command(launching_script)
self.fssim_cmd.run()
self.fssim_cmd.join()
time.sleep(5.0)
- print "EXITING LAUNCHER SCRIPT"
+ print("EXITING LAUNCHER SCRIPT")
sys.exit(0)
@@ -85,26 +85,26 @@ def start(self):
parser.add_argument("--output", dest = "output", help = "Output YAML file")
args = parser.parse_args()
- print '\033[91m' + " Welcome to: " + '\033[0m'
- print '\033[91m' + " `--.` " + '\033[0m'
- print '\033[91m' + " +hyhys+:` " + '\033[0m'
- print '\033[91m' + " +yyyyyyyhyo:. " + '\033[0m'
- print '\033[91m' + " +yyyyyyyyyyyhyo/-` " + '\033[0m'
- print '\033[91m' + " yyyyyyyyyyyyyyyyyyyysoo++ooosyyhyys/ " + '\033[0m'
- print '\033[91m' + " yyyyyyyyyyyyyyyys:. :hhyyy+ " + '\033[0m'
- print '\033[91m' + " `yyyyyyyyyyyyyyy: -hhyyys " + '\033[0m'
- print '\033[91m' + " `yyyyyyyo------. -://:-.`yyyyys " + '\033[0m'
- print '\033[91m' + " `hyyyyyh/ /hyhyyyyyyhyyys-//- -ssooo++//:--..` " + '\033[0m'
- print '\033[91m' + " .hyyyyyy+```````` :syyyyyyyyyyyyhhyy+` .+yyyyyyyyyhyhhyhhyyysoo+/:-.` " + '\033[0m'
- print '\033[91m' + " .hyyyyyyhhhhhhhhy: -oyyyyyyyyyyyyyyyyo+::::::://+oyhyyyyyyyyyyyyyyyyyyyyyyyyyyyyhyyo+/-` " + '\033[0m'
- print '\033[91m' + " -yyyyyyyyyyyyyyyyys:` `+yyyyyyyyyyyyyyyssshhyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy- " + '\033[0m'
- print '\033[91m' + " /yyyyyyyyyyyyyyyyyyyy/` .syhyyyyyyyyyyy/:/hyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy/ " + '\033[0m'
- print '\033[91m' + " +hyyyyyy.` `:yyyyy+ `yhyyyy: ../y` .h/ . -` .yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyo` " + '\033[0m'
- print '\033[91m' + " oyyyyyyy` -yyyyyo` yyyyyh+. -+yy -h: os -y. yyyyyyyyyyyyyyyyyyyyyyyyyyyyys- " + '\033[0m'
- print '\033[91m' + " shyyyyyy `` /yhyyys:/. os -y- yo /y. `yyyyyyyyyyyyyyhyyhysshyyyyyyy+/::--` " + '\033[0m'
- print '\033[91m' + " yyyyyyyy .+yhhyhyy++++syy++oyo+oys++syo+oyyyyyyhyyyyyhhy+-` -++++++++++++++. " + '\033[0m'
- print '\033[91m' + " ........ -syyyyyo///////+/+++++++++++++++++++++++oyyyyyys- " + '\033[0m'
- print '\033[91m' + " `.--.` `-::-` " + '\033[0m'
+ print('\033[91m' + " Welcome to: " + '\033[0m')
+ print('\033[91m' + " `--.` " + '\033[0m')
+ print('\033[91m' + " +hyhys+:` " + '\033[0m')
+ print('\033[91m' + " +yyyyyyyhyo:. " + '\033[0m')
+ print('\033[91m' + " +yyyyyyyyyyyhyo/-` " + '\033[0m')
+ print('\033[91m' + " yyyyyyyyyyyyyyyyyyyysoo++ooosyyhyys/ " + '\033[0m')
+ print('\033[91m' + " yyyyyyyyyyyyyyyys:. :hhyyy+ " + '\033[0m')
+ print('\033[91m' + " `yyyyyyyyyyyyyyy: -hhyyys " + '\033[0m')
+ print('\033[91m' + " `yyyyyyyo------. -://:-.`yyyyys " + '\033[0m')
+ print('\033[91m' + " `hyyyyyh/ /hyhyyyyyyhyyys-//- -ssooo++//:--..` " + '\033[0m')
+ print('\033[91m' + " .hyyyyyy+```````` :syyyyyyyyyyyyhhyy+` .+yyyyyyyyyhyhhyhhyyysoo+/:-.` " + '\033[0m')
+ print('\033[91m' + " .hyyyyyyhhhhhhhhy: -oyyyyyyyyyyyyyyyyo+::::::://+oyhyyyyyyyyyyyyyyyyyyyyyyyyyyyyhyyo+/-` " + '\033[0m')
+ print('\033[91m' + " -yyyyyyyyyyyyyyyyys:` `+yyyyyyyyyyyyyyyssshhyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy- " + '\033[0m')
+ print('\033[91m' + " /yyyyyyyyyyyyyyyyyyyy/` .syhyyyyyyyyyyy/:/hyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy/ " + '\033[0m')
+ print('\033[91m' + " +hyyyyyy.` `:yyyyy+ `yhyyyy: ../y` .h/ . -` .yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyo` " + '\033[0m')
+ print('\033[91m' + " oyyyyyyy` -yyyyyo` yyyyyh+. -+yy -h: os -y. yyyyyyyyyyyyyyyyyyyyyyyyyyyyys- " + '\033[0m')
+ print('\033[91m' + " shyyyyyy `` /yhyyys:/. os -y- yo /y. `yyyyyyyyyyyyyyhyyhysshyyyyyyy+/::--` " + '\033[0m')
+ print('\033[91m' + " yyyyyyyy .+yhhyhyy++++syy++oyo+oys++syo+oyyyyyyhyyyyyhhy+-` -++++++++++++++. " + '\033[0m')
+ print('\033[91m' + " ........ -syyyyyo///////+/+++++++++++++++++++++++oyyyyyys- " + '\033[0m')
+ print('\033[91m' + " `.--.` `-::-` " + '\033[0m')
fssim = Launcher(args)
fssim.start()
diff --git a/fssim/scripts/topics_monitor.py b/fssim/scripts/topics_monitor.py
index f4c7267..31babc4 100755
--- a/fssim/scripts/topics_monitor.py
+++ b/fssim/scripts/topics_monitor.py
@@ -51,13 +51,13 @@ def get_rostopic_hz(topic, window_size=-1, filter_expr=None, use_wtime=False, av
rt = rostopic.ROSTopicHz(window_size, filter_expr=filter_expr, use_wtime=use_wtime)
msg_class, real_topic, _ = rostopic.get_topic_class(topic, blocking=False)
- if real_topic is None:
+ if real_topic == None:
return 0.0
# pause hz until topic is published
# we use a large buffer size as we don't know what sort of messages we're dealing with.
# may parameterize this in the future
- if filter_expr is not None:
+ if filter_expr != None:
# have to subscribe with topic_type
rospy.Subscriber(real_topic, msg_class, rt.callback_hz, callback_args=topic)
else:
@@ -107,7 +107,7 @@ def run(self):
try:
rate.sleep()
except:
- print "ROSCORE has been killed during sleep"
+ print("ROSCORE has been killed during sleep")
def check_topics_hz(self, topic_config, precision=10.0):
success = True
@@ -135,7 +135,7 @@ def check_topics_hz(self, topic_config, precision=10.0):
topics_config = rospy.get_param("~topics_frequency_config")
with open(topics_config, 'r') as f:
- topics_frequency_config = yaml.load(f)
+ topics_frequency_config = yaml.load(f, Loader=yaml.FullLoader)
topic_monitor = MonitorTopics(topics_frequency_config['topics'])
topic_monitor.run()
diff --git a/fssim/src/fssim/ecu.py b/fssim/src/fssim/ecu.py
index b07b467..fe27e06 100644
--- a/fssim/src/fssim/ecu.py
+++ b/fssim/src/fssim/ecu.py
@@ -52,7 +52,7 @@ class EcuState(Enum):
class Ecu:
def __init__(self):
- print "ECU"
+ print("ECU")
self.state = EcuState.NOT_STARTED
self.car_state_stoped = 0.0
diff --git a/fssim/src/fssim/shell.py b/fssim/src/fssim/shell.py
index 13359ed..24a5609 100644
--- a/fssim/src/fssim/shell.py
+++ b/fssim/src/fssim/shell.py
@@ -44,10 +44,10 @@ def __init__(self, cmd, env=None):
self._env = os.environ.copy()
self._popen = None
- if env is None:
+ if env == None:
env = {}
- for key, value in env.iteritems():
+ for key, value in env.items():
self._env[key] = value
def run(self):
@@ -57,7 +57,7 @@ def run(self):
def is_running(self):
if self._popen:
self._popen.poll()
- if self._popen.returncode is None:
+ if self._popen.returncode == None:
return True
return False
@@ -70,21 +70,21 @@ def ensure_terminated(self, status=""):
try:
os.kill(self._popen.pid, signal.SIGINT)
except OSError:
- print "Process does not exist"
+ print("Process does not exist")
return
time.sleep(0.5)
if self._popen:
self._popen.poll()
- if self._popen.returncode is None:
+ if self._popen.returncode == None:
self._popen.terminate()
time.sleep(0.2)
self._popen.poll()
- while self._popen.returncode is None:
+ while self._popen.returncode == None:
time.sleep(1)
if status:
print(status)
self._popen.poll()
else:
- print "ERROR Popen is NONE"
\ No newline at end of file
+ print("ERROR Popen == None")
\ No newline at end of file
diff --git a/fssim/src/fssim/statistics.py b/fssim/src/fssim/statistics.py
index 0ca4d79..40433c2 100644
--- a/fssim/src/fssim/statistics.py
+++ b/fssim/src/fssim/statistics.py
@@ -75,7 +75,7 @@ def __init__(self, folder, mission):
self.starting_time = 0.0
self.res_go_time = 0.0
self.lap_time = []
- if folder is None:
+ if folder == None:
self.report_file_name = None
else:
if not os.path.isdir(folder):
@@ -93,14 +93,14 @@ def is_mission_finnished(self):
return self.lap_count == 11 and self.last_state.vx <= 1.5
elif self.mission == 'acceleration':
rospy.logwarn("State x: %f", self.last_state.x)
- return self.last_state.x > 76 and self.last_state.x < 120 and len(self.lap_time) is not 0
+ return self.last_state.x > 76 and self.last_state.x < 120 and len(self.lap_time) != 0
elif self.mission == 'skidpad':
rospy.logwarn("State x: %f, Laps: %i", self.last_state.x, self.lap_count)
return self.last_state.x > 4 and self.last_state.x < 20 and self.lap_count == 5
def update_state(self, state):
self.state_received = True
- if self.mission is 'trackdrive' or self.mission is 'skidpad':
+ if self.mission == 'trackdrive' or self.mission == 'skidpad':
cross_line = intersect(self.start_A, self.start_B, to_point(self.last_state), to_point(state))
if cross_line:
self.lap_count = self.lap_count + 1
@@ -134,7 +134,7 @@ def update_state(self, state):
self.last_state = state
def get_rosbag_name(self, folder, sim_id):
- if folder is None:
+ if folder == None:
return None
else:
self.rosbag_name = str(sim_id) + '.bag'
@@ -159,15 +159,15 @@ def get_statistics(self, id, ecu_state):
return repetition
def write_report(self, id, ecu_state):
- if self.report_file_name is not None:
+ if self.report_file_name != None:
with open(self.report_file_name, 'r+') as yamlfile:
- report_yaml = yaml.load(yamlfile)
+ report_yaml = yaml.load(yamlfile, Loader=yaml.FullLoader)
- if report_yaml is None:
+ if report_yaml == None:
report_yaml = {"name": "default_name", "repetition": {}}
repetitions = report_yaml['repetitions']
- if repetitions is None:
+ if repetitions == None:
report_yaml['repetitions'] = self.get_statistics(id, ecu_state)
else:
report_yaml["repetitions"].update(self.get_statistics(id,ecu_state))
diff --git a/fssim/src/fssim/vehicle_position_validate.py b/fssim/src/fssim/vehicle_position_validate.py
index 081c1d1..a5b63ad 100644
--- a/fssim/src/fssim/vehicle_position_validate.py
+++ b/fssim/src/fssim/vehicle_position_validate.py
@@ -202,7 +202,7 @@ def callback_track(self, data):
self.end_A = self.start_A
self.end_B = self.start_B
- if len(self.cones_left) is 0 or len(self.cones_right) is 0:
+ if len(self.cones_left) == 0 or len(self.cones_right) == 0:
return
polygon_left = Polygon(self.cones_left)
polygon_right = Polygon(self.cones_right)
@@ -213,5 +213,5 @@ def callback_track(self, data):
else:
self.polygon_outside = polygon_left
self.polygon_inside = polygon_right
-
+
self.received_track = True
diff --git a/fssim_continuous_integration/rosdep/fssim-dependencies.yaml b/fssim_continuous_integration/rosdep/fssim-dependencies.yaml
index 197c08d..b1e94aa 100644
--- a/fssim_continuous_integration/rosdep/fssim-dependencies.yaml
+++ b/fssim_continuous_integration/rosdep/fssim-dependencies.yaml
@@ -1,20 +1,20 @@
fssim-python-wstool:
- ubuntu: [python-wstool]
+ ubuntu: [python3-wstool]
fssim-python-pip:
- ubuntu: [python-pip]
+ ubuntu: [python3-pip]
fssim-python-catkin-tools:
- ubuntu: [python-catkin-tools]
+ ubuntu: [python3-catkin-tools]
fssim-python-rospkg:
- ubuntu: [python-rospkg]
+ ubuntu: [python3-rospkg]
fssim-python-empy:
- ubuntu: [python-empy]
+ ubuntu: [python3-empy]
fssim-pcl-ros:
- ubuntu: [ros-kinetic-pcl-ros]
+ ubuntu: [ros-noetic-pcl-ros]
fssim-xacro:
- ubuntu: [ros-kinetic-xacro]
+ ubuntu: [ros-noetic-xacro]
fssim-shapely:
- ubuntu: [python-shapely]
+ ubuntu: [python3-shapely]
fssim-subprocess32:
ubuntu: [python-subprocess32]
fsssim-python-lxml:
- ubuntu: [python-lxml]
+ ubuntu: [python3-lxml]
diff --git a/fssim_description/scripts/load_vehicle.py b/fssim_description/scripts/load_vehicle.py
index 67793e7..581f6f1 100755
--- a/fssim_description/scripts/load_vehicle.py
+++ b/fssim_description/scripts/load_vehicle.py
@@ -12,10 +12,10 @@ def load_robot_description():
model_filepath = rospy.get_param('/fssim/model_filepath')
- print sensors_config_file
- print car_config_file
- print car_dimensions_file
- print model_filepath
+ print(sensors_config_file)
+ print(car_config_file)
+ print(car_dimensions_file)
+ print(model_filepath)
try:
command_string = "rosrun xacro xacro --inorder {} robot_name:='{}' sensors_config_file:='{}' car_config_file:='{}' car_dimensions_file:='{}".format(model_filepath,
diff --git a/fssim_gazebo/models/track/FSG.sdf b/fssim_gazebo/models/track/FSG.sdf
index 886bd71..a559acf 100644
--- a/fssim_gazebo/models/track/FSG.sdf
+++ b/fssim_gazebo/models/track/FSG.sdf
@@ -1,945 +1,944 @@
-
model://fssim_gazebo/models/cone_blue
-1.20535457134 -2.43421959877 0 0 0 0
- cone_right
+ cone_right0
model://fssim_gazebo/models/cone_blue
3.17810750008 -1.93232870102 0 0 0 0
- cone_right
+ cone_right1
model://fssim_gazebo/models/cone_blue
7.48558950424 -1.81353652477 0 0 0 0
- cone_right
+ cone_right2
model://fssim_gazebo/models/cone_blue
12.1316843033 -1.68396937847 0 0 0 0
- cone_right
+ cone_right3
model://fssim_gazebo/models/cone_blue
16.790725708 -1.63397860527 0 0 0 0
- cone_right
+ cone_right4
model://fssim_gazebo/models/cone_blue
21.0159664154 -1.57767641544 0 0 0 0
- cone_right
+ cone_right5
model://fssim_gazebo/models/cone_blue
25.9492740631 -1.68443393707 0 0 0 0
- cone_right
+ cone_right6
model://fssim_gazebo/models/cone_blue
29.0480709076 -1.72887885571 0 0 0 0
- cone_right
+ cone_right7
model://fssim_gazebo/models/cone_blue
32.6119613647 -1.83696222305 0 0 0 0
- cone_right
+ cone_right8
model://fssim_gazebo/models/cone_blue
35.9588241577 -2.02518367767 0 0 0 0
- cone_right
+ cone_right9
model://fssim_gazebo/models/cone_blue
39.9082717896 -3.04691457748 0 0 0 0
- cone_right
+ cone_right10
model://fssim_gazebo/models/cone_blue
42.1978797913 -5.7311501503 0 0 0 0
- cone_right
+ cone_right11
model://fssim_gazebo/models/cone_blue
42.643951416 -7.45758581161 0 0 0 0
- cone_right
+ cone_right12
model://fssim_gazebo/models/cone_blue
42.6222114563 -9.84560966492 0 0 0 0
- cone_right
+ cone_right13
model://fssim_gazebo/models/cone_blue
42.524066925 -11.4708385468 0 0 0 0
- cone_right
+ cone_right14
model://fssim_gazebo/models/cone_blue
42.1597290039 -13.4860305786 0 0 0 0
- cone_right
+ cone_right15
model://fssim_gazebo/models/cone_blue
41.4561500549 -15.1818876266 0 0 0 0
- cone_right
+ cone_right16
model://fssim_gazebo/models/cone_blue
40.5325622559 -16.8797531128 0 0 0 0
- cone_right
+ cone_right17
model://fssim_gazebo/models/cone_blue
38.5180549622 -18.9988632202 0 0 0 0
- cone_right
+ cone_right18
model://fssim_gazebo/models/cone_blue
36.2039756775 -20.470911026 0 0 0 0
- cone_right
+ cone_right19
model://fssim_gazebo/models/cone_blue
32.7586708069 -21.9738197327 0 0 0 0
- cone_right
+ cone_right20
model://fssim_gazebo/models/cone_blue
28.4735565186 -23.48179245 0 0 0 0
- cone_right
+ cone_right21
model://fssim_gazebo/models/cone_blue
25.4437217712 -24.171453476 0 0 0 0
- cone_right
+ cone_right22
model://fssim_gazebo/models/cone_blue
22.9642963409 -24.4952335358 0 0 0 0
- cone_right
+ cone_right23
model://fssim_gazebo/models/cone_blue
18.7935180664 -23.4676418304 0 0 0 0
- cone_right
+ cone_right24
model://fssim_gazebo/models/cone_blue
16.1738128662 -21.4524841309 0 0 0 0
- cone_right
+ cone_right25
model://fssim_gazebo/models/cone_blue
13.5638856888 -18.8669128418 0 0 0 0
- cone_right
+ cone_right26
model://fssim_gazebo/models/cone_blue
11.8896331787 -17.3642730713 0 0 0 0
- cone_right
+ cone_right27
model://fssim_gazebo/models/cone_blue
10.3036079407 -16.0993785858 0 0 0 0
- cone_right
+ cone_right28
model://fssim_gazebo/models/cone_blue
6.79945707321 -13.8660707474 0 0 0 0
- cone_right
+ cone_right29
model://fssim_gazebo/models/cone_blue
4.32830381393 -13.5115633011 0 0 0 0
- cone_right
+ cone_right30
model://fssim_gazebo/models/cone_blue
1.12301874161 -13.7139015198 0 0 0 0
- cone_right
+ cone_right31
model://fssim_gazebo/models/cone_blue
-2.75382375717 -14.1658267975 0 0 0 0
- cone_right
+ cone_right32
model://fssim_gazebo/models/cone_blue
-4.81454324722 -15.6764669418 0 0 0 0
- cone_right
+ cone_right33
model://fssim_gazebo/models/cone_blue
-7.15268754959 -19.0835819244 0 0 0 0
- cone_right
+ cone_right34
model://fssim_gazebo/models/cone_blue
-8.18414115906 -23.2163944244 0 0 0 0
- cone_right
+ cone_right35
model://fssim_gazebo/models/cone_blue
-7.90778493881 -27.239025116 0 0 0 0
- cone_right
+ cone_right36
model://fssim_gazebo/models/cone_blue
-7.00891208649 -31.0450992584 0 0 0 0
- cone_right
+ cone_right37
model://fssim_gazebo/models/cone_blue
-8.42116451263 -34.9212493896 0 0 0 0
- cone_right
+ cone_right38
model://fssim_gazebo/models/cone_blue
-10.3438024521 -39.3102645874 0 0 0 0
- cone_right
+ cone_right39
model://fssim_gazebo/models/cone_blue
-9.21577548981 -43.814994812 0 0 0 0
- cone_right
+ cone_right40
model://fssim_gazebo/models/cone_blue
-6.92036342621 -47.6307487488 0 0 0 0
- cone_right
+ cone_right41
model://fssim_gazebo/models/cone_blue
-4.60597276688 -50.5477561951 0 0 0 0
- cone_right
+ cone_right42
model://fssim_gazebo/models/cone_blue
-2.35273480415 -51.8892745972 0 0 0 0
- cone_right
+ cone_right43
model://fssim_gazebo/models/cone_blue
-0.165838509798 -52.8908081055 0 0 0 0
- cone_right
+ cone_right44
model://fssim_gazebo/models/cone_blue
2.04115414619 -53.2722854614 0 0 0 0
- cone_right
+ cone_right45
model://fssim_gazebo/models/cone_blue
4.15739202499 -53.3649635315 0 0 0 0
- cone_right
+ cone_right46
model://fssim_gazebo/models/cone_blue
8.75176334381 -53.057434082 0 0 0 0
- cone_right
+ cone_right47
model://fssim_gazebo/models/cone_blue
12.9969034195 -52.1645202637 0 0 0 0
- cone_right
+ cone_right48
model://fssim_gazebo/models/cone_blue
16.4463691711 -52.5517196655 0 0 0 0
- cone_right
+ cone_right49
model://fssim_gazebo/models/cone_blue
19.5894641876 -54.8799247742 0 0 0 0
- cone_right
+ cone_right50
model://fssim_gazebo/models/cone_blue
21.4045848846 -57.2886390686 0 0 0 0
- cone_right
+ cone_right51
model://fssim_gazebo/models/cone_blue
25.2624359131 -63.3929138184 0 0 0 0
- cone_right
+ cone_right52
model://fssim_gazebo/models/cone_blue
26.2041530609 -65.9767227173 0 0 0 0
- cone_right
+ cone_right53
model://fssim_gazebo/models/cone_blue
26.14752388 -68.3555679321 0 0 0 0
- cone_right
+ cone_right54
model://fssim_gazebo/models/cone_blue
24.7101478577 -69.9159011841 0 0 0 0
- cone_right
+ cone_right55
model://fssim_gazebo/models/cone_blue
21.1220054626 -70.2595901489 0 0 0 0
- cone_right
+ cone_right56
model://fssim_gazebo/models/cone_blue
16.8929786682 -69.4648895264 0 0 0 0
- cone_right
+ cone_right57
model://fssim_gazebo/models/cone_blue
12.5505523682 -68.4203567505 0 0 0 0
- cone_right
+ cone_right58
model://fssim_gazebo/models/cone_blue
8.61410617828 -67.5245056152 0 0 0 0
- cone_right
+ cone_right59
model://fssim_gazebo/models/cone_blue
5.1298866272 -66.6364212036 0 0 0 0
- cone_right
+ cone_right60
model://fssim_gazebo/models/cone_blue
1.29183077812 -65.5188293457 0 0 0 0
- cone_right
+ cone_right61
model://fssim_gazebo/models/cone_blue
-0.911457359791 -64.0004882812 0 0 0 0
- cone_right
+ cone_right62
model://fssim_gazebo/models/cone_blue
-3.19275069237 -61.7590522766 0 0 0 0
- cone_right
+ cone_right63
model://fssim_gazebo/models/cone_blue
-4.88976144791 -59.9218444824 0 0 0 0
- cone_right
+ cone_right64
model://fssim_gazebo/models/cone_blue
-7.34266805649 -57.3790779114 0 0 0 0
- cone_right
+ cone_right65
model://fssim_gazebo/models/cone_blue
-9.3245267868 -55.36485672 0 0 0 0
- cone_right
+ cone_right66
model://fssim_gazebo/models/cone_blue
-11.9270553589 -52.5858802795 0 0 0 0
- cone_right
+ cone_right67
model://fssim_gazebo/models/cone_blue
-13.9907217026 -50.4050865173 0 0 0 0
- cone_right
+ cone_right68
model://fssim_gazebo/models/cone_blue
-15.8556175232 -48.291885376 0 0 0 0
- cone_right
+ cone_right69
model://fssim_gazebo/models/cone_blue
-17.4213695526 -46.3403968811 0 0 0 0
- cone_right
+ cone_right70
model://fssim_gazebo/models/cone_blue
-18.9634799957 -44.4720039368 0 0 0 0
- cone_right
+ cone_right71
model://fssim_gazebo/models/cone_blue
-20.8690338135 -41.1217308044 0 0 0 0
- cone_right
+ cone_right72
model://fssim_gazebo/models/cone_blue
-21.7830982208 -39.5535202026 0 0 0 0
- cone_right
+ cone_right73
model://fssim_gazebo/models/cone_blue
-22.1184329987 -36.9808921814 0 0 0 0
- cone_right
+ cone_right74
model://fssim_gazebo/models/cone_blue
-22.3782291412 -33.3755264282 0 0 0 0
- cone_right
+ cone_right75
model://fssim_gazebo/models/cone_blue
-22.2891635895 -29.8054790497 0 0 0 0
- cone_right
+ cone_right76
model://fssim_gazebo/models/cone_blue
-21.6834945679 -25.6726341248 0 0 0 0
- cone_right
+ cone_right77
model://fssim_gazebo/models/cone_blue
-20.8396205902 -21.7996044159 0 0 0 0
- cone_right
+ cone_right78
model://fssim_gazebo/models/cone_blue
-19.7338562012 -18.0672054291 0 0 0 0
- cone_right
+ cone_right79
model://fssim_gazebo/models/cone_blue
-18.7997817993 -14.3690509796 0 0 0 0
- cone_right
+ cone_right80
model://fssim_gazebo/models/cone_blue
-17.489906311 -10.8447647095 0 0 0 0
- cone_right
+ cone_right81
model://fssim_gazebo/models/cone_blue
-15.5611610413 -8.86956310272 0 0 0 0
- cone_right
+ cone_right82
model://fssim_gazebo/models/cone_blue
-13.9415597916 -7.27876806259 0 0 0 0
- cone_right
+ cone_right83
model://fssim_gazebo/models/cone_blue
-11.831618309 -5.82119607925 0 0 0 0
- cone_right
+ cone_right84
model://fssim_gazebo/models/cone_blue
-9.60293102264 -4.78612518311 0 0 0 0
- cone_right
+ cone_right85
model://fssim_gazebo/models/cone_blue
-6.35780382156 -3.627846241 0 0 0 0
- cone_right
+ cone_right86
model://fssim_gazebo/models/cone_blue
-3.79635858536 -2.90468358994 0 0 0 0
- cone_right
+ cone_right87
model://fssim_gazebo/models/cone_blue
-1.20535457134 -2.43421959877 0 0 0 0
- cone_right
+ cone_right88
model://fssim_gazebo/models/cone_yellow
-1.76674330235 1.47030568123 0 0 0 0
- cone_left
+ cone_left0
model://fssim_gazebo/models/cone_yellow
2.76089644432 1.7153673172 0 0 0 0
- cone_left
+ cone_left1
model://fssim_gazebo/models/cone_yellow
7.27864980698 1.72703278065 0 0 0 0
- cone_left
+ cone_left2
model://fssim_gazebo/models/cone_yellow
12.0349731445 1.77701556683 0 0 0 0
- cone_left
+ cone_left3
model://fssim_gazebo/models/cone_yellow
16.5392436981 1.85751795769 0 0 0 0
- cone_left
+ cone_left4
model://fssim_gazebo/models/cone_yellow
21.0335502625 1.90579783916 0 0 0 0
- cone_left
+ cone_left5
model://fssim_gazebo/models/cone_yellow
25.8245449066 1.8236978054 0 0 0 0
- cone_left
+ cone_left6
model://fssim_gazebo/models/cone_yellow
29.008146286 1.6881827116 0 0 0 0
- cone_left
+ cone_left7
model://fssim_gazebo/models/cone_yellow
32.8539886475 1.53236305714 0 0 0 0
- cone_left
+ cone_left8
model://fssim_gazebo/models/cone_yellow
36.0541305542 1.40598678589 0 0 0 0
- cone_left
+ cone_left9
model://fssim_gazebo/models/cone_yellow
39.6403274536 1.33292233944 0 0 0 0
- cone_left
+ cone_left10
model://fssim_gazebo/models/cone_yellow
41.8411369324 0.461700230837 0 0 0 0
- cone_left
+ cone_left11
model://fssim_gazebo/models/cone_yellow
43.7949371338 -0.824330449104 0 0 0 0
- cone_left
+ cone_left12
model://fssim_gazebo/models/cone_yellow
45.3121299744 -2.6312391758 0 0 0 0
- cone_left
+ cone_left13
model://fssim_gazebo/models/cone_yellow
46.1553916931 -4.62501859665 0 0 0 0
- cone_left
+ cone_left14
model://fssim_gazebo/models/cone_yellow
46.4220657349 -7.10254859924 0 0 0 0
- cone_left
+ cone_left15
model://fssim_gazebo/models/cone_yellow
46.5321502686 -9.60714149475 0 0 0 0
- cone_left
+ cone_left16
model://fssim_gazebo/models/cone_yellow
46.3761138916 -11.952504158 0 0 0 0
- cone_left
+ cone_left17
model://fssim_gazebo/models/cone_yellow
45.840171814 -14.4087295532 0 0 0 0
- cone_left
+ cone_left18
model://fssim_gazebo/models/cone_yellow
45.0923805237 -16.5197753906 0 0 0 0
- cone_left
+ cone_left19
model://fssim_gazebo/models/cone_yellow
43.6631126404 -19.0952739716 0 0 0 0
- cone_left
+ cone_left20
model://fssim_gazebo/models/cone_yellow
41.3715438843 -21.7454833984 0 0 0 0
- cone_left
+ cone_left21
model://fssim_gazebo/models/cone_yellow
37.8901481628 -24.0829868317 0 0 0 0
- cone_left
+ cone_left22
model://fssim_gazebo/models/cone_yellow
34.6198616028 -25.5377349854 0 0 0 0
- cone_left
+ cone_left23
model://fssim_gazebo/models/cone_yellow
31.2377872467 -26.407459259 0 0 0 0
- cone_left
+ cone_left24
model://fssim_gazebo/models/cone_yellow
27.4351100922 -27.5946540833 0 0 0 0
- cone_left
+ cone_left25
model://fssim_gazebo/models/cone_yellow
24.4867210388 -28.2711963654 0 0 0 0
- cone_left
+ cone_left26
model://fssim_gazebo/models/cone_yellow
21.9398899078 -28.5392894745 0 0 0 0
- cone_left
+ cone_left27
model://fssim_gazebo/models/cone_yellow
17.7762260437 -27.4994373322 0 0 0 0
- cone_left
+ cone_left28
model://fssim_gazebo/models/cone_yellow
14.9727420807 -25.1546077728 0 0 0 0
- cone_left
+ cone_left29
model://fssim_gazebo/models/cone_yellow
13.4640274048 -23.7030353546 0 0 0 0
- cone_left
+ cone_left30
model://fssim_gazebo/models/cone_yellow
11.3517541885 -21.9376277924 0 0 0 0
- cone_left
+ cone_left31
model://fssim_gazebo/models/cone_yellow
9.28481960297 -20.3903141022 0 0 0 0
- cone_left
+ cone_left32
model://fssim_gazebo/models/cone_yellow
7.48336029053 -18.9196147919 0 0 0 0
- cone_left
+ cone_left33
model://fssim_gazebo/models/cone_yellow
4.77595758438 -17.4981155396 0 0 0 0
- cone_left
+ cone_left34
model://fssim_gazebo/models/cone_yellow
1.28759145737 -17.3332099915 0 0 0 0
- cone_left
+ cone_left35
model://fssim_gazebo/models/cone_yellow
-1.63388907909 -18.2758922577 0 0 0 0
- cone_left
+ cone_left36
model://fssim_gazebo/models/cone_yellow
-3.17948746681 -21.7290554047 0 0 0 0
- cone_left
+ cone_left37
model://fssim_gazebo/models/cone_yellow
-3.27370977402 -25.0204906464 0 0 0 0
- cone_left
+ cone_left38
model://fssim_gazebo/models/cone_yellow
-3.07798027992 -27.4282741547 0 0 0 0
- cone_left
+ cone_left39
model://fssim_gazebo/models/cone_yellow
-2.77587771416 -30.082868576 0 0 0 0
- cone_left
+ cone_left40
model://fssim_gazebo/models/cone_yellow
-3.4180958271 -34.0824127197 0 0 0 0
- cone_left
+ cone_left41
model://fssim_gazebo/models/cone_yellow
-5.1704211235 -37.1482276917 0 0 0 0
- cone_left
+ cone_left42
model://fssim_gazebo/models/cone_yellow
-6.55036306381 -40.3913154602 0 0 0 0
- cone_left
+ cone_left43
model://fssim_gazebo/models/cone_yellow
-5.8644695282 -42.8615951538 0 0 0 0
- cone_left
+ cone_left44
model://fssim_gazebo/models/cone_yellow
-4.37757587433 -44.9487495422 0 0 0 0
- cone_left
+ cone_left45
model://fssim_gazebo/models/cone_yellow
-2.17299032211 -47.4768180847 0 0 0 0
- cone_left
+ cone_left46
model://fssim_gazebo/models/cone_yellow
0.147873014212 -48.7639732361 0 0 0 0
- cone_left
+ cone_left47
model://fssim_gazebo/models/cone_yellow
4.08864498138 -48.8794441223 0 0 0 0
- cone_left
+ cone_left48
model://fssim_gazebo/models/cone_yellow
8.05480289459 -47.9160308838 0 0 0 0
- cone_left
+ cone_left49
model://fssim_gazebo/models/cone_yellow
11.9599332809 -47.6551475525 0 0 0 0
- cone_left
+ cone_left50
model://fssim_gazebo/models/cone_yellow
14.9592418671 -47.935005188 0 0 0 0
- cone_left
+ cone_left51
model://fssim_gazebo/models/cone_yellow
17.607585907 -48.6755485535 0 0 0 0
- cone_left
+ cone_left52
model://fssim_gazebo/models/cone_yellow
19.5705165863 -49.7755432129 0 0 0 0
- cone_left
+ cone_left53
model://fssim_gazebo/models/cone_yellow
22.6065139771 -52.1836853027 0 0 0 0
- cone_left
+ cone_left54
model://fssim_gazebo/models/cone_yellow
25.3403396606 -55.3910713196 0 0 0 0
- cone_left
+ cone_left55
model://fssim_gazebo/models/cone_yellow
27.5260028839 -58.7276992798 0 0 0 0
- cone_left
+ cone_left56
model://fssim_gazebo/models/cone_yellow
29.1637706757 -62.0333976746 0 0 0 0
- cone_left
+ cone_left57
model://fssim_gazebo/models/cone_yellow
30.1006793976 -65.7219696045 0 0 0 0
- cone_left
+ cone_left58
model://fssim_gazebo/models/cone_yellow
30.1847019196 -69.0223770142 0 0 0 0
- cone_left
+ cone_left59
model://fssim_gazebo/models/cone_yellow
29.283241272 -72.0737915039 0 0 0 0
- cone_left
+ cone_left60
model://fssim_gazebo/models/cone_yellow
26.9897003174 -73.9624557495 0 0 0 0
- cone_left
+ cone_left61
model://fssim_gazebo/models/cone_yellow
23.7649536133 -74.3958969116 0 0 0 0
- cone_left
+ cone_left62
model://fssim_gazebo/models/cone_yellow
19.9833869934 -74.2500610352 0 0 0 0
- cone_left
+ cone_left63
model://fssim_gazebo/models/cone_yellow
15.7009191513 -73.0300064087 0 0 0 0
- cone_left
+ cone_left64
model://fssim_gazebo/models/cone_yellow
11.5243768692 -71.9855651855 0 0 0 0
- cone_left
+ cone_left65
model://fssim_gazebo/models/cone_yellow
7.55422449112 -71.0143661499 0 0 0 0
- cone_left
+ cone_left66
model://fssim_gazebo/models/cone_yellow
3.99313831329 -70.1475296021 0 0 0 0
- cone_left
+ cone_left67
model://fssim_gazebo/models/cone_yellow
-0.0668102130294 -69.1028747559 0 0 0 0
- cone_left
+ cone_left68
model://fssim_gazebo/models/cone_yellow
-3.44227051735 -67.2309875488 0 0 0 0
- cone_left
+ cone_left69
model://fssim_gazebo/models/cone_yellow
-5.49040985107 -65.1119689941 0 0 0 0
- cone_left
+ cone_left70
model://fssim_gazebo/models/cone_yellow
-7.24591016769 -62.596157074 0 0 0 0
- cone_left
+ cone_left71
model://fssim_gazebo/models/cone_yellow
-9.36122608185 -60.3147392273 0 0 0 0
- cone_left
+ cone_left72
model://fssim_gazebo/models/cone_yellow
-11.5451803207 -58.3123054504 0 0 0 0
- cone_left
+ cone_left73
model://fssim_gazebo/models/cone_yellow
-14.4151830673 -54.9342651367 0 0 0 0
- cone_left
+ cone_left74
model://fssim_gazebo/models/cone_yellow
-16.5186710358 -52.5611877441 0 0 0 0
- cone_left
+ cone_left75
model://fssim_gazebo/models/cone_yellow
-18.5795021057 -50.1394462585 0 0 0 0
- cone_left
+ cone_left76
model://fssim_gazebo/models/cone_yellow
-21.8124217987 -46.6210861206 0 0 0 0
- cone_left
+ cone_left77
model://fssim_gazebo/models/cone_yellow
-24.7267017365 -43.475151062 0 0 0 0
- cone_left
+ cone_left78
model://fssim_gazebo/models/cone_yellow
-25.7503032684 -40.3981132507 0 0 0 0
- cone_left
+ cone_left79
model://fssim_gazebo/models/cone_yellow
-26.0476665497 -36.9700622559 0 0 0 0
- cone_left
+ cone_left80
model://fssim_gazebo/models/cone_yellow
-25.9315605164 -32.6193275452 0 0 0 0
- cone_left
+ cone_left81
model://fssim_gazebo/models/cone_yellow
-25.7263412476 -28.6441040039 0 0 0 0
- cone_left
+ cone_left82
model://fssim_gazebo/models/cone_yellow
-25.0013523102 -24.6762886047 0 0 0 0
- cone_left
+ cone_left83
model://fssim_gazebo/models/cone_yellow
-24.2922458649 -20.8632926941 0 0 0 0
- cone_left
+ cone_left84
model://fssim_gazebo/models/cone_yellow
-23.3050079346 -17.2035923004 0 0 0 0
- cone_left
+ cone_left85
model://fssim_gazebo/models/cone_yellow
-22.1317424774 -13.6423683167 0 0 0 0
- cone_left
+ cone_left86
model://fssim_gazebo/models/cone_yellow
-20.7885227203 -9.28058624268 0 0 0 0
- cone_left
+ cone_left87
model://fssim_gazebo/models/cone_yellow
-18.4632396698 -6.41253614426 0 0 0 0
- cone_left
+ cone_left88
model://fssim_gazebo/models/cone_yellow
-15.6022777557 -3.68699145317 0 0 0 0
- cone_left
+ cone_left89
model://fssim_gazebo/models/cone_yellow
-13.5745029449 -2.1660451889 0 0 0 0
- cone_left
+ cone_left90
model://fssim_gazebo/models/cone_yellow
-11.5146017075 -0.892862737179 0 0 0 0
- cone_left
+ cone_left91
model://fssim_gazebo/models/cone_yellow
-7.04339790344 0.440190732479 0 0 0 0
- cone_left
+ cone_left92
model://fssim_gazebo/models/cone_yellow
-4.10932731628 1.10956943035 0 0 0 0
- cone_left
+ cone_left93
model://fssim_gazebo/models/cone_yellow
-1.76674330235 1.47030568123 0 0 0 0
- cone_left
+ cone_left94
model://fssim_gazebo/models/cone_orange_big
4.7 2.5 0 0 0 0
- cone_orange_big
+ cone_orange_big0
model://fssim_gazebo/models/cone_orange_big
4.7 -2.5 0 0 0 0
- cone_orange_big
+ cone_orange_big1
model://fssim_gazebo/models/cone_orange_big
7.3 2.5 0 0 0 0
- cone_orange_big
+ cone_orange_big2
model://fssim_gazebo/models/cone_orange_big
7.3 -2.5 0 0 0 0
- cone_orange_big
+ cone_orange_big3
model://fssim_gazebo/models/time_keeping
@@ -952,4 +951,4 @@
tk_device_1
-
+
\ No newline at end of file
diff --git a/fssim_gazebo/models/track/FSI.sdf b/fssim_gazebo/models/track/FSI.sdf
index 9f8b865..c4a4c3f 100644
--- a/fssim_gazebo/models/track/FSI.sdf
+++ b/fssim_gazebo/models/track/FSI.sdf
@@ -1,800 +1,799 @@
-
model://fssim_gazebo/models/cone_blue
-0.540000021458 -2.65199995041 0 0 0 0
- cone_right
+ cone_right0
model://fssim_gazebo/models/cone_blue
2.48799991608 -1.80599999428 0 0 0 0
- cone_right
+ cone_right1
model://fssim_gazebo/models/cone_blue
4.67899990082 -1.60300004482 0 0 0 0
- cone_right
+ cone_right2
model://fssim_gazebo/models/cone_blue
6.97700023651 -1.37300002575 0 0 0 0
- cone_right
+ cone_right3
model://fssim_gazebo/models/cone_blue
9.12399959564 -1.38499999046 0 0 0 0
- cone_right
+ cone_right4
model://fssim_gazebo/models/cone_blue
9.92899990082 -1.57400000095 0 0 0 0
- cone_right
+ cone_right5
model://fssim_gazebo/models/cone_blue
13.6180000305 -1.21399998665 0 0 0 0
- cone_right
+ cone_right6
model://fssim_gazebo/models/cone_blue
17.1529998779 -1.06200003624 0 0 0 0
- cone_right
+ cone_right7
model://fssim_gazebo/models/cone_blue
20.672000885 -1.22099995613 0 0 0 0
- cone_right
+ cone_right8
model://fssim_gazebo/models/cone_blue
23.9090003967 -2.11500000954 0 0 0 0
- cone_right
+ cone_right9
model://fssim_gazebo/models/cone_blue
26.1359996796 -3.76300001144 0 0 0 0
- cone_right
+ cone_right10
model://fssim_gazebo/models/cone_blue
28.4850006104 -5.78900003433 0 0 0 0
- cone_right
+ cone_right11
model://fssim_gazebo/models/cone_blue
30.9979991913 -7.1939997673 0 0 0 0
- cone_right
+ cone_right12
model://fssim_gazebo/models/cone_blue
33.4580001831 -7.6139998436 0 0 0 0
- cone_right
+ cone_right13
model://fssim_gazebo/models/cone_blue
36.4329986572 -7.75 0 0 0 0
- cone_right
+ cone_right14
model://fssim_gazebo/models/cone_blue
39.3709983826 -7.52899980545 0 0 0 0
- cone_right
+ cone_right15
model://fssim_gazebo/models/cone_blue
42.0950012207 -7.88399982452 0 0 0 0
- cone_right
+ cone_right16
model://fssim_gazebo/models/cone_blue
43.8289985657 -9.10900020599 0 0 0 0
- cone_right
+ cone_right17
model://fssim_gazebo/models/cone_blue
44.061000824 -11.4799995422 0 0 0 0
- cone_right
+ cone_right18
model://fssim_gazebo/models/cone_blue
42.3660011292 -13.3009996414 0 0 0 0
- cone_right
+ cone_right19
model://fssim_gazebo/models/cone_blue
40.3650016785 -13.7299995422 0 0 0 0
- cone_right
+ cone_right20
model://fssim_gazebo/models/cone_blue
37.9630012512 -14.2019996643 0 0 0 0
- cone_right
+ cone_right21
model://fssim_gazebo/models/cone_blue
35.091999054 -14.6180000305 0 0 0 0
- cone_right
+ cone_right22
model://fssim_gazebo/models/cone_blue
32.5550003052 -15.0080003738 0 0 0 0
- cone_right
+ cone_right23
model://fssim_gazebo/models/cone_blue
31.8950004578 -14.732000351 0 0 0 0
- cone_right
+ cone_right24
model://fssim_gazebo/models/cone_blue
29.1480007172 -15.1739997864 0 0 0 0
- cone_right
+ cone_right25
model://fssim_gazebo/models/cone_blue
26.7929992676 -15.5839996338 0 0 0 0
- cone_right
+ cone_right26
model://fssim_gazebo/models/cone_blue
24.7269992828 -16.4740009308 0 0 0 0
- cone_right
+ cone_right27
model://fssim_gazebo/models/cone_blue
22.8250007629 -17.8600006104 0 0 0 0
- cone_right
+ cone_right28
model://fssim_gazebo/models/cone_blue
20.8500003815 -19.7590007782 0 0 0 0
- cone_right
+ cone_right29
model://fssim_gazebo/models/cone_blue
17.9470005035 -20.7770004272 0 0 0 0
- cone_right
+ cone_right30
model://fssim_gazebo/models/cone_blue
14.5600004196 -21.3239994049 0 0 0 0
- cone_right
+ cone_right31
model://fssim_gazebo/models/cone_blue
11.5360002518 -21.7299995422 0 0 0 0
- cone_right
+ cone_right32
model://fssim_gazebo/models/cone_blue
8.89700031281 -22.1539993286 0 0 0 0
- cone_right
+ cone_right33
model://fssim_gazebo/models/cone_blue
5.6139998436 -22.468000412 0 0 0 0
- cone_right
+ cone_right34
model://fssim_gazebo/models/cone_blue
1.72800004482 -22.7849998474 0 0 0 0
- cone_right
+ cone_right35
model://fssim_gazebo/models/cone_blue
-1.81500005722 -23.047000885 0 0 0 0
- cone_right
+ cone_right36
model://fssim_gazebo/models/cone_blue
-5.52099990845 -23.2689990997 0 0 0 0
- cone_right
+ cone_right37
model://fssim_gazebo/models/cone_blue
-9.17300033569 -23.4880008698 0 0 0 0
- cone_right
+ cone_right38
model://fssim_gazebo/models/cone_blue
-12.0869998932 -23.6790008545 0 0 0 0
- cone_right
+ cone_right39
model://fssim_gazebo/models/cone_blue
-15.0380001068 -22.7900009155 0 0 0 0
- cone_right
+ cone_right40
model://fssim_gazebo/models/cone_blue
-17.5669994354 -21.2730007172 0 0 0 0
- cone_right
+ cone_right41
model://fssim_gazebo/models/cone_blue
-19.3540000916 -19.4440002441 0 0 0 0
- cone_right
+ cone_right42
model://fssim_gazebo/models/cone_blue
-21.0060005188 -17.5510005951 0 0 0 0
- cone_right
+ cone_right43
model://fssim_gazebo/models/cone_blue
-22.7240009308 -15.8210000992 0 0 0 0
- cone_right
+ cone_right44
model://fssim_gazebo/models/cone_blue
-24.5990009308 -14.1979999542 0 0 0 0
- cone_right
+ cone_right45
model://fssim_gazebo/models/cone_blue
-26.9249992371 -12.501999855 0 0 0 0
- cone_right
+ cone_right46
model://fssim_gazebo/models/cone_blue
-29.311000824 -11.6929998398 0 0 0 0
- cone_right
+ cone_right47
model://fssim_gazebo/models/cone_blue
-31.783000946 -11.5489997864 0 0 0 0
- cone_right
+ cone_right48
model://fssim_gazebo/models/cone_blue
-34.0820007324 -12.2559995651 0 0 0 0
- cone_right
+ cone_right49
model://fssim_gazebo/models/cone_blue
-36.3359985352 -13.2209997177 0 0 0 0
- cone_right
+ cone_right50
model://fssim_gazebo/models/cone_blue
-39.2430000305 -14.1700000763 0 0 0 0
- cone_right
+ cone_right51
model://fssim_gazebo/models/cone_blue
-42.5760002136 -13.9759998322 0 0 0 0
- cone_right
+ cone_right52
model://fssim_gazebo/models/cone_blue
-44.625 -12.6990003586 0 0 0 0
- cone_right
+ cone_right53
model://fssim_gazebo/models/cone_blue
-45.9099998474 -10.3050003052 0 0 0 0
- cone_right
+ cone_right54
model://fssim_gazebo/models/cone_blue
-45.936000824 -7.51599979401 0 0 0 0
- cone_right
+ cone_right55
model://fssim_gazebo/models/cone_blue
-45.0180015564 -5.19299983978 0 0 0 0
- cone_right
+ cone_right56
model://fssim_gazebo/models/cone_blue
-43.327999115 -3.99399995804 0 0 0 0
- cone_right
+ cone_right57
model://fssim_gazebo/models/cone_blue
-41.0579986572 -3.13800001144 0 0 0 0
- cone_right
+ cone_right58
model://fssim_gazebo/models/cone_blue
-38.0559997559 -2.77900004387 0 0 0 0
- cone_right
+ cone_right59
model://fssim_gazebo/models/cone_blue
-35.5419998169 -2.63100004196 0 0 0 0
- cone_right
+ cone_right60
model://fssim_gazebo/models/cone_blue
-32.5849990845 -3.24799990654 0 0 0 0
- cone_right
+ cone_right61
model://fssim_gazebo/models/cone_blue
-30.0990009308 -4.16200017929 0 0 0 0
- cone_right
+ cone_right62
model://fssim_gazebo/models/cone_blue
-27.093000412 -5.53499984741 0 0 0 0
- cone_right
+ cone_right63
model://fssim_gazebo/models/cone_blue
-24.1639995575 -7.27400016785 0 0 0 0
- cone_right
+ cone_right64
model://fssim_gazebo/models/cone_blue
-22.2089996338 -8.57400035858 0 0 0 0
- cone_right
+ cone_right65
model://fssim_gazebo/models/cone_blue
-20.1049995422 -10.0129995346 0 0 0 0
- cone_right
+ cone_right66
model://fssim_gazebo/models/cone_blue
-17.4720001221 -11.529999733 0 0 0 0
- cone_right
+ cone_right67
model://fssim_gazebo/models/cone_blue
-14.6750001907 -12.5710000992 0 0 0 0
- cone_right
+ cone_right68
model://fssim_gazebo/models/cone_blue
-11.7889995575 -12.5520000458 0 0 0 0
- cone_right
+ cone_right69
model://fssim_gazebo/models/cone_blue
-9.25199985504 -11.5089998245 0 0 0 0
- cone_right
+ cone_right70
model://fssim_gazebo/models/cone_blue
-7.12799978256 -9.54300022125 0 0 0 0
- cone_right
+ cone_right71
model://fssim_gazebo/models/cone_blue
-5.63399982452 -7.48799991608 0 0 0 0
- cone_right
+ cone_right72
model://fssim_gazebo/models/cone_blue
-3.56699991226 -5.01000022888 0 0 0 0
- cone_right
+ cone_right73
model://fssim_gazebo/models/cone_blue
-0.540000021458 -2.65199995041 0 0 0 0
- cone_right
+ cone_right74
model://fssim_gazebo/models/cone_yellow
-50.1940002441 -8.58199977875 0 0 0 0
- cone_left
+ cone_left0
model://fssim_gazebo/models/cone_yellow
-49.8790016174 -5.79799985886 0 0 0 0
- cone_left
+ cone_left1
model://fssim_gazebo/models/cone_yellow
-48.7550010681 -3.35299992561 0 0 0 0
- cone_left
+ cone_left2
model://fssim_gazebo/models/cone_yellow
-47.0410003662 -1.51600003242 0 0 0 0
- cone_left
+ cone_left3
model://fssim_gazebo/models/cone_yellow
-45.0699996948 -0.0130000002682 0 0 0 0
- cone_left
+ cone_left4
model://fssim_gazebo/models/cone_yellow
-42.5289993286 0.818000018597 0 0 0 0
- cone_left
+ cone_left5
model://fssim_gazebo/models/cone_yellow
-39.5909996033 1.18099999428 0 0 0 0
- cone_left
+ cone_left6
model://fssim_gazebo/models/cone_yellow
-36.0509986877 1.08000004292 0 0 0 0
- cone_left
+ cone_left7
model://fssim_gazebo/models/cone_yellow
-32.7799987793 0.856999993324 0 0 0 0
- cone_left
+ cone_left8
model://fssim_gazebo/models/cone_yellow
-29.3479995728 -0.268000006676 0 0 0 0
- cone_left
+ cone_left9
model://fssim_gazebo/models/cone_yellow
-26.9869995117 -1.58099997044 0 0 0 0
- cone_left
+ cone_left10
model://fssim_gazebo/models/cone_yellow
-24.4120006561 -3.36100006104 0 0 0 0
- cone_left
+ cone_left11
model://fssim_gazebo/models/cone_yellow
-21.9260005951 -4.92199993134 0 0 0 0
- cone_left
+ cone_left12
model://fssim_gazebo/models/cone_yellow
-19.7220001221 -6.30800008774 0 0 0 0
- cone_left
+ cone_left13
model://fssim_gazebo/models/cone_yellow
-16.5879993439 -7.80499982834 0 0 0 0
- cone_left
+ cone_left14
model://fssim_gazebo/models/cone_yellow
-13.9879999161 -8.64500045776 0 0 0 0
- cone_left
+ cone_left15
model://fssim_gazebo/models/cone_yellow
-11.5509996414 -7.89099979401 0 0 0 0
- cone_left
+ cone_left16
model://fssim_gazebo/models/cone_yellow
-9.34500026703 -6.11800003052 0 0 0 0
- cone_left
+ cone_left17
model://fssim_gazebo/models/cone_yellow
-7.46099996567 -3.76799988747 0 0 0 0
- cone_left
+ cone_left18
model://fssim_gazebo/models/cone_yellow
-5.46000003815 -1.6779999733 0 0 0 0
- cone_left
+ cone_left19
model://fssim_gazebo/models/cone_yellow
-3.23399996758 -0.0450000017881 0 0 0 0
- cone_left
+ cone_left20
model://fssim_gazebo/models/cone_yellow
-0.532000005245 1.24800002575 0 0 0 0
- cone_left
+ cone_left21
model://fssim_gazebo/models/cone_yellow
2.52800011635 2.00900006294 0 0 0 0
- cone_left
+ cone_left22
model://fssim_gazebo/models/cone_yellow
4.77899980545 2.0 0 0 0 0
- cone_left
+ cone_left23
model://fssim_gazebo/models/cone_yellow
7.19899988174 1.93900001049 0 0 0 0
- cone_left
+ cone_left24
model://fssim_gazebo/models/cone_yellow
8.77000045776 2.13400006294 0 0 0 0
- cone_left
+ cone_left25
model://fssim_gazebo/models/cone_yellow
12.6000003815 2.23699998856 0 0 0 0
- cone_left
+ cone_left26
model://fssim_gazebo/models/cone_yellow
16.2380008698 2.32899999619 0 0 0 0
- cone_left
+ cone_left27
model://fssim_gazebo/models/cone_yellow
19.2479991913 2.46000003815 0 0 0 0
- cone_left
+ cone_left28
model://fssim_gazebo/models/cone_yellow
22.4549999237 2.34299993515 0 0 0 0
- cone_left
+ cone_left29
model://fssim_gazebo/models/cone_yellow
25.1560001373 1.30400002003 0 0 0 0
- cone_left
+ cone_left30
model://fssim_gazebo/models/cone_yellow
27.4260005951 -0.435000002384 0 0 0 0
- cone_left
+ cone_left31
model://fssim_gazebo/models/cone_yellow
29.4349994659 -1.89600002766 0 0 0 0
- cone_left
+ cone_left32
model://fssim_gazebo/models/cone_yellow
31.9449996948 -3.20300006866 0 0 0 0
- cone_left
+ cone_left33
model://fssim_gazebo/models/cone_yellow
34.5089988708 -3.55500006676 0 0 0 0
- cone_left
+ cone_left34
model://fssim_gazebo/models/cone_yellow
37.3989982605 -3.77300000191 0 0 0 0
- cone_left
+ cone_left35
model://fssim_gazebo/models/cone_yellow
40.3209991455 -3.58699989319 0 0 0 0
- cone_left
+ cone_left36
model://fssim_gazebo/models/cone_yellow
43.3030014038 -3.77699995041 0 0 0 0
- cone_left
+ cone_left37
model://fssim_gazebo/models/cone_yellow
45.5550003052 -4.66800022125 0 0 0 0
- cone_left
+ cone_left38
model://fssim_gazebo/models/cone_yellow
47.2430000305 -6.21400022507 0 0 0 0
- cone_left
+ cone_left39
model://fssim_gazebo/models/cone_yellow
48.5320014954 -8.45600032806 0 0 0 0
- cone_left
+ cone_left40
model://fssim_gazebo/models/cone_yellow
49.0960006714 -10.5240001678 0 0 0 0
- cone_left
+ cone_left41
model://fssim_gazebo/models/cone_yellow
48.8979988098 -12.718000412 0 0 0 0
- cone_left
+ cone_left42
model://fssim_gazebo/models/cone_yellow
47.7949981689 -14.5920000076 0 0 0 0
- cone_left
+ cone_left43
model://fssim_gazebo/models/cone_yellow
46.4630012512 -15.9910001755 0 0 0 0
- cone_left
+ cone_left44
model://fssim_gazebo/models/cone_yellow
44.8180007935 -16.9599990845 0 0 0 0
- cone_left
+ cone_left45
model://fssim_gazebo/models/cone_yellow
42.8089981079 -17.5100002289 0 0 0 0
- cone_left
+ cone_left46
model://fssim_gazebo/models/cone_yellow
40.4049987793 -17.8840007782 0 0 0 0
- cone_left
+ cone_left47
model://fssim_gazebo/models/cone_yellow
37.7420005798 -18.1189994812 0 0 0 0
- cone_left
+ cone_left48
model://fssim_gazebo/models/cone_yellow
34.2439994812 -18.3899993896 0 0 0 0
- cone_left
+ cone_left49
model://fssim_gazebo/models/cone_yellow
30.6550006866 -18.7619991302 0 0 0 0
- cone_left
+ cone_left50
model://fssim_gazebo/models/cone_yellow
28.0669994354 -19.2759990692 0 0 0 0
- cone_left
+ cone_left51
model://fssim_gazebo/models/cone_yellow
26.0049991608 -20.466999054 0 0 0 0
- cone_left
+ cone_left52
model://fssim_gazebo/models/cone_yellow
23.9869995117 -22.0030002594 0 0 0 0
- cone_left
+ cone_left53
model://fssim_gazebo/models/cone_yellow
20.6539993286 -23.579000473 0 0 0 0
- cone_left
+ cone_left54
model://fssim_gazebo/models/cone_yellow
17.5090007782 -24.4430007935 0 0 0 0
- cone_left
+ cone_left55
model://fssim_gazebo/models/cone_yellow
14.4239997864 -24.767999649 0 0 0 0
- cone_left
+ cone_left56
model://fssim_gazebo/models/cone_yellow
10.3439998627 -25.2430000305 0 0 0 0
- cone_left
+ cone_left57
model://fssim_gazebo/models/cone_yellow
6.29099988937 -25.5650005341 0 0 0 0
- cone_left
+ cone_left58
model://fssim_gazebo/models/cone_yellow
1.7460000515 -26.0189990997 0 0 0 0
- cone_left
+ cone_left59
model://fssim_gazebo/models/cone_yellow
-2.79699993134 -26.4449996948 0 0 0 0
- cone_left
+ cone_left60
model://fssim_gazebo/models/cone_yellow
-6.82600021362 -26.8239994049 0 0 0 0
- cone_left
+ cone_left61
model://fssim_gazebo/models/cone_yellow
-11.0530004501 -27.2240009308 0 0 0 0
- cone_left
+ cone_left62
model://fssim_gazebo/models/cone_yellow
-14.1199998856 -26.9869995117 0 0 0 0
- cone_left
+ cone_left63
model://fssim_gazebo/models/cone_yellow
-16.6630001068 -26.0720005035 0 0 0 0
- cone_left
+ cone_left64
model://fssim_gazebo/models/cone_yellow
-18.7789993286 -25.0030002594 0 0 0 0
- cone_left
+ cone_left65
model://fssim_gazebo/models/cone_yellow
-20.3990001678 -23.5310001373 0 0 0 0
- cone_left
+ cone_left66
model://fssim_gazebo/models/cone_yellow
-21.7129993439 -21.936000824 0 0 0 0
- cone_left
+ cone_left67
model://fssim_gazebo/models/cone_yellow
-23.1319999695 -20.3180007935 0 0 0 0
- cone_left
+ cone_left68
model://fssim_gazebo/models/cone_yellow
-24.9260005951 -18.5909996033 0 0 0 0
- cone_left
+ cone_left69
model://fssim_gazebo/models/cone_yellow
-27.1989994049 -16.4270000458 0 0 0 0
- cone_left
+ cone_left70
model://fssim_gazebo/models/cone_yellow
-30.2660007477 -15.5310001373 0 0 0 0
- cone_left
+ cone_left71
model://fssim_gazebo/models/cone_yellow
-32.736000061 -15.843000412 0 0 0 0
- cone_left
+ cone_left72
model://fssim_gazebo/models/cone_yellow
-35.1360015869 -16.8859996796 0 0 0 0
- cone_left
+ cone_left73
model://fssim_gazebo/models/cone_yellow
-38.1879997253 -17.7479991913 0 0 0 0
- cone_left
+ cone_left74
model://fssim_gazebo/models/cone_yellow
-41.3409996033 -17.7150001526 0 0 0 0
- cone_left
+ cone_left75
model://fssim_gazebo/models/cone_yellow
-44.3079986572 -16.9549999237 0 0 0 0
- cone_left
+ cone_left76
model://fssim_gazebo/models/cone_yellow
-46.4230003357 -15.5129995346 0 0 0 0
- cone_left
+ cone_left77
model://fssim_gazebo/models/cone_yellow
-48.3009986877 -13.4919996262 0 0 0 0
- cone_left
+ cone_left78
model://fssim_gazebo/models/cone_yellow
-49.5219993591 -11.2259998322 0 0 0 0
- cone_left
+ cone_left79
model://fssim_gazebo/models/cone_orange_big
4.7 2.5 0 0 0 0
- cone_orange_big
+ cone_orange_big0
model://fssim_gazebo/models/cone_orange_big
4.7 -2.5 0 0 0 0
- cone_orange_big
+ cone_orange_big1
model://fssim_gazebo/models/cone_orange_big
7.3 2.5 0 0 0 0
- cone_orange_big
+ cone_orange_big2
model://fssim_gazebo/models/cone_orange_big
7.3 -2.5 0 0 0 0
- cone_orange_big
+ cone_orange_big3
model://fssim_gazebo/models/time_keeping
@@ -807,4 +806,4 @@
tk_device_1
-
+
\ No newline at end of file
diff --git a/fssim_gazebo/models/track/acceleration.sdf b/fssim_gazebo/models/track/acceleration.sdf
index 0e60c67..a8891af 100644
--- a/fssim_gazebo/models/track/acceleration.sdf
+++ b/fssim_gazebo/models/track/acceleration.sdf
@@ -1,395 +1,394 @@
-
model://fssim_gazebo/models/cone_blue
5.0 -1.9 0 0 0 0
- cone_right
+ cone_right0
model://fssim_gazebo/models/cone_blue
10.0 -1.9 0 0 0 0
- cone_right
+ cone_right1
model://fssim_gazebo/models/cone_blue
15.0 -1.9 0 0 0 0
- cone_right
+ cone_right2
model://fssim_gazebo/models/cone_blue
20.0 -1.9 0 0 0 0
- cone_right
+ cone_right3
model://fssim_gazebo/models/cone_blue
25.0 -1.9 0 0 0 0
- cone_right
+ cone_right4
model://fssim_gazebo/models/cone_blue
30.0 -1.9 0 0 0 0
- cone_right
+ cone_right5
model://fssim_gazebo/models/cone_blue
35.0 -1.9 0 0 0 0
- cone_right
+ cone_right6
model://fssim_gazebo/models/cone_blue
40.0 -1.9 0 0 0 0
- cone_right
+ cone_right7
model://fssim_gazebo/models/cone_blue
45.0 -1.9 0 0 0 0
- cone_right
+ cone_right8
model://fssim_gazebo/models/cone_blue
50.0 -1.9 0 0 0 0
- cone_right
+ cone_right9
model://fssim_gazebo/models/cone_blue
55.0 -1.9 0 0 0 0
- cone_right
+ cone_right10
model://fssim_gazebo/models/cone_blue
60.0 -1.9 0 0 0 0
- cone_right
+ cone_right11
model://fssim_gazebo/models/cone_blue
65.0 -1.9 0 0 0 0
- cone_right
+ cone_right12
model://fssim_gazebo/models/cone_blue
70.0 -1.9 0 0 0 0
- cone_right
+ cone_right13
model://fssim_gazebo/models/cone_yellow
5.0 1.9 0 0 0 0
- cone_left
+ cone_left0
model://fssim_gazebo/models/cone_yellow
10.0 1.9 0 0 0 0
- cone_left
+ cone_left1
model://fssim_gazebo/models/cone_yellow
15.0 1.9 0 0 0 0
- cone_left
+ cone_left2
model://fssim_gazebo/models/cone_yellow
20.0 1.9 0 0 0 0
- cone_left
+ cone_left3
model://fssim_gazebo/models/cone_yellow
25.0 1.9 0 0 0 0
- cone_left
+ cone_left4
model://fssim_gazebo/models/cone_yellow
30.0 1.9 0 0 0 0
- cone_left
+ cone_left5
model://fssim_gazebo/models/cone_yellow
35.0 1.9 0 0 0 0
- cone_left
+ cone_left6
model://fssim_gazebo/models/cone_yellow
40.0 1.9 0 0 0 0
- cone_left
+ cone_left7
model://fssim_gazebo/models/cone_yellow
45.0 1.9 0 0 0 0
- cone_left
+ cone_left8
model://fssim_gazebo/models/cone_yellow
50.0 1.9 0 0 0 0
- cone_left
+ cone_left9
model://fssim_gazebo/models/cone_yellow
55.0 1.9 0 0 0 0
- cone_left
+ cone_left10
model://fssim_gazebo/models/cone_yellow
60.0 1.9 0 0 0 0
- cone_left
+ cone_left11
model://fssim_gazebo/models/cone_yellow
65.0 1.9 0 0 0 0
- cone_left
+ cone_left12
model://fssim_gazebo/models/cone_yellow
70.0 1.9 0 0 0 0
- cone_left
+ cone_left13
model://fssim_gazebo/models/cone_orange
80.0 1.9 0 0 0 0
- cone_orange
+ cone_orange0
model://fssim_gazebo/models/cone_orange
80.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange1
model://fssim_gazebo/models/cone_orange
85.0 1.9 0 0 0 0
- cone_orange
+ cone_orange2
model://fssim_gazebo/models/cone_orange
85.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange3
model://fssim_gazebo/models/cone_orange
90.0 1.9 0 0 0 0
- cone_orange
+ cone_orange4
model://fssim_gazebo/models/cone_orange
90.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange5
model://fssim_gazebo/models/cone_orange
95.0 1.9 0 0 0 0
- cone_orange
+ cone_orange6
model://fssim_gazebo/models/cone_orange
95.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange7
model://fssim_gazebo/models/cone_orange
100.0 1.9 0 0 0 0
- cone_orange
+ cone_orange8
model://fssim_gazebo/models/cone_orange
100.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange9
model://fssim_gazebo/models/cone_orange
105.0 1.9 0 0 0 0
- cone_orange
+ cone_orange10
model://fssim_gazebo/models/cone_orange
105.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange11
model://fssim_gazebo/models/cone_orange
110.0 1.9 0 0 0 0
- cone_orange
+ cone_orange12
model://fssim_gazebo/models/cone_orange
110.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange13
model://fssim_gazebo/models/cone_orange
115.0 1.9 0 0 0 0
- cone_orange
+ cone_orange14
model://fssim_gazebo/models/cone_orange
115.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange15
model://fssim_gazebo/models/cone_orange
120.0 1.9 0 0 0 0
- cone_orange
+ cone_orange16
model://fssim_gazebo/models/cone_orange
120.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange17
model://fssim_gazebo/models/cone_orange
125.0 1.9 0 0 0 0
- cone_orange
+ cone_orange18
model://fssim_gazebo/models/cone_orange
125.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange19
model://fssim_gazebo/models/cone_orange
130.0 1.9 0 0 0 0
- cone_orange
+ cone_orange20
model://fssim_gazebo/models/cone_orange
130.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange21
model://fssim_gazebo/models/cone_orange
135.0 1.9 0 0 0 0
- cone_orange
+ cone_orange22
model://fssim_gazebo/models/cone_orange
135.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange23
model://fssim_gazebo/models/cone_orange
140.0 1.9 0 0 0 0
- cone_orange
+ cone_orange24
model://fssim_gazebo/models/cone_orange
140.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange25
model://fssim_gazebo/models/cone_orange
145.0 1.9 0 0 0 0
- cone_orange
+ cone_orange26
model://fssim_gazebo/models/cone_orange
145.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange27
model://fssim_gazebo/models/cone_orange
150.0 1.9 0 0 0 0
- cone_orange
+ cone_orange28
model://fssim_gazebo/models/cone_orange
150.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange29
model://fssim_gazebo/models/cone_orange
155.0 1.9 0 0 0 0
- cone_orange
+ cone_orange30
model://fssim_gazebo/models/cone_orange
155.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange31
model://fssim_gazebo/models/cone_orange
160.0 1.9 0 0 0 0
- cone_orange
+ cone_orange32
model://fssim_gazebo/models/cone_orange
160.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange33
model://fssim_gazebo/models/cone_orange
165.0 1.9 0 0 0 0
- cone_orange
+ cone_orange34
model://fssim_gazebo/models/cone_orange
165.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange35
model://fssim_gazebo/models/cone_orange
170.0 1.9 0 0 0 0
- cone_orange
+ cone_orange36
model://fssim_gazebo/models/cone_orange
170.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange37
model://fssim_gazebo/models/cone_orange
175.0 1.9 0 0 0 0
- cone_orange
+ cone_orange38
model://fssim_gazebo/models/cone_orange
175.0 -1.9 0 0 0 0
- cone_orange
+ cone_orange39
model://fssim_gazebo/models/cone_orange
175.0 0.0 0 0 0 0
- cone_orange
+ cone_orange40
model://fssim_gazebo/models/cone_orange
175.0 1.9 0 0 0 0
- cone_orange
+ cone_orange41
model://fssim_gazebo/models/cone_orange_big
-0.3 1.9 0 0 0 0
- cone_orange_big
+ cone_orange_big0
model://fssim_gazebo/models/cone_orange_big
-0.3 -1.9 0 0 0 0
- cone_orange_big
+ cone_orange_big1
model://fssim_gazebo/models/cone_orange_big
0.3 1.9 0 0 0 0
- cone_orange_big
+ cone_orange_big2
model://fssim_gazebo/models/cone_orange_big
0.3 -1.9 0 0 0 0
- cone_orange_big
+ cone_orange_big3
model://fssim_gazebo/models/cone_orange_big
74.7 1.9 0 0 0 0
- cone_orange_big
+ cone_orange_big4
model://fssim_gazebo/models/cone_orange_big
74.7 -1.9 0 0 0 0
- cone_orange_big
+ cone_orange_big5
model://fssim_gazebo/models/cone_orange_big
75.3 1.9 0 0 0 0
- cone_orange_big
+ cone_orange_big6
model://fssim_gazebo/models/cone_orange_big
75.3 -1.9 0 0 0 0
- cone_orange_big
+ cone_orange_big7
model://fssim_gazebo/models/time_keeping
@@ -412,4 +411,4 @@
tk_device_3
-
+
\ No newline at end of file
diff --git a/fssim_gazebo/models/track/backupFSG.sdf b/fssim_gazebo/models/track/backupFSG.sdf
new file mode 100644
index 0000000..bae3345
--- /dev/null
+++ b/fssim_gazebo/models/track/backupFSG.sdf
@@ -0,0 +1,1082 @@
+
+
+
+
+ model://fssim_gazebo/models/cone_blue
+ -1.20535457134 -2.43421959877 0 0 0 0
+ cone_right1
+
+
+ model://fssim_gazebo/models/cone_blue
+ 3.17810750008 -1.93232870102 0 0 0 0
+ cone_right2
+
+
+ model://fssim_gazebo/models/cone_blue
+ 7.48558950424 -1.81353652477 0 0 0 0
+ cone_right3
+
+
+ model://fssim_gazebo/models/cone_blue
+ 12.1316843033 -1.68396937847 0 0 0 0
+ cone_right4
+
+
+ model://fssim_gazebo/models/cone_blue
+ 16.790725708 -1.63397860527 0 0 0 0
+ cone_right5
+
+
+ model://fssim_gazebo/models/cone_blue
+ 21.0159664154 -1.57767641544 0 0 0 0
+ cone_right6
+
+
+ model://fssim_gazebo/models/cone_blue
+ 25.9492740631 -1.68443393707 0 0 0 0
+ cone_right7
+
+
+ model://fssim_gazebo/models/cone_blue
+ 29.0480709076 -1.72887885571 0 0 0 0
+ cone_right8
+
+
+ model://fssim_gazebo/models/cone_blue
+ 32.6119613647 -1.83696222305 0 0 0 0
+ cone_right9
+
+
+ model://fssim_gazebo/models/cone_blue
+ 35.9588241577 -2.02518367767 0 0 0 0
+ cone_right10
+
+
+ model://fssim_gazebo/models/cone_blue
+ 39.9082717896 -3.04691457748 0 0 0 0
+ cone_right11
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.1978797913 -5.7311501503 0 0 0 0
+ cone_right12
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.643951416 -7.45758581161 0 0 0 0
+ cone_right13
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.6222114563 -9.84560966492 0 0 0 0
+ cone_right14
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.524066925 -11.4708385468 0 0 0 0
+ cone_right15
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.1597290039 -13.4860305786 0 0 0 0
+ cone_right16
+
+
+ model://fssim_gazebo/models/cone_blue
+ 41.4561500549 -15.1818876266 0 0 0 0
+ cone_right17
+
+
+ model://fssim_gazebo/models/cone_blue
+ 40.5325622559 -16.8797531128 0 0 0 0
+ cone_right18
+
+
+ model://cone_orange_big
+ 4.7 2.5 0 0 0 0
+ cone_orange_big1
+
+
+ model://cone_orange_big
+ 4.7 -2.5 0 0 0 0
+ cone_orange_big2
+
+
+ model://cone_orange_big
+ 7.3 2.5 0 0 0 0
+ cone_orange_big3
+
+
+ model://cone_orange_big
+ 7.3 -2.5 0 0 0 0
+ cone_orange_big4
+
+
+ model://time_keeping
+ 6.0 3.0 0 0 0 0
+ tk_device_0
+
+
+ model://time_keeping
+ 6.0 -3.0 0 0 0 0
+ tk_device_1
+
+
+
+
+
+
+
+
+
+ model://fssim_gazebo/models/cone_blue
+ -1.20535457134 -2.43421959877 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 3.17810750008 -1.93232870102 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 7.48558950424 -1.81353652477 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 12.1316843033 -1.68396937847 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 16.790725708 -1.63397860527 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 21.0159664154 -1.57767641544 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 25.9492740631 -1.68443393707 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 29.0480709076 -1.72887885571 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 32.6119613647 -1.83696222305 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 35.9588241577 -2.02518367767 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 39.9082717896 -3.04691457748 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.1978797913 -5.7311501503 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.643951416 -7.45758581161 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.6222114563 -9.84560966492 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.524066925 -11.4708385468 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 42.1597290039 -13.4860305786 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 41.4561500549 -15.1818876266 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 40.5325622559 -16.8797531128 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 38.5180549622 -18.9988632202 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 36.2039756775 -20.470911026 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 32.7586708069 -21.9738197327 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 28.4735565186 -23.48179245 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 25.4437217712 -24.171453476 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 22.9642963409 -24.4952335358 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 18.7935180664 -23.4676418304 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 16.1738128662 -21.4524841309 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 13.5638856888 -18.8669128418 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 11.8896331787 -17.3642730713 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 10.3036079407 -16.0993785858 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 6.79945707321 -13.8660707474 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 4.32830381393 -13.5115633011 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 1.12301874161 -13.7139015198 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -2.75382375717 -14.1658267975 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -4.81454324722 -15.6764669418 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -7.15268754959 -19.0835819244 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -8.18414115906 -23.2163944244 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -7.90778493881 -27.239025116 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -7.00891208649 -31.0450992584 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -8.42116451263 -34.9212493896 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -10.3438024521 -39.3102645874 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -9.21577548981 -43.814994812 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -6.92036342621 -47.6307487488 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -4.60597276688 -50.5477561951 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -2.35273480415 -51.8892745972 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -0.165838509798 -52.8908081055 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 2.04115414619 -53.2722854614 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 4.15739202499 -53.3649635315 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 8.75176334381 -53.057434082 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 12.9969034195 -52.1645202637 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 16.4463691711 -52.5517196655 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 19.5894641876 -54.8799247742 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 21.4045848846 -57.2886390686 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 25.2624359131 -63.3929138184 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 26.2041530609 -65.9767227173 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 26.14752388 -68.3555679321 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 24.7101478577 -69.9159011841 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 21.1220054626 -70.2595901489 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 16.8929786682 -69.4648895264 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 12.5505523682 -68.4203567505 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 8.61410617828 -67.5245056152 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 5.1298866272 -66.6364212036 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ 1.29183077812 -65.5188293457 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -0.911457359791 -64.0004882812 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -3.19275069237 -61.7590522766 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -4.88976144791 -59.9218444824 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -7.34266805649 -57.3790779114 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -9.3245267868 -55.36485672 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -11.9270553589 -52.5858802795 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -13.9907217026 -50.4050865173 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -15.8556175232 -48.291885376 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -17.4213695526 -46.3403968811 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -18.9634799957 -44.4720039368 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -20.8690338135 -41.1217308044 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -21.7830982208 -39.5535202026 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -22.1184329987 -36.9808921814 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -22.3782291412 -33.3755264282 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -22.2891635895 -29.8054790497 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -21.6834945679 -25.6726341248 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -20.8396205902 -21.7996044159 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -19.7338562012 -18.0672054291 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -18.7997817993 -14.3690509796 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -17.489906311 -10.8447647095 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -15.5611610413 -8.86956310272 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -13.9415597916 -7.27876806259 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -11.831618309 -5.82119607925 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -9.60293102264 -4.78612518311 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -6.35780382156 -3.627846241 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -3.79635858536 -2.90468358994 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_blue
+ -1.20535457134 -2.43421959877 0 0 0 0
+ cone_right
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -1.76674330235 1.47030568123 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 2.76089644432 1.7153673172 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 7.27864980698 1.72703278065 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 12.0349731445 1.77701556683 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 16.5392436981 1.85751795769 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 21.0335502625 1.90579783916 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 25.8245449066 1.8236978054 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 29.008146286 1.6881827116 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 32.8539886475 1.53236305714 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 36.0541305542 1.40598678589 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 39.6403274536 1.33292233944 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 41.8411369324 0.461700230837 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 43.7949371338 -0.824330449104 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 45.3121299744 -2.6312391758 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 46.1553916931 -4.62501859665 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 46.4220657349 -7.10254859924 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 46.5321502686 -9.60714149475 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 46.3761138916 -11.952504158 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 45.840171814 -14.4087295532 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 45.0923805237 -16.5197753906 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 43.6631126404 -19.0952739716 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 41.3715438843 -21.7454833984 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 37.8901481628 -24.0829868317 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 34.6198616028 -25.5377349854 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 31.2377872467 -26.407459259 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 27.4351100922 -27.5946540833 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 24.4867210388 -28.2711963654 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 21.9398899078 -28.5392894745 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 17.7762260437 -27.4994373322 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 14.9727420807 -25.1546077728 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 13.4640274048 -23.7030353546 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 11.3517541885 -21.9376277924 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 9.28481960297 -20.3903141022 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 7.48336029053 -18.9196147919 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 4.77595758438 -17.4981155396 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 1.28759145737 -17.3332099915 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -1.63388907909 -18.2758922577 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -3.17948746681 -21.7290554047 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -3.27370977402 -25.0204906464 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -3.07798027992 -27.4282741547 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -2.77587771416 -30.082868576 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -3.4180958271 -34.0824127197 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -5.1704211235 -37.1482276917 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -6.55036306381 -40.3913154602 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -5.8644695282 -42.8615951538 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -4.37757587433 -44.9487495422 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -2.17299032211 -47.4768180847 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 0.147873014212 -48.7639732361 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 4.08864498138 -48.8794441223 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 8.05480289459 -47.9160308838 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 11.9599332809 -47.6551475525 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 14.9592418671 -47.935005188 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 17.607585907 -48.6755485535 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 19.5705165863 -49.7755432129 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 22.6065139771 -52.1836853027 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 25.3403396606 -55.3910713196 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 27.5260028839 -58.7276992798 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 29.1637706757 -62.0333976746 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 30.1006793976 -65.7219696045 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 30.1847019196 -69.0223770142 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 29.283241272 -72.0737915039 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 26.9897003174 -73.9624557495 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 23.7649536133 -74.3958969116 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 19.9833869934 -74.2500610352 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 15.7009191513 -73.0300064087 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 11.5243768692 -71.9855651855 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 7.55422449112 -71.0143661499 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ 3.99313831329 -70.1475296021 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -0.0668102130294 -69.1028747559 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -3.44227051735 -67.2309875488 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -5.49040985107 -65.1119689941 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -7.24591016769 -62.596157074 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -9.36122608185 -60.3147392273 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -11.5451803207 -58.3123054504 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -14.4151830673 -54.9342651367 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -16.5186710358 -52.5611877441 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -18.5795021057 -50.1394462585 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -21.8124217987 -46.6210861206 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -24.7267017365 -43.475151062 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -25.7503032684 -40.3981132507 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -26.0476665497 -36.9700622559 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -25.9315605164 -32.6193275452 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -25.7263412476 -28.6441040039 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -25.0013523102 -24.6762886047 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -24.2922458649 -20.8632926941 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -23.3050079346 -17.2035923004 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -22.1317424774 -13.6423683167 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -20.7885227203 -9.28058624268 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -18.4632396698 -6.41253614426 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -15.6022777557 -3.68699145317 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -13.5745029449 -2.1660451889 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -11.5146017075 -0.892862737179 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -7.04339790344 0.440190732479 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -4.10932731628 1.10956943035 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_yellow
+ -1.76674330235 1.47030568123 0 0 0 0
+ cone_left
+
+
+ model://fssim_gazebo/models/cone_orange_big
+ 4.7 2.5 0 0 0 0
+ cone_orange_big
+
+
+ model://fssim_gazebo/models/cone_orange_big
+ 4.7 -2.5 0 0 0 0
+ cone_orange_big
+
+
+ model://fssim_gazebo/models/cone_orange_big
+ 7.3 2.5 0 0 0 0
+ cone_orange_big
+
+
+ model://fssim_gazebo/models/cone_orange_big
+ 7.3 -2.5 0 0 0 0
+ cone_orange_big
+
+
+ model://fssim_gazebo/models/time_keeping
+ 6.0 3.0 0 0 0 0
+ tk_device_0
+
+
+ model://fssim_gazebo/models/time_keeping
+ 6.0 -3.0 0 0 0 0
+ tk_device_1
+
+
+
diff --git a/fssim_gazebo/models/track/model.config b/fssim_gazebo/models/track/model.config
index c8e20f4..76e5742 100644
--- a/fssim_gazebo/models/track/model.config
+++ b/fssim_gazebo/models/track/model.config
@@ -1,6 +1,6 @@
-
+
diff --git a/fssim_gazebo/models/track/skidpad.sdf b/fssim_gazebo/models/track/skidpad.sdf
index ccbff33..530a42a 100644
--- a/fssim_gazebo/models/track/skidpad.sdf
+++ b/fssim_gazebo/models/track/skidpad.sdf
@@ -1,375 +1,374 @@
-
model://fssim_gazebo/models/cone_blue
-0.0 1.6500000000000004 0 0 0 0
- cone_right
+ cone_right0
model://fssim_gazebo/models/cone_blue
-2.860558656929046 2.2190004944781316 0 0 0 0
- cone_right
+ cone_right1
model://fssim_gazebo/models/cone_blue
-5.285623189369442 3.839376810630557 0 0 0 0
- cone_right
+ cone_right2
model://fssim_gazebo/models/cone_blue
-6.905999505521868 6.264441343070954 0 0 0 0
- cone_right
+ cone_right3
model://fssim_gazebo/models/cone_blue
-7.475 9.125 0 0 0 0
- cone_right
+ cone_right4
model://fssim_gazebo/models/cone_blue
-6.905999505521868 11.985558656929046 0 0 0 0
- cone_right
+ cone_right5
model://fssim_gazebo/models/cone_blue
-5.285623189369443 14.410623189369442 0 0 0 0
- cone_right
+ cone_right6
model://fssim_gazebo/models/cone_blue
-2.860558656929047 16.03099950552187 0 0 0 0
- cone_right
+ cone_right7
model://fssim_gazebo/models/cone_blue
-9.154234823626465e-16 16.6 0 0 0 0
- cone_right
+ cone_right8
model://fssim_gazebo/models/cone_blue
2.860558656929045 16.03099950552187 0 0 0 0
- cone_right
+ cone_right9
model://fssim_gazebo/models/cone_blue
5.285623189369442 14.410623189369444 0 0 0 0
- cone_right
+ cone_right10
model://fssim_gazebo/models/cone_blue
6.905999505521867 11.98555865692905 0 0 0 0
- cone_right
+ cone_right11
model://fssim_gazebo/models/cone_blue
7.475 9.125000000000002 0 0 0 0
- cone_right
+ cone_right12
model://fssim_gazebo/models/cone_blue
6.9059995055218675 6.264441343070953 0 0 0 0
- cone_right
+ cone_right13
model://fssim_gazebo/models/cone_blue
5.285623189369444 3.839376810630559 0 0 0 0
- cone_right
+ cone_right14
model://fssim_gazebo/models/cone_blue
2.8605586569290504 2.2190004944781334 0 0 0 0
- cone_right
+ cone_right15
model://fssim_gazebo/models/cone_blue
7.619075567285049 -1.5059244327149495 0 0 0 0
- cone_right
+ cone_right16
model://fssim_gazebo/models/cone_blue
9.954801962809116 -5.001586016266157 0 0 0 0
- cone_right
+ cone_right17
model://fssim_gazebo/models/cone_blue
10.775 -9.125 0 0 0 0
- cone_right
+ cone_right18
model://fssim_gazebo/models/cone_blue
9.954801962809116 -13.248413983733842 0 0 0 0
- cone_right
+ cone_right19
model://fssim_gazebo/models/cone_blue
7.6190755672850505 -16.744075567285048 0 0 0 0
- cone_right
+ cone_right20
model://fssim_gazebo/models/cone_blue
4.123413983733844 -19.079801962809114 0 0 0 0
- cone_right
+ cone_right21
model://fssim_gazebo/models/cone_blue
1.3195569260812732e-15 -19.9 0 0 0 0
- cone_right
+ cone_right22
model://fssim_gazebo/models/cone_blue
-4.123413983733841 -19.079801962809114 0 0 0 0
- cone_right
+ cone_right23
model://fssim_gazebo/models/cone_blue
-7.619075567285049 -16.74407556728505 0 0 0 0
- cone_right
+ cone_right24
model://fssim_gazebo/models/cone_blue
-9.954801962809112 -13.248413983733847 0 0 0 0
- cone_right
+ cone_right25
model://fssim_gazebo/models/cone_blue
-10.775 -9.125000000000002 0 0 0 0
- cone_right
+ cone_right26
model://fssim_gazebo/models/cone_blue
-9.954801962809114 -5.001586016266155 0 0 0 0
- cone_right
+ cone_right27
model://fssim_gazebo/models/cone_blue
-7.619075567285051 -1.5059244327149521 0 0 0 0
- cone_right
+ cone_right28
model://fssim_gazebo/models/cone_yellow
-7.619075567285049 1.5059244327149495 0 0 0 0
- cone_left
+ cone_left0
model://fssim_gazebo/models/cone_yellow
-9.954801962809116 5.001586016266157 0 0 0 0
- cone_left
+ cone_left1
model://fssim_gazebo/models/cone_yellow
-10.775 9.125 0 0 0 0
- cone_left
+ cone_left2
model://fssim_gazebo/models/cone_yellow
-9.954801962809116 13.248413983733842 0 0 0 0
- cone_left
+ cone_left3
model://fssim_gazebo/models/cone_yellow
-7.6190755672850505 16.744075567285048 0 0 0 0
- cone_left
+ cone_left4
model://fssim_gazebo/models/cone_yellow
-4.123413983733844 19.079801962809114 0 0 0 0
- cone_left
+ cone_left5
model://fssim_gazebo/models/cone_yellow
-1.3195569260812732e-15 19.9 0 0 0 0
- cone_left
+ cone_left6
model://fssim_gazebo/models/cone_yellow
4.123413983733841 19.079801962809114 0 0 0 0
- cone_left
+ cone_left7
model://fssim_gazebo/models/cone_yellow
7.619075567285049 16.74407556728505 0 0 0 0
- cone_left
+ cone_left8
model://fssim_gazebo/models/cone_yellow
9.954801962809112 13.248413983733847 0 0 0 0
- cone_left
+ cone_left9
model://fssim_gazebo/models/cone_yellow
10.775 9.125000000000002 0 0 0 0
- cone_left
+ cone_left10
model://fssim_gazebo/models/cone_yellow
9.954801962809114 5.001586016266155 0 0 0 0
- cone_left
+ cone_left11
model://fssim_gazebo/models/cone_yellow
7.619075567285051 1.5059244327149521 0 0 0 0
- cone_left
+ cone_left12
model://fssim_gazebo/models/cone_yellow
0.0 -1.6500000000000004 0 0 0 0
- cone_left
+ cone_left13
model://fssim_gazebo/models/cone_yellow
2.860558656929046 -2.2190004944781316 0 0 0 0
- cone_left
+ cone_left14
model://fssim_gazebo/models/cone_yellow
5.285623189369442 -3.839376810630557 0 0 0 0
- cone_left
+ cone_left15
model://fssim_gazebo/models/cone_yellow
6.905999505521868 -6.264441343070954 0 0 0 0
- cone_left
+ cone_left16
model://fssim_gazebo/models/cone_yellow
7.475 -9.125 0 0 0 0
- cone_left
+ cone_left17
model://fssim_gazebo/models/cone_yellow
6.905999505521868 -11.985558656929046 0 0 0 0
- cone_left
+ cone_left18
model://fssim_gazebo/models/cone_yellow
5.285623189369443 -14.410623189369442 0 0 0 0
- cone_left
+ cone_left19
model://fssim_gazebo/models/cone_yellow
2.860558656929047 -16.03099950552187 0 0 0 0
- cone_left
+ cone_left20
model://fssim_gazebo/models/cone_yellow
9.154234823626465e-16 -16.6 0 0 0 0
- cone_left
+ cone_left21
model://fssim_gazebo/models/cone_yellow
-2.860558656929045 -16.03099950552187 0 0 0 0
- cone_left
+ cone_left22
model://fssim_gazebo/models/cone_yellow
-5.285623189369442 -14.410623189369444 0 0 0 0
- cone_left
+ cone_left23
model://fssim_gazebo/models/cone_yellow
-6.905999505521867 -11.98555865692905 0 0 0 0
- cone_left
+ cone_left24
model://fssim_gazebo/models/cone_yellow
-7.475 -9.125000000000002 0 0 0 0
- cone_left
+ cone_left25
model://fssim_gazebo/models/cone_yellow
-6.9059995055218675 -6.264441343070953 0 0 0 0
- cone_left
+ cone_left26
model://fssim_gazebo/models/cone_yellow
-5.285623189369444 -3.839376810630559 0 0 0 0
- cone_left
+ cone_left27
model://fssim_gazebo/models/cone_yellow
-2.8605586569290504 -2.2190004944781334 0 0 0 0
- cone_left
+ cone_left28
model://fssim_gazebo/models/cone_orange
-11.25 1.65 0 0 0 0
- cone_orange
+ cone_orange0
model://fssim_gazebo/models/cone_orange
-11.25 -1.65 0 0 0 0
- cone_orange
+ cone_orange1
model://fssim_gazebo/models/cone_orange
-15.0 1.65 0 0 0 0
- cone_orange
+ cone_orange2
model://fssim_gazebo/models/cone_orange
-15.0 -1.65 0 0 0 0
- cone_orange
+ cone_orange3
model://fssim_gazebo/models/cone_orange
11.0 1.65 0 0 0 0
- cone_orange
+ cone_orange4
model://fssim_gazebo/models/cone_orange
11.0 -1.65 0 0 0 0
- cone_orange
+ cone_orange5
model://fssim_gazebo/models/cone_orange
16.0 1.65 0 0 0 0
- cone_orange
+ cone_orange6
model://fssim_gazebo/models/cone_orange
16.0 -1.65 0 0 0 0
- cone_orange
+ cone_orange7
model://fssim_gazebo/models/cone_orange
21.0 1.65 0 0 0 0
- cone_orange
+ cone_orange8
model://fssim_gazebo/models/cone_orange
21.0 -1.65 0 0 0 0
- cone_orange
+ cone_orange9
model://fssim_gazebo/models/cone_orange
21.0 0.0 0 0 0 0
- cone_orange
+ cone_orange10
model://fssim_gazebo/models/cone_orange
21.0 1.65 0 0 0 0
- cone_orange
+ cone_orange11
model://fssim_gazebo/models/cone_orange_big
-1.3 2.0 0 0 0 0
- cone_orange_big
+ cone_orange_big0
model://fssim_gazebo/models/cone_orange_big
-1.3 -2.0 0 0 0 0
- cone_orange_big
+ cone_orange_big1
model://fssim_gazebo/models/cone_orange_big
1.3 2.0 0 0 0 0
- cone_orange_big
+ cone_orange_big2
model://fssim_gazebo/models/cone_orange_big
1.3 -2.0 0 0 0 0
- cone_orange_big
+ cone_orange_big3
model://fssim_gazebo/models/time_keeping
@@ -382,4 +381,4 @@
tk_device_1
-
+
\ No newline at end of file
diff --git a/fssim_gazebo_plugins/CMakeLists.txt b/fssim_gazebo_plugins/CMakeLists.txt
index 5d23527..3512c42 100644
--- a/fssim_gazebo_plugins/CMakeLists.txt
+++ b/fssim_gazebo_plugins/CMakeLists.txt
@@ -20,7 +20,7 @@
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
project(fssim_gazebo_plugins)
set(CMAKE_CXX_STANDARD 11)
@@ -41,7 +41,7 @@ find_package(catkin REQUIRED COMPONENTS
pcl_ros
visualization_msgs)
-find_package(gazebo 7 REQUIRED)
+find_package(gazebo 11 REQUIRED)
find_package(PCL REQUIRED)
find_package(Boost REQUIRED COMPONENTS thread)
diff --git a/fssim_gazebo_plugins/gazebo_cone_sensor/src/cone_sensor_model.cpp b/fssim_gazebo_plugins/gazebo_cone_sensor/src/cone_sensor_model.cpp
index f867442..27892fa 100644
--- a/fssim_gazebo_plugins/gazebo_cone_sensor/src/cone_sensor_model.cpp
+++ b/fssim_gazebo_plugins/gazebo_cone_sensor/src/cone_sensor_model.cpp
@@ -114,7 +114,7 @@ bool ConeSensorModel::load(const physics::ModelPtr &model, const sdf::ElementPtr
pub_markers_ = nh_.advertise("cone_sensor", 1);
parent_model_ = model;
- model_ = model->GetWorld()->GetModel("track");
+ model_ = model->GetWorld()->ModelByName("track");
if (model_ == nullptr) { return false; }
vehicle_frame_ = _sdf->Get("base_link");
@@ -174,7 +174,7 @@ bool ConeSensorModel::checkInit() {
if (loaded_sacesfully_) { return true; }
parent_model_->Reset();
- model_ = parent_model_->GetWorld()->GetModel("track");
+ model_ = parent_model_->GetWorld()->ModelByName("track");
if (model_ == NULL) {
ROS_ERROR("ConeSensorModel: DID NOT FIND MODEL");
return false;
@@ -195,6 +195,7 @@ void ConeSensorModel::updateTrack() {
const auto child = model_->GetChild(i);
const auto entity = boost::dynamic_pointer_cast(child);
+ /*
if (entity->GetName() == "cone_left::link") {
left_.push_back(entity);
} else if (entity->GetName() == "cone_right::link") {
@@ -202,6 +203,15 @@ void ConeSensorModel::updateTrack() {
} else if (entity->GetName() == "cone_orange::link" or entity->GetName() == "cone_orange_big::link") {
orange_.push_back(entity);
}
+ */
+
+ if (entity->GetName().substr(0, 9) == "cone_left") {
+ left_.push_back(entity);
+ } else if (entity->GetName().substr(0, 10) == "cone_right") {
+ right_.push_back(entity);
+ } else if (entity->GetName().substr(0, 11) == "cone_orange" or entity->GetName().substr(0, 15) == "cone_orange_big") {
+ orange_.push_back(entity);
+ }
}
}
diff --git a/fssim_gazebo_plugins/gazebo_cone_sensor/src/gazebo_cone_sensor.cpp b/fssim_gazebo_plugins/gazebo_cone_sensor/src/gazebo_cone_sensor.cpp
index 9458148..d00b7bd 100644
--- a/fssim_gazebo_plugins/gazebo_cone_sensor/src/gazebo_cone_sensor.cpp
+++ b/fssim_gazebo_plugins/gazebo_cone_sensor/src/gazebo_cone_sensor.cpp
@@ -65,7 +65,7 @@ void ConeSensor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
void ConeSensor::Update() {
std::lock_guard lock(mutex);
- common::Time cur_time = model_->GetWorld()->GetSimTime();
+ common::Time cur_time = model_->GetWorld()->SimTime();
double dt;
if (!isLoopTime(cur_time, dt)) {
diff --git a/fssim_gazebo_plugins/gazebo_race_car_model/include/axle.hpp b/fssim_gazebo_plugins/gazebo_race_car_model/include/axle.hpp
index fb8d688..24f4d82 100644
--- a/fssim_gazebo_plugins/gazebo_race_car_model/include/axle.hpp
+++ b/fssim_gazebo_plugins/gazebo_race_car_model/include/axle.hpp
@@ -47,7 +47,7 @@ class Axle {
transport::NodePtr &gznode,
boost::shared_ptr &nh);
- const math::Vector3 &getAxlePos() const;
+ const ignition::math::Vector3d &getAxlePos() const;
void printInfo();
@@ -75,7 +75,7 @@ class Axle {
WheelType wheel_r_;
// Center Position of the axle
- math::Vector3 axlePos;
+ ignition::math::Vector3d axlePos;
// Parameters
double axle_width_; // Width [m]
diff --git a/fssim_gazebo_plugins/gazebo_race_car_model/include/definitions.hpp b/fssim_gazebo_plugins/gazebo_race_car_model/include/definitions.hpp
index 1f55a7e..2df20d2 100644
--- a/fssim_gazebo_plugins/gazebo_race_car_model/include/definitions.hpp
+++ b/fssim_gazebo_plugins/gazebo_race_car_model/include/definitions.hpp
@@ -42,6 +42,7 @@ struct State {
std::stringstream &operator<<(std::stringstream &os) {
os << getString();
+ return os;
}
inline std::string getString() const {
diff --git a/fssim_gazebo_plugins/gazebo_race_car_model/include/vehicle.hpp b/fssim_gazebo_plugins/gazebo_race_car_model/include/vehicle.hpp
index d8be973..07714b2 100644
--- a/fssim_gazebo_plugins/gazebo_race_car_model/include/vehicle.hpp
+++ b/fssim_gazebo_plugins/gazebo_race_car_model/include/vehicle.hpp
@@ -39,6 +39,7 @@
#include "fssim_common/State.h"
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
+#include "visualization_msgs/Marker.h"
// Utills
#include "gazebo_utils/include/gazebo_to_ros.hpp"
@@ -124,6 +125,7 @@ class Vehicle {
// ROS Publishrs
ros::Publisher pub_ground_truth_;
ros::Publisher pub_car_info_;
+ ros::Publisher pub_go_signal_;
// ROS Subscribers
ros::Subscriber sub_cmd_;
@@ -132,6 +134,7 @@ class Vehicle {
// ROS TF
tf::TransformBroadcaster tf_br_;
+ ros::Time last_tf_time_;
/// Pointer to the parent model
physics::ModelPtr model;
diff --git a/fssim_gazebo_plugins/gazebo_race_car_model/include/wheel.hpp b/fssim_gazebo_plugins/gazebo_race_car_model/include/wheel.hpp
index a22ff7c..c0c60f4 100644
--- a/fssim_gazebo_plugins/gazebo_race_car_model/include/wheel.hpp
+++ b/fssim_gazebo_plugins/gazebo_race_car_model/include/wheel.hpp
@@ -68,7 +68,7 @@ class Wheel {
virtual void setAngle(const double delta) {}
- const math::Vector3 &getCenterPos() const;
+ const ignition::math::Vector3d &getCenterPos() const;
private:
@@ -81,7 +81,7 @@ class Wheel {
Param::Tire param_; // Pacejka Tire parameters
double radius; // Radious of the tire from STL [m]
- gazebo::math::Vector3 center_pos_; // Center location of the tire
+ ignition::math::Vector3d center_pos_; // Center location of the tire
};
} // namespace fssim
diff --git a/fssim_gazebo_plugins/gazebo_race_car_model/src/axle.cpp b/fssim_gazebo_plugins/gazebo_race_car_model/src/axle.cpp
index f0ae0f3..04a6514 100644
--- a/fssim_gazebo_plugins/gazebo_race_car_model/src/axle.cpp
+++ b/fssim_gazebo_plugins/gazebo_race_car_model/src/axle.cpp
@@ -41,7 +41,7 @@ Axle::Axle(physics::ModelPtr &_model,
name_(name) {
const auto vec3 = wheel_l_.getCenterPos() - wheel_r_.getCenterPos();
- axle_width_ = vec3.GetLength();
+ axle_width_ = vec3.Length();
axlePos = (wheel_l_.getCenterPos() + wheel_r_.getCenterPos()) / 2.0;
@@ -49,7 +49,7 @@ Axle::Axle(physics::ModelPtr &_model,
}
template
-const math::Vector3 &Axle::getAxlePos() const {
+const ignition::math::Vector3d &Axle::getAxlePos() const {
return axlePos;
}
diff --git a/fssim_gazebo_plugins/gazebo_race_car_model/src/gazebo_ros_race_car_model.cpp b/fssim_gazebo_plugins/gazebo_race_car_model/src/gazebo_ros_race_car_model.cpp
index 65e6490..33b54fc 100644
--- a/fssim_gazebo_plugins/gazebo_race_car_model/src/gazebo_ros_race_car_model.cpp
+++ b/fssim_gazebo_plugins/gazebo_race_car_model/src/gazebo_ros_race_car_model.cpp
@@ -66,7 +66,7 @@ void RaceCarModelPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
this->dataPtr->worldControlPub = this->dataPtr->gznode->Advertise("~/world_control");
- this->dataPtr->lastSimTime = this->dataPtr->world->GetSimTime();
+ this->dataPtr->lastSimTime = this->dataPtr->world->SimTime();
}
void RaceCarModelPlugin::Reset() {
@@ -78,9 +78,9 @@ void RaceCarModelPlugin::update() {
publishInfo();
- common::Time curTime = this->dataPtr->world->GetSimTime();
+ common::Time curTime = this->dataPtr->world->SimTime();
double dt = 0.0;
- if (!isLoopTime(this->dataPtr->world->GetSimTime(), dt)) {
+ if (!isLoopTime(this->dataPtr->world->SimTime(), dt)) {
return;
}
@@ -90,7 +90,7 @@ void RaceCarModelPlugin::update() {
}
void RaceCarModelPlugin::publishInfo() {
- this->dataPtr->vehicle->publish(this->dataPtr->world->GetSimTime().Double());
+ this->dataPtr->vehicle->publish(this->dataPtr->world->SimTime().Double());
}
bool RaceCarModelPlugin::isLoopTime(const common::Time &time, double &dt) {
diff --git a/fssim_gazebo_plugins/gazebo_race_car_model/src/vehicle.cpp b/fssim_gazebo_plugins/gazebo_race_car_model/src/vehicle.cpp
index 0a9c450..761f8fe 100644
--- a/fssim_gazebo_plugins/gazebo_race_car_model/src/vehicle.cpp
+++ b/fssim_gazebo_plugins/gazebo_race_car_model/src/vehicle.cpp
@@ -43,6 +43,7 @@ Vehicle::Vehicle(physics::ModelPtr &_model,
// ROS Publishers
pub_ground_truth_ = nh->advertise("/fssim/base_pose_ground_truth", 1);
pub_car_info_ = nh->advertise("/fssim/car_info", 1);
+ pub_go_signal_ = nh->advertise("/fssim/GO_marker", 1);
// ROS Subscribers
sub_res_ = nh->subscribe("/fssim/res_state", 1, &Vehicle::onRes, this);
@@ -65,14 +66,14 @@ Vehicle::Vehicle(physics::ModelPtr &_model,
}
void Vehicle::setPositionFromWorld() {
- auto pos = model->GetWorldPose();
- const auto vel = model->GetWorldLinearVel();
- const auto accel = model->GetWorldLinearAccel();
- const auto r = model->GetWorldAngularVel();
+ auto pos = model->WorldPose();
+ const auto vel = model->WorldLinearVel();
+ const auto accel = model->WorldLinearAccel();
+ const auto r = model->WorldAngularVel();
state_.x = -1;
- state_.y = pos.pos.y;
- state_.yaw = pos.rot.GetYaw();
+ state_.y = pos.Pos().Y();
+ state_.yaw = pos.Rot().Yaw();
state_.v_x = 0.0;
state_.v_y = 0.0;
state_.r = 0.0;
@@ -90,7 +91,7 @@ void Vehicle::initModel(sdf::ElementPtr &_sdf) {
// then the wheelbase is the distance between the axle centers
auto vec3 = front_axle_.getAxlePos() - rear_axle_.getAxlePos();
- param_.kinematic.l = vec3.GetLength();
+ param_.kinematic.l = vec3.Length();
}
void Vehicle::initVehicleParam(sdf::ElementPtr &_sdf) {
@@ -148,6 +149,30 @@ void Vehicle::onRes(const fssim_common::ResStateConstPtr &msg) {
res_state_ = *msg;
if (res_state_.push_button) { car_info_.torque_ok = static_cast(true); }
if (res_state_.emergency) { car_info_.torque_ok = static_cast(false); }
+
+ // Visualize when GO is pressed
+ visualization_msgs::Marker GO_marker;
+ GO_marker.header.frame_id = "/map";
+ GO_marker.header.stamp = ros::Time::now();
+ GO_marker.lifetime = ros::Duration(3);
+ GO_marker.ns = "basic_shapes";
+ GO_marker.id = 139287;
+ GO_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+ GO_marker.action = visualization_msgs::Marker::ADD;
+ GO_marker.pose.position.x = 0.0;
+ GO_marker.pose.position.y = 0.0;
+ GO_marker.pose.position.z = 3.0;
+ GO_marker.pose.orientation.x = 0.0;
+ GO_marker.pose.orientation.y = 0.0;
+ GO_marker.pose.orientation.z = 0.0;
+ GO_marker.pose.orientation.w = 1.0;
+ GO_marker.text = "GO!";
+ GO_marker.scale.z = 1;
+ GO_marker.color.r = 0.0f;
+ GO_marker.color.g = 1.0f;
+ GO_marker.color.b = 0.0f;
+ GO_marker.color.a = 1.0;
+ pub_go_signal_.publish(GO_marker);
}
void Vehicle::printInfo() {
@@ -185,6 +210,7 @@ State Vehicle::f(const State &x,
std::ostream &operator<<(std::ostream &os, const State s) {
os << s.getString();
+ return os;
}
State Vehicle::f_kin_correction(const State &x_in,
@@ -222,7 +248,11 @@ void Vehicle::publishTf(const State &x) {
transform.setRotation(q);
// Send TF
- tf_br_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/fssim_map", "/fssim/vehicle/base_link"));
+ ros::Time tf_time = ros::Time::now();
+ if (last_tf_time_ < tf_time) {
+ tf_br_.sendTransform(tf::StampedTransform(transform, tf_time, "/fssim_map", "/fssim/vehicle/base_link"));
+ last_tf_time_ = tf_time;
+ }
}
double Vehicle::getFx(const State &x, const Input &u) {
@@ -260,9 +290,9 @@ double Vehicle::getNormalForce(const State &x) {
}
void Vehicle::setModelState(const State &x) {
- const math::Pose pose(x.x, x.y, 0.0, 0, 0.0, x.yaw);
- const math::Vector3 vel(x.v_x, x.v_y, 0.0);
- const math::Vector3 angular(0.0, 0.0, x.r);
+ const ignition::math::Pose3d pose(x.x, x.y, 0.0, 0, 0.0, x.yaw);
+ const ignition::math::Vector3d vel(x.v_x, x.v_y, 0.0);
+ const ignition::math::Vector3d angular(0.0, 0.0, x.r);
model->SetWorldPose(pose);
model->SetAngularVel(angular);
model->SetLinearVel(vel);
@@ -302,6 +332,15 @@ void Vehicle::publishCarInfo(const AxleTires &alphaF,
car_info.Fy_r_r = FyR.right;
car_info.Fx = Fx;
+
+ // Add state info
+ car_info.vx = state_.v_x;
+ car_info.vy = state_.v_y;
+ car_info.r = state_.r;
+
+ car_info.dc = input_.dc;
+ car_info.delta = input_.delta;
+
pub_car_info_.publish(car_info);
}
diff --git a/fssim_gazebo_plugins/gazebo_race_car_model/src/wheel.cpp b/fssim_gazebo_plugins/gazebo_race_car_model/src/wheel.cpp
index 9d1d30f..4b45522 100644
--- a/fssim_gazebo_plugins/gazebo_race_car_model/src/wheel.cpp
+++ b/fssim_gazebo_plugins/gazebo_race_car_model/src/wheel.cpp
@@ -41,7 +41,7 @@ Wheel::Wheel(gazebo::physics::ModelPtr &_model,
unsigned int id = 0;
radius = getCollisionRadius(joint->GetChild()->GetCollision(id));
- center_pos_ = joint->GetChild()->GetCollision(id)->GetWorldPose().pos;
+ center_pos_ = joint->GetChild()->GetCollision(id)->WorldPose().Pos();
}
double Wheel::getCollisionRadius(gazebo::physics::CollisionPtr _coll) {
@@ -73,7 +73,7 @@ double Wheel::getFy(const double alpha, const double Fz) {
return Fy;
}
-const math::Vector3 &Wheel::getCenterPos() const { return center_pos_; }
+const ignition::math::Vector3d &Wheel::getCenterPos() const { return center_pos_; }
} // namespace fssim
} // namespace gazebo
\ No newline at end of file
diff --git a/fssim_gazebo_plugins/gazebo_track/src/gazebo_track.cpp b/fssim_gazebo_plugins/gazebo_track/src/gazebo_track.cpp
index a9c6081..e480530 100644
--- a/fssim_gazebo_plugins/gazebo_track/src/gazebo_track.cpp
+++ b/fssim_gazebo_plugins/gazebo_track/src/gazebo_track.cpp
@@ -55,13 +55,13 @@ class TrackStreamer : public ModelPlugin {
};
Point2d toPoint2d(const boost::shared_ptr &e) const {
- return {e->GetWorldPose().pos.x, e->GetWorldPose().pos.y};
+ return {e->WorldPose().Pos().X(), e->WorldPose().Pos().Y()};
}
geometry_msgs::Point toGeomPoint(const boost::shared_ptr &e) const {
geometry_msgs::Point p;
- p.x = e->GetWorldPose().pos.x;
- p.y = e->GetWorldPose().pos.y;
+ p.x = e->WorldPose().Pos().X();
+ p.y = e->WorldPose().Pos().Y();
p.z = 0.0;
return p;
}
@@ -118,7 +118,7 @@ class TrackStreamer : public ModelPlugin {
nh.advertise(_sdf->Get("markers") + "/markers", 1, true);
pub_custome_track = nh.advertise(_sdf->Get("markers"), 1, true);
- const auto model = _parent->GetWorld()->GetModel("track");
+ const auto model = _parent->GetWorld()->ModelByName("track");
std::vector> left;
std::vector> right;
@@ -130,13 +130,13 @@ class TrackStreamer : public ModelPlugin {
const auto child = model->GetChild(i);
const auto entity = boost::dynamic_pointer_cast(child);
const auto name = entity->GetName();
- if (name == "cone_left::link") {
+ if (name.substr(0, 9) == "cone_left") {
left.push_back(entity);
- } else if (name == "cone_right::link") {
+ } else if (name.substr(0, 10) == "cone_right") {
right.push_back(entity);
- } else if (name == "cone_orange::link") {
+ } else if (name.substr(0, 11) == "cone_orange") {
orange.push_back(entity);
- } else if (name == "cone_orange_big::link") {
+ } else if (name.substr(0, 15) == "cone_orange_big") {
orange_big.push_back(entity);
} else if (name.find("tk_device") != std::string::npos) {
if (name.find("0") != std::string::npos or name.find("1") != std::string::npos) {
diff --git a/fssim_gazebo_plugins/gazebo_utils/include/gazebo_utills.hpp b/fssim_gazebo_plugins/gazebo_utils/include/gazebo_utills.hpp
index 6b0cb50..65e1bb6 100644
--- a/fssim_gazebo_plugins/gazebo_utils/include/gazebo_utills.hpp
+++ b/fssim_gazebo_plugins/gazebo_utils/include/gazebo_utills.hpp
@@ -75,7 +75,7 @@ type getParam(sdf::ElementPtr &sdf, std::string paramName) {
} // namespace gazebo
inline Eigen::Vector3d toVector(const boost::shared_ptr &v) {
- return {v->GetWorldPose().pos.x, v->GetWorldPose().pos.y, 0.0};
+ return {v->WorldPose().Pos().X(), v->WorldPose().Pos().Y(), 0.0};
}
#endif //FSSIM_GAZEBO_GAZEBOUTILLS_H
diff --git a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/cone_editor.py b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/cone_editor.py
index 7095f62..552287d 100644
--- a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/cone_editor.py
+++ b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/cone_editor.py
@@ -236,20 +236,20 @@ def export_model(self, path, name):
tree.write(gazebo_models + ".sdf", pretty_print=True, xml_declaration=True, encoding='UTF-8')
self.track.export_to_yaml(self.model_path + "/track/tracks_yaml", name,create_dir=False)
- print "[INFO] Saving track to: ",gazebo_models + ".sdf"
+ print("[INFO] Saving track to: ",gazebo_models + ".sdf")
def handle_btn_import(self, path,outside,inside,center):
if path.endswith('.bag'):
self.track.load_track_from_bag(path,outside,inside,center)
self.update_all()
else:
- print "[ERROR] Wrong file extension. Only ROSBAG supported"
+ print("[ERROR] Wrong file extension. Only ROSBAG supported")
def on_mouse_up(self, event):
pass
def on_mouse_move(self, event):
- print event
+ print(event)
def on_mouse_down(self, event):
if not self.enable_editing:
diff --git a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/main.py b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/main.py
index af501d8..de9b96d 100644
--- a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/main.py
+++ b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/main.py
@@ -125,11 +125,11 @@ def __init__(self, context):
def pushButton_skidpad(self):
id = self._widget.toolBoxDiscipline.currentIndex()
if id is 0:
- print "Generating SKIDPAD"
+ print("Generating SKIDPAD")
self._cones_view.generate_skipdpad(self._widget)
self._widget.line_edit_track_name.setText("skidpad")
elif id is 1:
- print "Generating Acceleration"
+ print("Generating Acceleration")
self._cones_view.generate_acceleration(self._widget)
self._widget.line_edit_track_name.setText("acceleration")
@@ -141,7 +141,7 @@ def comboBox_snap_topics_changed(self,event):
self._cones_view.snapshots.import_snapshots(event)
self._cones_view.change_view(2)
self._widget.horSlid_snap.setMaximum(self._cones_view.snapshots.max)
- print "INFO: Snapshot imported"
+ print("INFO: Snapshot imported")
def handle_input_snap(self):
if not self._topics_for_snpshot_found:
diff --git a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/message_filter.py b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/message_filter.py
index ca54152..d9cae21 100644
--- a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/message_filter.py
+++ b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/message_filter.py
@@ -30,7 +30,7 @@ def __init__(self, filename = None):
self.file_name = filename
def get_topic_names(self, msg):
- print " Opening bag:",self.file_name
+ print(" Opening bag:",self.file_name)
bag = rosbag.Bag(self.file_name)
val = bag.get_type_and_topic_info()[1].values()
diff --git a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/snapshot_handler.py b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/snapshot_handler.py
index 5f739cd..338fb63 100644
--- a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/snapshot_handler.py
+++ b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/snapshot_handler.py
@@ -77,7 +77,7 @@ def load_snap_from_list(self, i):
self.cones = np.vstack([self.cones, xy])
def import_snapshots(self, topic_name):
- print "Importing Snapshots from: ", topic_name
+ print("Importing Snapshots from: ", topic_name)
bag = rosbag.Bag(self.bag_filter.file_name)
@@ -93,7 +93,7 @@ def import_snapshots(self, topic_name):
bag.close()
self.load_snap_from_list(0)
- print "Importing DONE"
+ print("Importing DONE")
def load_topic_names(self, path):
diff --git a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/track.py b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/track.py
index 9ba0207..f8daafd 100644
--- a/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/track.py
+++ b/fssim_rqt_plugins/rqt_fssim_track_editor/src/rqt_fssim_track_editor/track.py
@@ -123,7 +123,7 @@ def interpolate(self, circular = True):
self.control_points[-2, :] -
self.control_points[-1, :]))
else:
- print "WARNING: Track was already computed"
+ print("WARNING: Track was already computed")
return
kind = "cubic"
@@ -397,7 +397,7 @@ def import_snapshots(self, path):
bag.close()
def export_to_yaml(self, path, name, create_dir=True):
- print path, name
+ print(path, name)
dict_track = {"cones_left": np.array(self.cones_left).tolist(),
"cones_right": np.array(self.cones_right).tolist(),
"cones_orange": np.array(self.cones_orange).tolist(),
@@ -416,7 +416,7 @@ def export_to_yaml(self, path, name, create_dir=True):
file_path = dir + '/' + name + ".yaml"
with open(file_path, 'w') as outfile:
yaml.dump(dict_track, outfile, default_flow_style = False)
- print "[INFO] Saving track to: ",file_path
+ print("[INFO] Saving track to: ",file_path)
def export_to_mat(self, path, name):
if len(self.cones_left) == len(self.middle) and len(self.cones_right) == len(self.middle):
@@ -436,4 +436,4 @@ def export_to_mat(self, path, name):
os.makedirs(dir)
file_path = dir +'/'+ name + '.mat'
savemat(file_path, dict_track, oned_as = 'row')
- print "[INFO] Saving track to: ",file_path
+ print("[INFO] Saving track to: ",file_path)