diff --git a/subt_ign/launch/cave_circuit.ign b/subt_ign/launch/cave_circuit.ign index 04fdada6..15dccc7d 100644 --- a/subt_ign/launch/cave_circuit.ign +++ b/subt_ign/launch/cave_circuit.ign @@ -36,6 +36,8 @@ spawnWorldYaw = 0 guiCameraPose = "-6.3 -4.2 3.6 0 0.268 0.304" + teamBaseSpawned = false + # Check if robotNameX and robotConfigX exists spawnRowSize = 4 spawnColSize = 5 @@ -1038,6 +1040,38 @@ " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ "\n" end + + def spawnTeamBase(_x, _y, _z, _yaw) + "\n"\ + " TeamBase\n"\ + " false\n"\ + " #{$worldName}\n"\ + " false\n"\ + " \n"\ + " \n"\ + " #{_x} #{_y} #{_z} 0 0 #{_yaw}\n"\ + " true\n"\ + " \n"\ + " 0 0 0.05 0 0 0\n"\ + " \n"\ + " \n"\ + " \n"\ + " .1 .1 .1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName}\n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName}\n"\ + "\n" + end %> <% @@ -1073,6 +1107,15 @@ %> <%= spawnX4(name, robotConfigN, posX, posY, spawnWorldZPos, spawnWorldYaw) %> <% + elsif (name.downcase == "teambase" || name.downcase == "team_base") && + (config.downcase == "teambase" || config.downcase == "team_base") + robotSpawned += 1 + if !teamBaseSpawned + teamBaseSpawned = true +%> +<%= spawnTeamBase(posX, posY, spawnWorldZPos, spawnWorldYaw) %> +<% + end else # Try a team-submitted vehicle. package = config.downcase diff --git a/subt_ign/launch/cloudsim_bridge.ign b/subt_ign/launch/cloudsim_bridge.ign index 3781781d..f08a585a 100644 --- a/subt_ign/launch/cloudsim_bridge.ign +++ b/subt_ign/launch/cloudsim_bridge.ign @@ -41,6 +41,8 @@ %> <% + teamBaseSpawned = false + # Check if robotNameX and robotConfigX exists maxRobotCount = 20 robotNames = [] @@ -174,6 +176,15 @@ " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ "\n" end + + def spawnTeamBase(_x, _y, _z, _yaw) + "\n"\ + " roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName}\n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName}\n"\ + "\n" + end %> <% @@ -189,7 +200,16 @@ <%= spawnX3(robotNames[i], robotConfigs[i][-1]) %> <% elsif robotConfigs[i][1] == "4" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5") %> <%= spawnX4(robotNames[i], robotConfigs[i][-1]) %> -<% else +<% + elsif (name.downcase == "teambase" || name.downcase == "team_base") && + (config.downcase == "teambase" || config.downcase == "team_base") + robotSpawned += 1 + if !teamBaseSpawned + teamBaseSpawned = true +%> +<%= spawnTeamBase(posX, posY, spawnWorldZPos, spawnWorldYaw) %> +<% end + else # Try a team-submitted vehicle. package = config.downcase installDir = `rospack find #{package}`.chomp diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index 95e6faba..3b0558e1 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -57,6 +57,8 @@ guiCameraPose = "-6.3 -4.2 3.6 0 0.268 0.304" end + teamBaseSpawned = false + # Check if robotNameX and robotConfigX exists spawnRowSize = 4 spawnColSize = 5 @@ -1038,6 +1040,32 @@ " \n"\ "\n" end + + def spawnTeamBase(_x, _y, _z, _yaw) + "\n"\ + " TeamBase\n"\ + " false\n"\ + " #{$worldName}\n"\ + " false\n"\ + " \n"\ + " \n"\ + " #{_x} #{_y} #{_z} 0 0 #{_yaw}\n"\ + " true\n"\ + " \n"\ + " 0 0 0.05 0 0 0\n"\ + " \n"\ + " \n"\ + " \n"\ + " .1 .1 .1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + "\n" + end %> <% @@ -1073,6 +1101,15 @@ %> <%= spawnX4(name, robotConfigN, posX, posY, spawnWorldZPos, spawnWorldYaw) %> <% + elsif (name.downcase == "teambase" || name.downcase == "team_base") && + (config.downcase == "teambase" || config.downcase == "team_base") + robotSpawned += 1 + if !teamBaseSpawned + teamBaseSpawned = true +%> +<%= spawnTeamBase(posX, posY, spawnWorldZPos, spawnWorldYaw) %> +<% + end else # Try a team-submitted vehicle. package = config.downcase diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign index 90fd87dc..6b2058a2 100644 --- a/subt_ign/launch/competition.ign +++ b/subt_ign/launch/competition.ign @@ -26,6 +26,8 @@ %> <% + teamBaseSpawned = false + # Check if robotNameX and robotConfigX exists spawnRowSize = 4 spawnColSize = 5 @@ -946,6 +948,38 @@ " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ "\n" end + + def spawnTeamBase(_x, _y, _z, _yaw) + "\n"\ + " TeamBase\n"\ + " false\n"\ + " #{$worldName}\n"\ + " false\n"\ + " \n"\ + " \n"\ + " #{_x} #{_y} #{_z} 0 0 #{_yaw}\n"\ + " true\n"\ + " \n"\ + " 0 0 0.05 0 0 0\n"\ + " \n"\ + " \n"\ + " \n"\ + " .1 .1 .1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName}\n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName}\n"\ + "\n" + end %> <% @@ -975,6 +1009,15 @@ %> <%= spawnX4(name, robotConfigN, posX, posY) %> <% + elsif (name.downcase == "teambase" || name.downcase == "team_base") && + (config.downcase == "teambase" || config.downcase == "team_base") + robotSpawned += 1 + if !teamBaseSpawned + teamBaseSpawned = true +%> +<%= spawnTeamBase(posX, posY, spawnWorldZPos, spawnWorldYaw) %> +<% + end else # Try a team-submitted vehicle. package = config.downcase diff --git a/subt_ign/launch/tunnel_circuit_practice.ign b/subt_ign/launch/tunnel_circuit_practice.ign index e1779cce..2498a710 100644 --- a/subt_ign/launch/tunnel_circuit_practice.ign +++ b/subt_ign/launch/tunnel_circuit_practice.ign @@ -29,6 +29,8 @@ %> <% + teamBaseSpawned = false + # Check if robotNameX and robotConfigX exists spawnRowSize = 4 spawnColSize = 5 @@ -938,6 +940,38 @@ " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ "\n" end + + def spawnTeamBase(_x, _y, _z, _yaw) + "\n"\ + " TeamBase\n"\ + " false\n"\ + " #{$worldName}\n"\ + " false\n"\ + " \n"\ + " \n"\ + " #{_x} #{_y} #{_z} 0 0 #{_yaw}\n"\ + " true\n"\ + " \n"\ + " 0 0 0.05 0 0 0\n"\ + " \n"\ + " \n"\ + " \n"\ + " .1 .1 .1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName}\n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName}\n"\ + "\n" + end %> <% @@ -967,6 +1001,15 @@ %> <%= spawnX4(name, robotConfigN, posX, posY) %> <% + elsif (name.downcase == "teambase" || name.downcase == "team_base") && + (config.downcase == "teambase" || config.downcase == "team_base") + robotSpawned += 1 + if !teamBaseSpawned + teamBaseSpawned = true +%> +<%= spawnTeamBase(posX, posY, spawnWorldZPos, spawnWorldYaw) %> +<% + end else # Try a team-submitted vehicle. package = config.downcase diff --git a/subt_ign/launch/urban_circuit.ign b/subt_ign/launch/urban_circuit.ign index e9afb7d4..3cf97dc8 100644 --- a/subt_ign/launch/urban_circuit.ign +++ b/subt_ign/launch/urban_circuit.ign @@ -35,6 +35,8 @@ spawnWorldZPos = 7.5 spawnWorldYaw = 0 + teamBaseSpawned = false + # Check if robotNameX and robotConfigX exists spawnRowSize = 4 spawnColSize = 5 @@ -999,6 +1001,38 @@ " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ "\n" end + + def spawnTeamBase(_x, _y, _z, _yaw) + "\n"\ + " TeamBase\n"\ + " false\n"\ + " #{$worldName}\n"\ + " false\n"\ + " \n"\ + " \n"\ + " #{_x} #{_y} #{_z} 0 0 #{_yaw}\n"\ + " true\n"\ + " \n"\ + " 0 0 0.05 0 0 0\n"\ + " \n"\ + " \n"\ + " \n"\ + " .1 .1 .1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName}\n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName}\n"\ + "\n" + end %> <% @@ -1034,6 +1068,15 @@ %> <%= spawnX4(name, robotConfigN, posX, posY, spawnWorldZPos, spawnWorldYaw) %> <% + elsif (name.downcase == "teambase" || name.downcase == "team_base") && + (config.downcase == "teambase" || config.downcase == "team_base") + robotSpawned += 1 + if !teamBaseSpawned + teamBaseSpawned = true +%> +<%= spawnTeamBase(posX, posY, spawnWorldZPos, spawnWorldYaw) %> +<% + end else # Try a team-submitted vehicle. package = config.downcase diff --git a/subt_ign/launch/virtual_stix.ign b/subt_ign/launch/virtual_stix.ign index 9e692d1b..a49cfeff 100644 --- a/subt_ign/launch/virtual_stix.ign +++ b/subt_ign/launch/virtual_stix.ign @@ -26,6 +26,8 @@ %> <% + teamBaseSpawned = false + # Check if robotNameX and robotConfigX exists spawnRowSize = 4 spawnColSize = 5 @@ -908,6 +910,38 @@ " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ "\n" end + + def spawnTeamBase(_x, _y, _z, _yaw) + "\n"\ + " TeamBase\n"\ + " false\n"\ + " #{$worldName}\n"\ + " false\n"\ + " \n"\ + " \n"\ + " #{_x} #{_y} #{_z} 0 0 #{_yaw}\n"\ + " true\n"\ + " \n"\ + " 0 0 0.05 0 0 0\n"\ + " \n"\ + " \n"\ + " \n"\ + " .1 .1 .1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName}\n"\ + "\n"\ + "\n"\ + " roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName}\n"\ + "\n" + end %> <% @@ -937,6 +971,15 @@ %> <%= spawnX4(name, robotConfigN, posX, posY) %> <% + elsif (name.downcase == "teambase" || name.downcase == "team_base") && + (config.downcase == "teambase" || config.downcase == "team_base") + robotSpawned += 1 + if !teamBaseSpawned + teamBaseSpawned = true +%> +<%= spawnTeamBase(posX, posY, spawnWorldZPos, spawnWorldYaw) %> +<% + end else # Try a team-submitted vehicle. package = config.downcase diff --git a/subt_ros/launch/teambase_description.launch b/subt_ros/launch/teambase_description.launch new file mode 100644 index 00000000..fae2f5b4 --- /dev/null +++ b/subt_ros/launch/teambase_description.launch @@ -0,0 +1,4 @@ + + + + diff --git a/subt_ros/launch/teambase_topics.launch b/subt_ros/launch/teambase_topics.launch new file mode 100644 index 00000000..26de6110 --- /dev/null +++ b/subt_ros/launch/teambase_topics.launch @@ -0,0 +1,4 @@ + + + + diff --git a/teambase_description/CMakeLists.txt b/teambase_description/CMakeLists.txt new file mode 100644 index 00000000..a6b17bc6 --- /dev/null +++ b/teambase_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 2.8.3) +project(teambase_description) + +find_package(catkin REQUIRED COMPONENTS roslaunch) + +catkin_package() + +install( + DIRECTORY urdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/teambase_description/package.xml b/teambase_description/package.xml new file mode 100644 index 00000000..0c2636cd --- /dev/null +++ b/teambase_description/package.xml @@ -0,0 +1,14 @@ + + + teambase_description + 0.0.1 + The teambase_description package + Nate Koenig + Nate Koenig + Apache2 + + catkin + roslaunch + urdf + xacro + diff --git a/teambase_description/urdf/teambase.xacro b/teambase_description/urdf/teambase.xacro new file mode 100644 index 00000000..282359a6 --- /dev/null +++ b/teambase_description/urdf/teambase.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + +