From af2a4b3f9dc990aa7d21aa57753352ffe61cb473 Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Fri, 5 Nov 2021 17:52:43 +0800 Subject: [PATCH 01/12] FSSIM 2021 --- fssim/CMakeLists.txt | 4 +- fssim/config/simulation.yaml | 2 +- fssim/config/topics_frequency.yaml | 2 +- fssim/launch/auto_fssim.launch | 2 +- fssim/rviz/fssim.rviz | 78 +++++++++---------- fssim/scripts/automated_res.py | 40 ++++++---- fssim/scripts/launch.py | 48 ++++++------ fssim/scripts/topics_monitor.py | 8 +- fssim/src/fssim/ecu.py | 2 +- fssim/src/fssim/shell.py | 14 ++-- fssim/src/fssim/statistics.py | 16 ++-- fssim/src/fssim/vehicle_position_validate.py | 4 +- fssim_description/scripts/load_vehicle.py | 8 +- fssim_gazebo/models/track/model.config | 4 +- fssim_gazebo_plugins/CMakeLists.txt | 4 +- .../src/cone_sensor_model.cpp | 4 +- .../src/gazebo_cone_sensor.cpp | 2 +- .../gazebo_race_car_model/include/axle.hpp | 4 +- .../include/definitions.hpp | 1 + .../gazebo_race_car_model/include/vehicle.hpp | 2 + .../gazebo_race_car_model/include/wheel.hpp | 4 +- .../gazebo_race_car_model/src/axle.cpp | 4 +- .../src/gazebo_ros_race_car_model.cpp | 8 +- .../gazebo_race_car_model/src/vehicle.cpp | 55 ++++++++++--- .../gazebo_race_car_model/src/wheel.cpp | 4 +- .../gazebo_track/src/gazebo_track.cpp | 8 +- .../gazebo_utils/include/gazebo_utills.hpp | 2 +- .../src/rqt_fssim_track_editor/cone_editor.py | 6 +- .../src/rqt_fssim_track_editor/main.py | 6 +- .../rqt_fssim_track_editor/message_filter.py | 2 +- .../snapshot_handler.py | 4 +- .../src/rqt_fssim_track_editor/track.py | 8 +- 32 files changed, 200 insertions(+), 160 deletions(-) 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..b0ac6e9 100644 --- a/fssim/config/simulation.yaml +++ b/fssim/config/simulation.yaml @@ -30,7 +30,7 @@ pkg_config_storage: fssim_description # If relative path is given in following c # wrt to this package # Config files for repetitions repetitions: -- {sensors_config_file: cars/gotthard/config/sensors.yaml, track_name: FSG.sdf, autonomous_stack: } +- {sensors_config_file: cars/gotthard/config/sensors.yaml, track_name: FSI.sdf, autonomous_stack: } # Checks which must be true in order to automatically press RES GO res: 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/launch/auto_fssim.launch b/fssim/launch/auto_fssim.launch index 2d2d868..f68013d 100644 --- a/fssim/launch/auto_fssim.launch +++ b/fssim/launch/auto_fssim.launch @@ -7,6 +7,6 @@ - + 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_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/model.config b/fssim_gazebo/models/track/model.config index c8e20f4..8de1ec5 100644 --- a/fssim_gazebo/models/track/model.config +++ b/fssim_gazebo/models/track/model.config @@ -1,12 +1,12 @@ - + track 1.0 - FSG.sdf + FSI.sdf random track 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..044ac67 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; 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..ee64cdb 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_; 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..97cae84 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, @@ -260,9 +286,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 +328,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..ea75402 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; 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) From dfc5e9e5d9155300de6636e021bdb2aa9b43215d Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Fri, 5 Nov 2021 18:33:48 +0800 Subject: [PATCH 02/12] FSSIM 2021 - With maps --- fssim/config/simulation.yaml | 4 +- fssim_gazebo/models/track/FSG.sdf | 379 +++++++++--------- fssim_gazebo/models/track/FSI.sdf | 321 ++++++++------- fssim_gazebo/models/track/model.config | 2 +- .../src/cone_sensor_model.cpp | 10 + .../gazebo_track/src/gazebo_track.cpp | 8 +- 6 files changed, 366 insertions(+), 358 deletions(-) diff --git a/fssim/config/simulation.yaml b/fssim/config/simulation.yaml index b0ac6e9..a7acc46 100644 --- a/fssim/config/simulation.yaml +++ b/fssim/config/simulation.yaml @@ -27,10 +27,10 @@ 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: FSI.sdf, autonomous_stack: } +- {sensors_config_file: cars/gotthard/config/sensors.yaml, track_name: FSG.sdf, autonomous_stack: } # Checks which must be true in order to automatically press RES GO res: 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/model.config b/fssim_gazebo/models/track/model.config index 8de1ec5..76e5742 100644 --- a/fssim_gazebo/models/track/model.config +++ b/fssim_gazebo/models/track/model.config @@ -6,7 +6,7 @@ track 1.0 - FSI.sdf + FSG.sdf random track 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 044ac67..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 @@ -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_track/src/gazebo_track.cpp b/fssim_gazebo_plugins/gazebo_track/src/gazebo_track.cpp index ea75402..e480530 100644 --- a/fssim_gazebo_plugins/gazebo_track/src/gazebo_track.cpp +++ b/fssim_gazebo_plugins/gazebo_track/src/gazebo_track.cpp @@ -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) { From 10f397215886def7caa3c62448e86b770a313dc1 Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Sat, 6 Nov 2021 13:04:41 +0800 Subject: [PATCH 03/12] More maps --- fssim_gazebo/models/track/acceleration.sdf | 159 ++- fssim_gazebo/models/track/backupFSG.sdf | 1082 ++++++++++++++++++++ fssim_gazebo/models/track/skidpad.sdf | 151 ++- 3 files changed, 1236 insertions(+), 156 deletions(-) create mode 100644 fssim_gazebo/models/track/backupFSG.sdf 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/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 From 0579139f8d0934dd7d143cb0f5d622245868597f Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Sat, 6 Nov 2021 13:44:05 +0800 Subject: [PATCH 04/12] TF Warning solved --- .../gazebo_race_car_model/include/vehicle.hpp | 1 + fssim_gazebo_plugins/gazebo_race_car_model/src/vehicle.cpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) 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 ee64cdb..07714b2 100644 --- a/fssim_gazebo_plugins/gazebo_race_car_model/include/vehicle.hpp +++ b/fssim_gazebo_plugins/gazebo_race_car_model/include/vehicle.hpp @@ -134,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/src/vehicle.cpp b/fssim_gazebo_plugins/gazebo_race_car_model/src/vehicle.cpp index 97cae84..761f8fe 100644 --- a/fssim_gazebo_plugins/gazebo_race_car_model/src/vehicle.cpp +++ b/fssim_gazebo_plugins/gazebo_race_car_model/src/vehicle.cpp @@ -248,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) { From 145208a05d5a9e568a17503f107b671a38b98a45 Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Sat, 6 Nov 2021 14:05:14 +0800 Subject: [PATCH 05/12] Changed dependencies --- README.md | 8 ++++---- .../rosdep/fssim-dependencies.yaml | 20 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index deffac9..9f64ac3 100644 --- a/README.md +++ b/README.md @@ -5,24 +5,24 @@ # 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` +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** initialized with `catkin init` 2. Run `cd src/fssim` from the workspace. 3. Run `./update_dependencies.sh`, you will need to approve multiple packages to be installed 4. Run `catkin build` -5. Source the workspace +5. 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_continuous_integration/rosdep/fssim-dependencies.yaml b/fssim_continuous_integration/rosdep/fssim-dependencies.yaml index 197c08d..5d038aa 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] + ubuntu: [python3-subprocess32] fsssim-python-lxml: - ubuntu: [python-lxml] + ubuntu: [python3-lxml] From 16a200d55b17e1302749b8850e82ce822faaae97 Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Sat, 6 Nov 2021 14:17:52 +0800 Subject: [PATCH 06/12] Cloning instructions --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 9f64ac3..fbe9ca6 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,11 @@ FSSIM is developed by [Juraj Kabzan](https://www.linkedin.com/in/juraj-kabzan-14 # How to Run It in your 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** initialized with `catkin init` +`mkdir ws +mkdir ws/src +cd ws +catkin init +git clone https://github.com/Huguet57/fssim-2021.git src` 2. Run `cd src/fssim` from the workspace. 3. Run `./update_dependencies.sh`, you will need to approve multiple packages to be installed 4. Run `catkin build` From 25f4f7e130f2fe3ea210abfdfc8854298d3ba9c6 Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Sat, 6 Nov 2021 14:18:48 +0800 Subject: [PATCH 07/12] Cloning instructions - Breaks --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index fbe9ca6..06a3d34 100644 --- a/README.md +++ b/README.md @@ -14,11 +14,11 @@ FSSIM is developed by [Juraj Kabzan](https://www.linkedin.com/in/juraj-kabzan-14 # How to Run It in your 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** initialized with `catkin init` -`mkdir ws -mkdir ws/src -cd ws -catkin init -git clone https://github.com/Huguet57/fssim-2021.git src` +`mkdir ws` +`mkdir ws/src` +`cd ws` +`catkin init` +`git clone https://github.com/Huguet57/fssim-2021.git src` 2. Run `cd src/fssim` from the workspace. 3. Run `./update_dependencies.sh`, you will need to approve multiple packages to be installed 4. Run `catkin build` From 00972ca34fa0fff34e9ef16fb36651c4fccd855a Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Sat, 6 Nov 2021 14:20:19 +0800 Subject: [PATCH 08/12] Cloning instructions - Readability --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 06a3d34..9a8547c 100644 --- a/README.md +++ b/README.md @@ -13,11 +13,11 @@ FSSIM is developed by [Juraj Kabzan](https://www.linkedin.com/in/juraj-kabzan-14 # How to Run It in your 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** initialized with `catkin init` -`mkdir ws` -`mkdir ws/src` -`cd ws` -`catkin init` +1. Clone this repository to an existing **ROS Workspace**, run: +`mkdir ws` -> +`mkdir ws/src` -> +`cd ws` -> +`catkin init` -> `git clone https://github.com/Huguet57/fssim-2021.git src` 2. Run `cd src/fssim` from the workspace. 3. Run `./update_dependencies.sh`, you will need to approve multiple packages to be installed From 1650b551152d7e21e8f4e6238557cff982b04a9d Mon Sep 17 00:00:00 2001 From: AHuguetUPC Date: Mon, 8 Nov 2021 20:34:56 +0800 Subject: [PATCH 09/12] Dependency updated --- fssim_continuous_integration/rosdep/fssim-dependencies.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fssim_continuous_integration/rosdep/fssim-dependencies.yaml b/fssim_continuous_integration/rosdep/fssim-dependencies.yaml index 5d038aa..b1e94aa 100644 --- a/fssim_continuous_integration/rosdep/fssim-dependencies.yaml +++ b/fssim_continuous_integration/rosdep/fssim-dependencies.yaml @@ -15,6 +15,6 @@ fssim-xacro: fssim-shapely: ubuntu: [python3-shapely] fssim-subprocess32: - ubuntu: [python3-subprocess32] + ubuntu: [python-subprocess32] fsssim-python-lxml: ubuntu: [python3-lxml] From 417ebfaa4b9737db703fc78a748d2e72eeb9cbe2 Mon Sep 17 00:00:00 2001 From: Andreu Huguet Date: Sun, 21 Nov 2021 23:12:14 +0800 Subject: [PATCH 10/12] Changed instructions for better cloning --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9a8547c..3f36307 100644 --- a/README.md +++ b/README.md @@ -18,8 +18,9 @@ FSSIM is developed by [Juraj Kabzan](https://www.linkedin.com/in/juraj-kabzan-14 `mkdir ws/src` -> `cd ws` -> `catkin init` -> -`git clone https://github.com/Huguet57/fssim-2021.git src` -2. Run `cd src/fssim` from the workspace. +`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 `source devel/setup.bash` From 232260eda5786d8e7b607f7ed955a09ad1e5db4a Mon Sep 17 00:00:00 2001 From: Andreu Huguet Date: Sun, 21 Nov 2021 23:16:41 +0800 Subject: [PATCH 11/12] Allow RViz --- fssim/launch/auto_fssim.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fssim/launch/auto_fssim.launch b/fssim/launch/auto_fssim.launch index f68013d..2d2d868 100644 --- a/fssim/launch/auto_fssim.launch +++ b/fssim/launch/auto_fssim.launch @@ -7,6 +7,6 @@ - + From ac1594d7801c7de4ffa8148bc80095297985412d Mon Sep 17 00:00:00 2001 From: Andreu Huguet Date: Mon, 29 Nov 2021 19:51:25 +0800 Subject: [PATCH 12/12] Explanation on sourcing --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 3f36307..e777f75 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ FSSIM is developed by [Juraj Kabzan](https://www.linkedin.com/in/juraj-kabzan-14 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 `source devel/setup.bash` +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.