From 926415ccd9ec50fd72926c8746fc3ccdffd872e9 Mon Sep 17 00:00:00 2001 From: Angela Maio Date: Thu, 6 Aug 2020 23:54:51 -0400 Subject: [PATCH 1/2] rotate marsupial children backwards for R2 and OZBOT --- subt_ign/launch/cave_circuit.ign | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/subt_ign/launch/cave_circuit.ign b/subt_ign/launch/cave_circuit.ign index 0583dbe6..03e699f0 100644 --- a/subt_ign/launch/cave_circuit.ign +++ b/subt_ign/launch/cave_circuit.ign @@ -119,6 +119,13 @@ "CSIRO_DATA61_OZBOT_ATR" => Vector3d.new(-0.15, 0, 0.528) } + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = { + "X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_R2" => AngularVector3d.new(0, 0, -3.1416), + "CSIRO_DATA61_OZBOT_ATR" => AngularVector3d.new(0, 0, -3.1416) + } + allExecutables = "" # key: parent, value: child @@ -1366,7 +1373,9 @@ robot.pos.x = parentRobot.pos.x + Math.cos(-spawnWorldYaw) * offsetX + Math.sin(-spawnWorldYaw) * offsetY robot.pos.y = parentRobot.pos.y - Math.sin(-spawnWorldYaw) * offsetX + Math.cos(-spawnWorldYaw) * offsetY robot.pos.z = parentRobot.pos.z + MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS[parentRobot.type].z - robot.rot = parentRobot.rot + robot.rot.roll = parentRobot.rot.roll + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].roll + robot.rot.pitch = parentRobot.rot.pitch + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].pitch + robot.rot.yaw = parentRobot.rot.yaw + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].yaw else x = Math.cos(-spawnWorldYaw) * posX + Math.sin(-spawnWorldYaw) * posY y = -(Math.sin(-spawnWorldYaw)) * posX + Math.cos(-spawnWorldYaw) * posY From 4c853bc2b8b86d03556ae259ff98f8a469ac23a4 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 7 Aug 2020 05:43:21 -0700 Subject: [PATCH 2/2] Added rotations to other launch files Signed-off-by: Nate Koenig --- subt_ign/launch/cloudsim_sim.ign | 11 ++++++++++- subt_ign/launch/competition.ign | 11 ++++++++++- subt_ign/launch/tunnel_circuit_practice.ign | 11 ++++++++++- subt_ign/launch/urban_circuit.ign | 11 ++++++++++- 4 files changed, 40 insertions(+), 4 deletions(-) diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index b2defef7..f30c3598 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -126,6 +126,13 @@ "CSIRO_DATA61_OZBOT_ATR" => Vector3d.new(-0.15, 0, 0.528) } + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = { + "X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_R2" => AngularVector3d.new(0, 0, -3.1416), + "CSIRO_DATA61_OZBOT_ATR" => AngularVector3d.new(0, 0, -3.1416) + } + allExecutables = "" # key: parent, value: child @@ -1407,7 +1414,9 @@ robot.pos.x = parentRobot.pos.x + Math.cos(-spawnWorldYaw) * offsetX + Math.sin(-spawnWorldYaw) * offsetY robot.pos.y = parentRobot.pos.y - Math.sin(-spawnWorldYaw) * offsetX + Math.cos(-spawnWorldYaw) * offsetY robot.pos.z = parentRobot.pos.z + MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS[parentRobot.type].z - robot.rot = parentRobot.rot + robot.rot.roll = parentRobot.rot.roll + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].roll + robot.rot.pitch = parentRobot.rot.pitch + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].pitch + robot.rot.yaw = parentRobot.rot.yaw + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].yaw else x = Math.cos(-spawnWorldYaw) * posX + Math.sin(-spawnWorldYaw) * posY y = -(Math.sin(-spawnWorldYaw)) * posX + Math.cos(-spawnWorldYaw) * posY diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign index 00ca7dc4..d886879f 100644 --- a/subt_ign/launch/competition.ign +++ b/subt_ign/launch/competition.ign @@ -116,6 +116,13 @@ "CSIRO_DATA61_OZBOT_ATR" => Vector3d.new(-0.15, 0, 0.528) } + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = { + "X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_R2" => AngularVector3d.new(0, 0, -3.1416), + "CSIRO_DATA61_OZBOT_ATR" => AngularVector3d.new(0, 0, -3.1416) + } + allExecutables = "" # key: parent, value: child @@ -1363,7 +1370,9 @@ robot.pos.x = parentRobot.pos.x + Math.cos(-spawnWorldYaw) * offsetX + Math.sin(-spawnWorldYaw) * offsetY robot.pos.y = parentRobot.pos.y - Math.sin(-spawnWorldYaw) * offsetX + Math.cos(-spawnWorldYaw) * offsetY robot.pos.z = parentRobot.pos.z + MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS[parentRobot.type].z - robot.rot = parentRobot.rot + robot.rot.roll = parentRobot.rot.roll + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].roll + robot.rot.pitch = parentRobot.rot.pitch + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].pitch + robot.rot.yaw = parentRobot.rot.yaw + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].yaw else x = Math.cos(-spawnWorldYaw) * posX + Math.sin(-spawnWorldYaw) * posY y = -(Math.sin(-spawnWorldYaw)) * posX + Math.cos(-spawnWorldYaw) * posY diff --git a/subt_ign/launch/tunnel_circuit_practice.ign b/subt_ign/launch/tunnel_circuit_practice.ign index dd650cc0..9cac2e3f 100644 --- a/subt_ign/launch/tunnel_circuit_practice.ign +++ b/subt_ign/launch/tunnel_circuit_practice.ign @@ -119,6 +119,13 @@ "CSIRO_DATA61_OZBOT_ATR" => Vector3d.new(-0.15, 0, 0.528) } + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = { + "X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_R2" => AngularVector3d.new(0, 0, -3.1416), + "CSIRO_DATA61_OZBOT_ATR" => AngularVector3d.new(0, 0, -3.1416) + } + allExecutables = "" # key: parent, value: child @@ -1366,7 +1373,9 @@ robot.pos.x = parentRobot.pos.x + Math.cos(-spawnWorldYaw) * offsetX + Math.sin(-spawnWorldYaw) * offsetY robot.pos.y = parentRobot.pos.y - Math.sin(-spawnWorldYaw) * offsetX + Math.cos(-spawnWorldYaw) * offsetY robot.pos.z = parentRobot.pos.z + MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS[parentRobot.type].z - robot.rot = parentRobot.rot + robot.rot.roll = parentRobot.rot.roll + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].roll + robot.rot.pitch = parentRobot.rot.pitch + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].pitch + robot.rot.yaw = parentRobot.rot.yaw + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].yaw else x = Math.cos(-spawnWorldYaw) * posX + Math.sin(-spawnWorldYaw) * posY y = -(Math.sin(-spawnWorldYaw)) * posX + Math.cos(-spawnWorldYaw) * posY diff --git a/subt_ign/launch/urban_circuit.ign b/subt_ign/launch/urban_circuit.ign index 3e8daf36..f7934204 100644 --- a/subt_ign/launch/urban_circuit.ign +++ b/subt_ign/launch/urban_circuit.ign @@ -119,6 +119,13 @@ "CSIRO_DATA61_OZBOT_ATR" => Vector3d.new(-0.15, 0, 0.528) } + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = { + "X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_X1" => AngularVector3d.new(0, 0, 0), + "EXPLORER_R2" => AngularVector3d.new(0, 0, -3.1416), + "CSIRO_DATA61_OZBOT_ATR" => AngularVector3d.new(0, 0, -3.1416) + } + allExecutables = "" # key: parent, value: child @@ -1366,7 +1373,9 @@ robot.pos.x = parentRobot.pos.x + Math.cos(-spawnWorldYaw) * offsetX + Math.sin(-spawnWorldYaw) * offsetY robot.pos.y = parentRobot.pos.y - Math.sin(-spawnWorldYaw) * offsetX + Math.cos(-spawnWorldYaw) * offsetY robot.pos.z = parentRobot.pos.z + MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS[parentRobot.type].z - robot.rot = parentRobot.rot + robot.rot.roll = parentRobot.rot.roll + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].roll + robot.rot.pitch = parentRobot.rot.pitch + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].pitch + robot.rot.yaw = parentRobot.rot.yaw + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].yaw else x = Math.cos(-spawnWorldYaw) * posX + Math.sin(-spawnWorldYaw) * posY y = -(Math.sin(-spawnWorldYaw)) * posX + Math.cos(-spawnWorldYaw) * posY