diff --git a/examples/worlds/conveyor.sdf b/examples/worlds/conveyor.sdf new file mode 100644 index 0000000000..1c9bf44cbd --- /dev/null +++ b/examples/worlds/conveyor.sdf @@ -0,0 +1,350 @@ + + + + + + 0.004 + 1.0 + + + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 1 + + 0 0 0 0 0 0 + + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0 0 0 0 + + + 5 0.2 0.1 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0 0 0 0 + + + 5 0.2 0.1 + + + + + 2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + -2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + 1 + 0 + + + + base_link + + + + + + + 87 + + + data: 10.0 + + + + + + + 88 + + + data: -1.0 + + + + + + + 83 + + + data: 0.0 + + + + + + 0 0 1 0 0 0 + + + 1.06 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + + + 0 0 0 0 0 0 + + + 0.1 0.1 0.1 + + + + 1 1 1 1 + + + + + + 0.1 0.1 0.1 + + + 0 0 0 0 0 0 + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + Transform control + + + + + false + 230 + 50 + floating + false + #666666 + + + + + + + + + + + false + 200 + 50 + floating + false + #666666 + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/worlds/tracked_vehicle_simple.sdf b/examples/worlds/tracked_vehicle_simple.sdf new file mode 100644 index 0000000000..cf9cd29acc --- /dev/null +++ b/examples/worlds/tracked_vehicle_simple.sdf @@ -0,0 +1,1822 @@ + + + + + 0.004 + 1.0 + + + + + + 1 1 1 1 + 0.8 0.8 0.8 1 + 1 + + + 1 + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 3 0 0.1 0 0 0 + + 0 0 0 0 0 0 + + -0.122 0 0.118 1.5708 0 0 + 13.14 + + 0.10019 + 0 + 0 + 0.345043 + 0 + 0.302044 + + + + -0.122 0 0.118 0 0 0 + + + 0.50017 0.24093 0.139 + + + + + -0.122 0 0.118 0 0 0 + + + 0.50017 0.24093 0.139 + + + + 0 + 1 + 0 + + + 0 0.1985 0 0 0 0 + + 0 0 0.0141 0 0 0 + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + 1 + 0 + + + left_track + base_link + + + 0 -0.1985 0 0 0 0 + + 0 0 0.0141 0 0 0 + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + 1 + 0 + + + right_track + base_link + + + 0.25 0.272 0.0195 0 -0.5 0 + + 0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + front_left_flipper + left_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + -0.25 0.272 0.0195 3.14159 -0.5 3.14159 + + 0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + rear_left_flipper + left_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + 0.25 -0.272 0.0195 3.14159 0.5 3.14159 + + -0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + front_right_flipper + right_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + -0.25 -0.272 0.0195 0 0.5 0 + + -0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + rear_right_flipper + right_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + + left_track + front_left_flipper + rear_left_flipper + right_track + front_right_flipper + rear_right_flipper + 0.4 + 0.18094 + 0.5 + + + + left_track + -1.0 + 1.0 + + + + right_track + -1.0 + 1.0 + + + front_left_flipper + -1.0 + 1.0 + + + rear_left_flipper + -1.0 + 1.0 + + + front_right_flipper + -1.0 + 1.0 + + + rear_right_flipper + -1.0 + 1.0 + + + + + + 87 + + + linear: {x: 1.0}, angular: {z: 0.0} + + + + + + + 88 + + + linear: {x: -1.0}, angular: {z: 0.0} + + + + + + + 83 + + + linear: {x: 0.0}, angular: {z: 0.0} + + + + + + + 65 + + + linear: {x: 0.0}, angular: {z: 1.0} + + + + + + + 68 + + + linear: {x: 0.0}, angular: {z: -1.0} + + + + + + + 81 + + + linear: {x: 1.0}, angular: {z: 1.0} + + + + + + + 69 + + + linear: {x: 1.0}, angular: {z: -1.0} + + + + + + + 90 + + + linear: {x: -1.0}, angular: {z: 1.0} + + + + + + + 67 + + + linear: {x: -1.0}, angular: {z: -1.0} + + + + + 7 0 0 0 0 1.5708 + + + 0 1.6625 0.0375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.6625 0.0375 0 0 0 + + + 0 1.4875 0.1125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.4875 0.1125 0 0 0 + + + 0 1.3125 0.1875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.3125 0.1875 0 0 0 + + + 0 1.1375 0.2625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.1375 0.2625 0 0 0 + + + 0 0.9625 0.3375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.9625 0.3375 0 0 0 + + + 0 0.7875 0.4125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.7875 0.4125 0 0 0 + + + 0 0.6125 0.4875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.6125 0.4875 0 0 0 + + + 0 0.4375 0.5625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.4375 0.5625 0 0 0 + + + 0 0.2625 0.6375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.2625 0.6375 0 0 0 + + + 0 0.0875 0.7125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.0875 0.7125 0 0 0 + + + 0 -0.0875 0.7875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.0875 0.7875 0 0 0 + + + 0 -0.2625 0.8625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.2625 0.8625 0 0 0 + + + 0 -0.4375 0.9375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.4375 0.9375 0 0 0 + + + 0 -0.6125 1.0125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.6125 1.0125 0 0 0 + + + 0 -0.7875 1.0875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.7875 1.0875 0 0 0 + + + 0 -0.9625 1.1625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.9625 1.1625 0 0 0 + + + 0 -1.1375 1.2375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.1375 1.2375 0 0 0 + + + 0 -1.3125 1.3125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.3125 1.3125 0 0 0 + + + 0 -1.4875 1.3875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.4875 1.3875 0 0 0 + + + 0 -1.6625 1.4625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.6625 1.4625 0 0 0 + + + 1 + + + 1 + 0 0 -0.06 0 0 1.5708 + + + 0 0 0 0 1.5708 0 + + + 0.15 + 0.8 + + + + + + 1 + + + + + + 0 0 0 0 1.5708 0 + + + 0.15 + 0.8 + + + + + + + 1 + 2 2 0.028 1.7821 0 1.5708 + + + 0 0 0 0 0 0 + + + 0.85 0.15 0.5 + + + + + + 1 + + + + + + 0 0 0 0 0 0 + + + 0.85 0.15 0.5 + + + + + + + pallet + 2 -2 0.1 0 0 0 + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Euro pallet + + + + + + 3D View + 0 + docked + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + World control + 0 + 0 + 72 + 121 + 1 + floating + + + + + + 1 + 1 + 1 + + + + World stats + 0 + 0 + 110 + 290 + 1 + floating + + + + + + 1 + 1 + 1 + 1 + + + + Transform control + + + + + 0 + 230 + 50 + floating + 0 + #666666 + + + + + + + + + 0 + 200 + 50 + floating + 0 + #666666 + + + + + + + + + false + 5 + 5 + floating + false + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + \ No newline at end of file diff --git a/include/ignition/gazebo/components/Collision.hh b/include/ignition/gazebo/components/Collision.hh index 9c57c90c4c..f35f0e95d9 100644 --- a/include/ignition/gazebo/components/Collision.hh +++ b/include/ignition/gazebo/components/Collision.hh @@ -51,6 +51,15 @@ namespace components serializers::CollisionElementSerializer>; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CollisionElement", CollisionElement) + + /// \brief A component used to enable customization of contact surface for a + /// collision. The customization itself is done in callback of event + /// CollectContactSurfaceProperties from PhysicsEvents. + using EnableContactSurfaceCustomization = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.EnableContactSurfaceCustomization", + EnableContactSurfaceCustomization) } } } diff --git a/include/ignition/gazebo/physics/Events.hh b/include/ignition/gazebo/physics/Events.hh new file mode 100644 index 0000000000..668e19a2ad --- /dev/null +++ b/include/ignition/gazebo/physics/Events.hh @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_PHYSICS_EVENTS_HH_ +#define IGNITION_GAZEBO_PHYSICS_EVENTS_HH_ + +#include + +#include + +#include + +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/Entity.hh" + +#include + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + + namespace events + { + using Policy = physics::FeaturePolicy3d; + + /// \brief This event is called when the physics engine needs to collect + /// what customizations it should do to the surface of a contact point. It + /// is called during the Update phase after collision checking has been + /// finished and before the physics update has happened. The event + /// subscribers are expected to change the `params` argument. + using CollectContactSurfaceProperties = ignition::common::EventT< + void( + const Entity& /* collision1 */, + const Entity& /* collision2 */, + const math::Vector3d & /* point */, + const std::optional /* force */, + const std::optional /* normal */, + const std::optional /* depth */, + const size_t /* numContactsOnCollision */, + physics::SetContactPropertiesCallbackFeature:: + ContactSurfaceParams& /* params */ + ), + struct CollectContactSurfacePropertiesTag>; + } + } // namespace events + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PHYSICS_EVENTS_HH_ diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index dfc14c6cbe..aa4b5ce018 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -102,6 +102,8 @@ add_subdirectory(scene_broadcaster) add_subdirectory(sensors) add_subdirectory(thermal) add_subdirectory(touch_plugin) +add_subdirectory(track_controller) +add_subdirectory(tracked_vehicle) add_subdirectory(triggered_publisher) add_subdirectory(user_commands) add_subdirectory(velocity_control) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 2fcfdc7c0e..647250b4bf 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -122,6 +123,9 @@ #include "ignition/gazebo/components/ThreadPitch.hh" #include "ignition/gazebo/components/World.hh" +// Events +#include "ignition/gazebo/physics/Events.hh" + #include "EntityFeatureMap.hh" using namespace ignition; @@ -208,6 +212,14 @@ class ignition::gazebo::systems::PhysicsPrivate public: ignition::math::Pose3d RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const; + /// \brief Enable contact surface customization for the given world. + /// \param[in] _world The world to enable it for. + public: void EnableContactSurfaceCustomization(const Entity &_world); + + /// \brief Disable contact surface customization for the given world. + /// \param[in] _world The world to disable it for. + public: void DisableContactSurfaceCustomization(const Entity &_world); + /// \brief Cache the top-level model for each entity. /// The key is an entity and the value is its top level model. public: std::unordered_map topLevelModelMap; @@ -223,6 +235,9 @@ class ignition::gazebo::systems::PhysicsPrivate /// deleted the following iteration. public: std::unordered_set worldPoseCmdsToRemove; + /// \brief IDs of the ContactSurfaceHandler callbacks registered for worlds + public: std::unordered_map worldContactCallbackIDs; + /// \brief used to store whether physics objects have been created. public: bool initialized = false; @@ -303,6 +318,17 @@ class ignition::gazebo::systems::PhysicsPrivate ignition::physics::GetContactsFromLastStepFeature, ignition::physics::sdf::ConstructSdfCollision>{}; + /// \brief Feature list to handle contacts information. + public: struct ContactFeatureList : ignition::physics::FeatureList< + CollisionFeatureList, + ignition::physics::GetContactsFromLastStepFeature>{}; + + /// \brief Feature list to change contacts before they are applied to physics. + public: struct SetContactPropertiesCallbackFeatureList : + ignition::physics::FeatureList< + ContactFeatureList, + ignition::physics::SetContactPropertiesCallbackFeature>{}; + /// \brief Collision type with collision features. public: using ShapePtrType = ignition::physics::ShapePtr< ignition::physics::FeaturePolicy3d, CollisionFeatureList>; @@ -393,6 +419,8 @@ class ignition::gazebo::systems::PhysicsPrivate physics::World, MinimumFeatureList, CollisionFeatureList, + ContactFeatureList, + SetContactPropertiesCallbackFeatureList, NestedModelFeatureList>; /// \brief A map between world entity ids in the ECM to World Entities in @@ -461,6 +489,15 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief A map between collision entity ids in the ECM to FreeGroup Entities /// in ign-physics. public: EntityFreeGroupMap entityFreeGroupMap; + + /// \brief Event manager from simulation runner. + public: EventManager *eventManager = nullptr; + + /// \brief Keep track of what entities use customized contact surfaces. + /// Map keys are expected to be world entities so that we keep a set of + /// entities with customizations per world. + public: std::unordered_map> + customContactSurfaceEntities; }; ////////////////////////////////////////////////// @@ -472,7 +509,7 @@ Physics::Physics() : System(), dataPtr(std::make_unique()) void Physics::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) + EventManager &_eventMgr) { std::string pluginLib; @@ -585,7 +622,10 @@ void Physics::Configure(const Entity &_entity, ignerr << "Failed to load a valid physics engine from [" << pathToLib << "]." << std::endl; + return; } + + this->dataPtr->eventManager = &_eventMgr; } ////////////////////////////////////////////////// @@ -1145,6 +1185,41 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) } return true; }); + + // The components are removed after each update, so we want to process all + // components in every update. + _ecm.Each( + [&](const Entity & _entity, + const components::EnableContactSurfaceCustomization *_enable, + const components::Collision */*_collision*/, + const components::Name *_name) -> bool + { + const auto world = worldEntity(_entity, _ecm); + if (_enable->Data()) + { + if (this->customContactSurfaceEntities[world].empty()) + { + this->EnableContactSurfaceCustomization(world); + } + this->customContactSurfaceEntities[world].insert(_entity); + ignmsg << "Enabling contact surface customization for collision [" + << _name->Data() << "]" << std::endl; + } + else + { + if (this->customContactSurfaceEntities[world].erase(_entity) > 0) + { + ignmsg << "Disabling contact surface customization for collision [" + << _name->Data() << "]" << std::endl; + if (this->customContactSurfaceEntities[world].empty()) + { + this->DisableContactSurfaceCustomization(world); + } + } + } + return true; + }); } ////////////////////////////////////////////////// @@ -1161,6 +1236,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) [&](const Entity &_entity, const components::Model * /* _model */) -> bool { + const auto world = worldEntity(_ecm); // Remove model if found if (auto modelPtrPhys = this->entityModelMap.Get(_entity)) { @@ -1173,6 +1249,16 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) { this->entityCollisionMap.Remove(childCollision); this->topLevelModelMap.erase(childCollision); + if (this->customContactSurfaceEntities[world].erase( + childCollision)) + { + // if this was the last collision with contact customization, + // disable the whole feature in the physics engine + if (this->customContactSurfaceEntities[world].empty()) + { + this->DisableContactSurfaceCustomization(world); + } + } } this->entityLinkMap.Remove(childLink); this->topLevelModelMap.erase(childLink); @@ -2288,6 +2374,20 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) _ecm.RemoveComponent(entity); } + std::vector entitiesCustomContactSurface; + _ecm.Each( + [&](const Entity &_entity, + components::EnableContactSurfaceCustomization *) -> bool + { + entitiesCustomContactSurface.push_back(_entity); + return true; + }); + + for (const auto entity : entitiesCustomContactSurface) + { + _ecm.RemoveComponent(entity); + } + // Clear pending commands _ecm.Each( [&](const Entity &, components::JointForceCmd *_force) -> bool @@ -2502,6 +2602,7 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) }); } +////////////////////////////////////////////////// physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset( const LinkPtrType &_link, const math::Pose3d &_pose) const { @@ -2511,6 +2612,97 @@ physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset( return this->engine->Resolve(relFrameData, physics::FrameID::World()); } +////////////////////////////////////////////////// +void PhysicsPrivate::EnableContactSurfaceCustomization(const Entity &_world) +{ + // allow customization of contact joint surface parameters + auto setContactPropertiesCallbackFeature = + this->entityWorldMap.EntityCast< + SetContactPropertiesCallbackFeatureList>(_world); + if (!setContactPropertiesCallbackFeature) + return; + + using Policy = physics::FeaturePolicy3d; + using Feature = physics::SetContactPropertiesCallbackFeature; + using FeatureList = SetContactPropertiesCallbackFeatureList; + using GCFeature = physics::GetContactsFromLastStepFeature; + using GCFeatureWorld = GCFeature::World; + using ContactPoint = GCFeatureWorld::ContactPoint; + using ExtraContactData = GCFeature::ExtraContactDataT; + + const auto callbackID = "ignition::gazebo::systems::Physics"; + setContactPropertiesCallbackFeature->AddContactPropertiesCallback( + callbackID, + [this, _world](const GCFeatureWorld::Contact &_contact, + const size_t _numContactsOnCollision, + Feature::ContactSurfaceParams &_params) + { + const auto &contact = _contact.Get(); + auto coll1Entity = this->entityCollisionMap.Get( + ShapePtrType(contact.collision1)); + auto coll2Entity = this->entityCollisionMap.Get( + ShapePtrType(contact.collision2)); + + // check if at least one of the entities wants contact surface + // customization + if (this->customContactSurfaceEntities[_world].find(coll1Entity) == + this->customContactSurfaceEntities[_world].end() && + this->customContactSurfaceEntities[_world].find(coll2Entity) == + this->customContactSurfaceEntities[_world].end()) + { + return; + } + + std::optional force; + std::optional normal; + std::optional depth; + const auto* extraData = _contact.Query(); + if (extraData != nullptr) + { + force = math::eigen3::convert(extraData->force); + normal = math::eigen3::convert(extraData->normal); + depth = extraData->depth; + } + + // broadcast the event that we want to collect the customized + // contact surface properties; each connected client should + // filter in the callback to treat just the entities it knows + this->eventManager-> + Emit( + coll1Entity, coll2Entity, math::eigen3::convert(contact.point), + force, normal, depth, _numContactsOnCollision, _params); + } + ); + + this->worldContactCallbackIDs[_world] = callbackID; + + ignmsg << "Enabled contact surface customization for world entity [" << _world + << "]" << std::endl; +} + + +////////////////////////////////////////////////// +void PhysicsPrivate::DisableContactSurfaceCustomization(const Entity &_world) +{ + if (this->worldContactCallbackIDs.find(_world) == + this->worldContactCallbackIDs.end()) + { + return; + } + + auto setContactPropertiesCallbackFeature = + this->entityWorldMap.EntityCast< + SetContactPropertiesCallbackFeatureList>(_world); + if (!setContactPropertiesCallbackFeature) + return; + + setContactPropertiesCallbackFeature-> + RemoveContactPropertiesCallback(this->worldContactCallbackIDs[_world]); + + ignmsg << "Disabled contact surface customization for world entity [" + << _world << "]" << std::endl; +} + IGNITION_ADD_PLUGIN(Physics, ignition::gazebo::System, Physics::ISystemConfigure, diff --git a/src/systems/track_controller/CMakeLists.txt b/src/systems/track_controller/CMakeLists.txt new file mode 100644 index 0000000000..a1f7ac1df7 --- /dev/null +++ b/src/systems/track_controller/CMakeLists.txt @@ -0,0 +1,11 @@ +gz_add_system(track-controller + SOURCES + TrackController.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-physics${IGN_PHYSICS_VER}::ignition-physics${IGN_PHYSICS_VER} + ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER} + ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/track_controller/TrackController.cc b/src/systems/track_controller/TrackController.cc new file mode 100644 index 0000000000..116d3cb787 --- /dev/null +++ b/src/systems/track_controller/TrackController.cc @@ -0,0 +1,622 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "TrackController.hh" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::TrackControllerPrivate +{ + public : ~TrackControllerPrivate() {} + /// \brief Register a collision entity to work with this system (e.g. enable + /// custom collision processing). + /// \param[in] _ecm Entity Component Manager + /// \param[in] _entity The collision to register + /// \param[in] _link The link to which the collision belongs + public: void RegisterCollision(EntityComponentManager& _ecm, + const Entity& _entity, const Entity& _link); + + /// \brief Set velocity command to the track. + /// \param[in] _msg The command. + public: void OnCmdVel(const msgs::Double& _msg); + + /// \brief Set center of rotation command to the track. + /// \param[in] _msg The command. + public: void OnCenterOfRotation(const msgs::Vector3d& _msg); + + public: using P = physics::FeaturePolicy3d; + public: using F = physics::SetContactPropertiesCallbackFeature; + + /// \brief The callback for CollectContactSurfaceProperties - all the magic + /// happens here. + /// \param[in] _collision1 The first colliding body. + /// \param[in] _collision2 The second colliding body. + /// \param[in] _point The contact point (in world coords). + /// \param[in] _force Force in the contact point (may be omitted). + /// \param[in] _normal Unit normal of the collision. + /// \param[in] _depth Depth of penetration. + /// \param[in] _numContactsOnCollision Number of contacts that share the same + /// collision body. + /// \param[inout] _params The contact surface parameters to be set by this + /// system. + public: void ComputeSurfaceProperties( + const Entity& _collision1, + const Entity& _collision2, + const math::Vector3d& _point, + const std::optional& _normal, + F::ContactSurfaceParams

& _params); + + /// \brief Compute speed and direction of motion of the contact surface. + /// \param[in] _beltSpeed Speed of the belt. + /// \param[in] _beltDirection Direction of the belt (in world coords). + /// \param[in] _frictionDirection First friction direction (in world coords). + /// \return The computed contact surface speed. + public: double ComputeSurfaceMotion( + double _beltSpeed, const ignition::math::Vector3d &_beltDirection, + const ignition::math::Vector3d &_frictionDirection); + + /// \brief Compute the first friction direction of the contact surface. + /// \param[in] _centerOfRotation The point around which the track circles ( + /// +Inf vector in case of straight motion). + /// \param[in] _contactWorldPosition Position of the contact point. + /// \param[in] _contactNormal Normal of the contact surface (in world coords). + /// \param[in] _beltDirection Direction of the belt (in world coords). + public: ignition::math::Vector3d ComputeFrictionDirection( + const ignition::math::Vector3d &_centerOfRotation, + const ignition::math::Vector3d &_contactWorldPosition, + const ignition::math::Vector3d &_contactNormal, + const ignition::math::Vector3d &_beltDirection); + + /// \brief Name of the link to which the track is attached. + public: std::string linkName; + + /// \brief Orientation of the track relative to the link. It is assumed that + /// the track moves along the +x direction of the transformed coordinate + /// system. + public: math::Quaterniond trackOrientation; + + /// \brief Enables debugging prints and visualizations. + public: bool debug {false}; + /// \brief Cached marker message for debugging purposes. + public: msgs::Marker debugMarker; + /// \brief ID of the debug marker. Should reset to 0 at each iteration start. + public: uint64_t markerId; + + /// \brief Event manager. + public: EventManager* eventManager; + /// \brief Connection to CollectContactSurfaceProperties event. + public: common::ConnectionPtr eventConnection; + /// \brief Ignition transport node. + public: transport::Node node; + + /// \brief The model this plugin is attached to. + public: Model model; + /// \brief Entity of the link this track is attached to. + public: Entity linkEntity {kNullEntity}; + /// \brief Entities of all collision elements of the track's link. + public: std::unordered_set trackCollisions; + + /// \brief World pose of the track's link. + public: math::Pose3d linkWorldPose; + /// \brief World poses of all collision elements of the track's link. + public: std::unordered_map collisionsWorldPose; + + /// \brief The last commanded velocity. + public: double velocity {0}; + /// \brief Commanded velocity clipped to allowable range. + public: double limitedVelocity {0}; + /// \brief Previous clipped commanded velocity. + public: double prevVelocity {0}; + /// \brief Second previous clipped commanded velocity. + public: double prevPrevVelocity {0}; + /// \brief The point around which the track circles (in world coords). Should + /// be set to +Inf if the track is going straight. + public: math::Vector3d centerOfRotation {math::Vector3d::Zero * math::INF_D}; + /// \brief protects velocity and centerOfRotation + public: std::mutex cmdMutex; + + /// \brief Maximum age of a command in seconds. If a command is older, the + /// track automatically sets a zero velocity. Set this to max() to denote + /// commands do not time out. + public: std::chrono::steady_clock::duration maxCommandAge + {std::chrono::steady_clock::duration::max()}; + + /// \brief This variable is set to true each time a new command arrives. + /// It is intended to be set to false after the command is processed. + public: bool hasNewCommand{false}; + + /// \brief The time at which the last command has been received. + public: std::chrono::steady_clock::duration lastCommandTime; + + /// \brief Limiter of the commanded velocity. + public: math::SpeedLimiter limiter; +}; + +////////////////////////////////////////////////// +TrackController::TrackController() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +TrackController::~TrackController() +{ +} + +////////////////////////////////////////////////// +void TrackController::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + this->dataPtr->eventManager = &_eventMgr; + + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "TrackController should be attached to a model " + << "entity. Failed to initialize." << std::endl; + return; + } + + if (!_sdf->HasElement("link")) + { + ignerr << "TrackController plugin is missing element." << std::endl; + return; + } + this->dataPtr->linkName = _sdf->Get("link"); + + using P = physics::FeaturePolicy3d; + using F = physics::SetContactPropertiesCallbackFeature; + + this->dataPtr->eventConnection = this->dataPtr->eventManager-> + Connect( + [this]( + const Entity& _collision1, + const Entity& _collision2, + const math::Vector3d& _point, + const std::optional /* _force */, + const std::optional _normal, + const std::optional /* _depth */, + const size_t /*_numContactsOnCollision*/, + F::ContactSurfaceParams

& _params) + { + this->dataPtr->ComputeSurfaceProperties(_collision1, _collision2, + _point, _normal, _params); + } + ); + + _ecm.Each( + [&](const Entity & _collisionEntity, + const components::Collision */*_collision*/, + const components::Name */*_name*/, + const components::ParentEntity *_parent) + { + this->dataPtr->RegisterCollision(_ecm, _collisionEntity, _parent->Data()); + return true; + } + ); + + const auto topicPrefix = "/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + this->dataPtr->linkName; + + const auto kDefaultVelTopic = topicPrefix + "/track_cmd_vel"; + const auto velTopic = validTopic({_sdf->Get( + "velocity_topic", kDefaultVelTopic).first, kDefaultVelTopic}); + if (!this->dataPtr->node.Subscribe( + velTopic, &TrackControllerPrivate::OnCmdVel, this->dataPtr.get())) + { + ignerr << "Error subscribing to topic [" << velTopic << "]. " + << "Track will not receive commands." << std::endl; + return; + } + igndbg << "Subscribed to " << velTopic << " for receiving track velocity " + << "commands." << std::endl; + + const auto kDefaultCorTopic = topicPrefix + "/track_cmd_center_of_rotation"; + const auto corTopic = validTopic({_sdf->Get( + "center_of_rotation_topic", kDefaultCorTopic).first, kDefaultCorTopic}); + if (!this->dataPtr->node.Subscribe( + corTopic, &TrackControllerPrivate::OnCenterOfRotation, + this->dataPtr.get())) + { + ignerr << "Error subscribing to topic [" << corTopic << "]. " + << "Track will not receive center of rotation commands." + << std::endl; + return; + } + igndbg << "Subscribed to " << corTopic << " for receiving track center " + << "of rotation commands." << std::endl; + + this->dataPtr->trackOrientation = _sdf->Get( + "track_orientation", math::Quaterniond::Identity).first; + + if (_sdf->HasElement("max_command_age")) + { + const auto seconds = _sdf->Get("max_command_age"); + this->dataPtr->maxCommandAge = + std::chrono::duration_cast( + std::chrono::duration(seconds)); + igndbg << "Track commands will time out after " << seconds << " seconds" + << std::endl; + } + + auto hasVelocityLimits = false; + auto hasAccelerationLimits = false; + auto hasJerkLimits = false; + auto minVel = std::numeric_limits::lowest(); + auto maxVel = std::numeric_limits::max(); + auto minAccel = std::numeric_limits::lowest(); + auto maxAccel = std::numeric_limits::max(); + auto minJerk = std::numeric_limits::lowest(); + auto maxJerk = std::numeric_limits::max(); + + if (_sdf->HasElement("min_velocity")) + { + minVel = _sdf->Get("min_velocity"); + hasVelocityLimits = true; + } + if (_sdf->HasElement("max_velocity")) + { + maxVel = _sdf->Get("max_velocity"); + hasVelocityLimits = true; + } + if (_sdf->HasElement("min_acceleration")) + { + minAccel = _sdf->Get("min_acceleration"); + hasAccelerationLimits = true; + } + if (_sdf->HasElement("max_acceleration")) + { + maxAccel = _sdf->Get("max_acceleration"); + hasAccelerationLimits = true; + } + if (_sdf->HasElement("min_jerk")) + { + minJerk = _sdf->Get("min_jerk"); + hasJerkLimits = true; + } + if (_sdf->HasElement("max_jerk")) + { + maxJerk = _sdf->Get("max_jerk"); + hasJerkLimits = true; + } + + if (hasVelocityLimits) + { + this->dataPtr->limiter.SetMinVelocity(minVel); + this->dataPtr->limiter.SetMaxVelocity(maxVel); + } + if (hasAccelerationLimits) + { + this->dataPtr->limiter.SetMinAcceleration(minAccel); + this->dataPtr->limiter.SetMaxAcceleration(maxAccel); + } + if (hasJerkLimits) + { + this->dataPtr->limiter.SetMinJerk(minJerk); + this->dataPtr->limiter.SetMaxJerk(maxJerk); + } + + this->dataPtr->debug = _sdf->Get("debug", false).first; + if (this->dataPtr->debug) + { + this->dataPtr->debugMarker.set_ns(this->dataPtr->linkName + "/friction"); + this->dataPtr->debugMarker.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->debugMarker.set_type(ignition::msgs::Marker::BOX); + this->dataPtr->debugMarker.set_visibility(ignition::msgs::Marker::GUI); + this->dataPtr->debugMarker.mutable_lifetime()->set_sec(0); + this->dataPtr->debugMarker.mutable_lifetime()->set_nsec(4000000); + + // Set material properties + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_material()->mutable_ambient(), + ignition::math::Color(0, 0, 1, 1)); + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_material()->mutable_diffuse(), + ignition::math::Color(0, 0, 1, 1)); + + // Set marker scale + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_scale(), + ignition::math::Vector3d(0.3, 0.03, 0.03)); + } +} + +////////////////////////////////////////////////// +void TrackController::PreUpdate( + const UpdateInfo& _info, EntityComponentManager& _ecm) +{ + _ecm.EachNew( + [&](const Entity & _entity, + const components::Collision */*_collision*/, + const components::Name */*_name*/, + const components::ParentEntity *_parent) + { + this->dataPtr->RegisterCollision(_ecm, _entity, _parent->Data()); + return true; + } + ); + + // Find link entity + if (this->dataPtr->linkEntity == kNullEntity) + { + this->dataPtr->linkEntity = this->dataPtr->model.LinkByName(_ecm, + this->dataPtr->linkName); + } + if (this->dataPtr->linkEntity == kNullEntity) + { + ignwarn << "Could not find track link [" << this->dataPtr->linkName << "]" + << std::endl; + return; + } + + // Cache poses + this->dataPtr->linkWorldPose = worldPose(this->dataPtr->linkEntity, _ecm); + for (auto& collisionEntity : this->dataPtr->trackCollisions) + this->dataPtr->collisionsWorldPose[collisionEntity] = + worldPose(collisionEntity, _ecm); + + std::chrono::steady_clock::duration lastCommandTimeCopy; + { + std::lock_guard lock(this->dataPtr->cmdMutex); + if (this->dataPtr->hasNewCommand) + { + this->dataPtr->lastCommandTime = _info.simTime; + this->dataPtr->hasNewCommand = false; + } + lastCommandTimeCopy = this->dataPtr->lastCommandTime; + + // Compute limited velocity command + this->dataPtr->limitedVelocity = this->dataPtr->velocity; + } + + if (this->dataPtr->maxCommandAge != std::chrono::steady_clock::duration::max() + && (_info.simTime - lastCommandTimeCopy) > this->dataPtr->maxCommandAge) + { + this->dataPtr->limitedVelocity = 0; + } + + this->dataPtr->limiter.Limit( + this->dataPtr->limitedVelocity, // in-out parameter + this->dataPtr->prevVelocity, + this->dataPtr->prevPrevVelocity, _info.dt); + + this->dataPtr->prevPrevVelocity = this->dataPtr->prevVelocity; + this->dataPtr->prevVelocity = this->dataPtr->limitedVelocity; + + if (this->dataPtr->debug) + { + // Reset debug marker ID + this->dataPtr->markerId = 1; + } +} + +////////////////////////////////////////////////// +void TrackControllerPrivate::ComputeSurfaceProperties( + const Entity& _collision1, + const Entity& _collision2, + const math::Vector3d& _point, + const std::optional& _normal, + F::ContactSurfaceParams

& _params + ) +{ + using math::eigen3::convert; + + if (!_normal) + { + static bool informed = false; + if (!informed) + { + ignerr << "TrackController requires a physics engine that computes " + << "contact normals!" << std::endl; + informed = true; + } + return; + } + + const auto isCollision1Track = this->trackCollisions.find(_collision1) != + this->trackCollisions.end(); + const auto isCollision2Track = this->trackCollisions.find(_collision2) != + this->trackCollisions.end(); + if (!isCollision1Track && !isCollision2Track) + return; + + const auto trackCollision = isCollision1Track ? _collision1 : _collision2; + + auto contactNormal = _normal.value(); + + // In case we have not yet cached the collision pose, skip this iteration + if (this->collisionsWorldPose.find(trackCollision) == + this->collisionsWorldPose.end()) + return; + const auto& collisionPose = this->collisionsWorldPose[trackCollision]; + + // Flip the contact normal if it points outside the track collision + if (contactNormal.Dot(collisionPose.Pos() - _point) < 0) + contactNormal = -contactNormal; + + const auto trackWorldRot = this->linkWorldPose.Rot() * this->trackOrientation; + const auto trackYAxisGlobal = + trackWorldRot.RotateVector(math::Vector3d::UnitY); + + // Vector tangent to the belt pointing in the belt's movement direction + // The belt's bottom moves backwards when the robot should move forward! + auto beltDirection = contactNormal.Cross(trackYAxisGlobal); + + if (this->limitedVelocity < 0) + beltDirection = -beltDirection; + + math::Vector3d cor; + { + std::lock_guard lock(this->cmdMutex); + cor = this->centerOfRotation; + } + + const auto frictionDirection = this->ComputeFrictionDirection( + cor, _point, contactNormal, beltDirection); + + _params.firstFrictionalDirection = + convert(isCollision1Track ? frictionDirection : -frictionDirection); + + const auto surfaceMotion = this->ComputeSurfaceMotion( + this->limitedVelocity, beltDirection, frictionDirection); + + if (!_params.contactSurfaceMotionVelocity) + _params.contactSurfaceMotionVelocity.emplace(Eigen::Vector3d::Zero()); + _params.contactSurfaceMotionVelocity->y() = surfaceMotion; + + if (this->debug) + { + igndbg << "Link: " << linkName << std::endl; + igndbg << "- is collision 1 track " << (isCollision1Track ? "1" : "0") + << std::endl; + igndbg << "- velocity cmd " << this->velocity << std::endl; + igndbg << "- limited velocity cmd " << this->limitedVelocity << std::endl; + igndbg << "- friction direction " << frictionDirection << std::endl; + igndbg << "- surface motion " << surfaceMotion << std::endl; + igndbg << "- contact point " << convert(_point) << std::endl; + igndbg << "- contact normal " << contactNormal << std::endl; + igndbg << "- track rot " << trackWorldRot << std::endl; + igndbg << "- track Y " << trackYAxisGlobal << std::endl; + igndbg << "- belt direction " << beltDirection << std::endl; + + this->debugMarker.set_id(++this->markerId); + + math::Quaterniond rot; + rot.From2Axes(math::Vector3d::UnitX, frictionDirection); + math::Vector3d p = _point; + p += rot.RotateVector( + math::Vector3d::UnitX * this->debugMarker.scale().x() / 2); + + ignition::msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( + p.X(), p.Y(), p.Z(), rot.Roll(), rot.Pitch(), rot.Yaw())); + this->debugMarker.mutable_material()->mutable_diffuse()->set_r( + surfaceMotion >= 0 ? 0 : 1); + + this->node.Request("/marker", this->debugMarker); + } +} + +////////////////////////////////////////////////// +double TrackControllerPrivate::ComputeSurfaceMotion( + const double _beltSpeed, const ignition::math::Vector3d &_beltDirection, + const ignition::math::Vector3d &_frictionDirection) +{ + // the dot product is the cosine of the angle they + // form (because both are unit vectors) + // the belt should actually move in the opposite direction than is the desired + // motion of the whole track - that's why the value is negated + return -math::signum(_beltDirection.Dot(_frictionDirection)) * + fabs(_beltSpeed); +} + +////////////////////////////////////////////////// +ignition::math::Vector3d TrackControllerPrivate::ComputeFrictionDirection( + const ignition::math::Vector3d &_centerOfRotation, + const ignition::math::Vector3d &_contactWorldPosition, + const ignition::math::Vector3d &_contactNormal, + const ignition::math::Vector3d &_beltDirection) +{ + if (_centerOfRotation.IsFinite()) + { + // non-straight drive + + // vector pointing from the center of rotation to the contact point + const auto corToContact = + (_contactWorldPosition - _centerOfRotation).Normalize(); + + // the friction force should be perpendicular to corToContact + auto frictionDirection = _contactNormal.Cross(corToContact); + if (this->limitedVelocity < 0) + frictionDirection = - frictionDirection; + + return frictionDirection; + } + else + { + // straight drive + return _beltDirection; + } +} + +////////////////////////////////////////////////// +void TrackControllerPrivate::RegisterCollision(EntityComponentManager& _ecm, + const Entity& _entity, const Entity& _link) +{ + if (this->linkEntity == kNullEntity) + this->linkEntity = this->model.LinkByName(_ecm, this->linkName); + + if (_link != this->linkEntity) + return; + + this->trackCollisions.insert(_entity); + + _ecm.SetComponentData( + _entity, true); +} + +////////////////////////////////////////////////// +void TrackControllerPrivate::OnCmdVel(const msgs::Double& _msg) +{ + std::lock_guard lock(this->cmdMutex); + this->velocity = _msg.data(); + this->hasNewCommand = true; +} + +///////////////////////////////////////////////// +void TrackControllerPrivate::OnCenterOfRotation(const msgs::Vector3d& _msg) +{ + std::lock_guard lock(this->cmdMutex); + this->centerOfRotation = msgs::Convert(_msg); + this->hasNewCommand = true; +} + +IGNITION_ADD_PLUGIN(TrackController, + ignition::gazebo::System, + TrackController::ISystemConfigure, + TrackController::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(TrackController, + "ignition::gazebo::systems::TrackController") diff --git a/src/systems/track_controller/TrackController.hh b/src/systems/track_controller/TrackController.hh new file mode 100644 index 0000000000..f9d54a3204 --- /dev/null +++ b/src/systems/track_controller/TrackController.hh @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_TRACKCONTROLLER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_TRACKCONTROLLER_HH_ + +#include +#include +#include "ignition/gazebo/physics/Events.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class TrackControllerPrivate; + + /// \brief Controller of a track on either a conveyor belt or a tracked + /// vehicle. The system should be attached to a model. If implementing a + /// tracked vehicle, use also TrackedVehicle system. + /// + /// The system is implemented along the lines of M. Pecka, K. Zimmermann and + /// T. Svoboda, "Fast simulation of vehicles with non-deformable tracks," + /// 2017 IEEE/RSJ International Conference on Intelligent Robots and + /// Systems (IROS), 2017, pp. 6414-6419, doi: 10.1109/IROS.2017.8206546. It + /// does not provide 100% plausible track drivetrain simulation, but provides + /// one that is provably better than a set of wheels instead of the track. + /// Only velocity control is supported - no effort controller is available. + /// The basic idea of the implementation is utilizing the so called "contact + /// surface motion" parameter of each contact point between the track and the + /// environment. Instead of telling the physics engine to push velocity + /// towards zero in contact points (up to friction), it tells it to maintain + /// the desired track velocity in the contact point (up to friction). For + /// better behavior when turning with tracked vehicles, it also accepts the + /// position of the center of rotation of the whole vehicle so that it can + /// adjust the direction of friction along the desired circle. This system + /// does not simulate the effect of grousers. The best way to achieve a + /// similar effect is to set a very high `` for the track links. + /// + /// # Examples + /// + /// See example usage in worlds example/conveyor.sdf and + /// example/tracked_vehicle_simple.sdf . + /// + /// # System Parameters + /// + /// `` Name of the link the controller controls. Required parameter. + /// + /// `` If 1, the system will output debugging info and visualizations. + /// The default value is 0. + /// + /// `` Orientation of the track relative to the link. + /// It is assumed that the track moves along the +x direction of the + /// transformed coordinate system. Defaults to no rotation (`0 0 0`). + /// + /// `` Name of the topic on which the system accepts velocity + /// commands. + /// Defaults to `/model/${model_name}/link/${link_name}/track_cmd_vel`. + /// + /// `` The topic on which the track accepts center + /// of rotation commands. Defaults to + /// `/model/${model_name}/link/${link_name}/track_cmd_center_of_rotation`. + /// + /// `` If this parameter is set, each velocity or center of + /// rotation command will only act for the given number of seconds and the + /// track will be stopped if no command arrives before this timeout. + /// + /// ``/`` Min/max velocity of the track (m/s). + /// If not specified, the velocity is not limited (however the physics will, + /// in the end, have some implicit limit). + /// + /// ``/`` Min/max acceleration of the + /// track (m/s^2). If not specified, the acceleration is not limited + /// (however the physics will, in the end, have some implicit limit). + /// + /// ``/`` Min/max jerk of the track (m/s^3). If not + /// specified, the acceleration is not limited (however the physics will, + /// in the end, have some implicit limit). + class TrackController + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: TrackController(); + + /// \brief Destructor + public: ~TrackController() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/tracked_vehicle/CMakeLists.txt b/src/systems/tracked_vehicle/CMakeLists.txt new file mode 100644 index 0000000000..8ee3b43f1a --- /dev/null +++ b/src/systems/tracked_vehicle/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(tracked-vehicle + SOURCES + TrackedVehicle.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/tracked_vehicle/TrackedVehicle.cc b/src/systems/tracked_vehicle/TrackedVehicle.cc new file mode 100644 index 0000000000..e175d44329 --- /dev/null +++ b/src/systems/tracked_vehicle/TrackedVehicle.cc @@ -0,0 +1,778 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "TrackedVehicle.hh" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Velocity command. +struct Commands +{ + /// \brief Linear velocity. + double lin {0.0}; + + /// \brief Angular velocity. + double ang {0.0}; + + Commands() {} +}; + +class ignition::gazebo::systems::TrackedVehiclePrivate +{ + /// \brief Callback for velocity subscription + /// \param[in] _msg Velocity message + public: void OnCmdVel(const ignition::msgs::Twist &_msg); + + /// \brief Callback for steering efficiency subscription + /// \param[in] _msg Steering efficiency message + public: void OnSteeringEfficiency(const ignition::msgs::Double &_msg); + + /// \brief Update odometry and publish an odometry message. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Update the linear and angular velocities. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief The link of the vehicle body (should be between left and right + /// tracks, center of this link will be the center of rotation). + public: Entity bodyLink {kNullEntity}; + + /// \brief Entities of the left tracks + public: std::vector leftTracks; + + /// \brief Entities of the right tracks + public: std::vector rightTracks; + + /// \brief Name of the body link + public: std::string bodyLinkName; + + /// \brief Names of left tracks + public: std::vector leftTrackNames; + + /// \brief Names of right tracks + public: std::vector rightTrackNames; + + /// \brief Velocity publishers of tracks. + public: std::unordered_map + velPublishers; + + /// \brief Center of rotation publishers of tracks. + public: std::unordered_map + corPublishers; + + /// \brief Calculated speed of left tracks + public: double leftSpeed{0}; + + /// \brief Calculated speed of right tracks + public: double rightSpeed{0}; + + /// \brief Radius of the desired rotation (rad). + public: double desiredRotationRadiusSigned {0}; + + /// \brief Fake position encoder of left track (for computing odometry). + public: math::Angle odomLeftWheelPos {0}; + + /// \brief Fake position encoder of left track (for computing odometry). + public: math::Angle odomRightWheelPos {0}; + + /// \brief The point around which the vehicle should circle (in world coords). + public: math::Vector3d centerOfRotation {0, 0, 0}; + + /// \brief Distance between tracks. + public: double tracksSeparation{1.0}; + + /// \brief Height of the tracks. + public: double trackHeight{0.2}; + + /// \brief Steering efficiency. + public: double steeringEfficiency{0.5}; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief The model's canonical link. + public: Link canonicalLink{kNullEntity}; + + /// \brief Update period calculated from . + public: std::chrono::steady_clock::duration odomPubPeriod{0}; + + /// \brief Last sim time odom was published. + public: std::chrono::steady_clock::duration lastOdomPubTime{0}; + + /// \brief Diff drive odometry. + public: math::DiffDriveOdometry odom; + + /// \brief Diff drive odometry message publisher. + public: transport::Node::Publisher odomPub; + + /// \brief Diff drive tf message publisher. + public: transport::Node::Publisher tfPub; + + /// \brief Linear velocity limiter. + public: std::unique_ptr limiterLin; + + /// \brief Angular velocity limiter. + public: std::unique_ptr limiterAng; + + /// \brief Previous control command. + public: Commands last0Cmd; + + /// \brief Previous control command to last0Cmd. + public: Commands last1Cmd; + + /// \brief Last target velocity requested. + public: msgs::Twist targetVel; + + /// \brief This variable is set to true each time a new command arrives. + /// It is intended to be set to false after the command is processed. + public: bool hasNewCommand{false}; + + /// \brief A mutex to protect the target velocity command. + public: std::mutex mutex; + + /// \brief frame_id from sdf. + public: std::string sdfFrameId; + + /// \brief child_frame_id from sdf. + public: std::string sdfChildFrameId; + + /// \brief Enables debugging prints and visualizations. + public: bool debug {false}; + + /// \brief Cached marker message for debugging purposes. + public: msgs::Marker debugMarker; +}; + +////////////////////////////////////////////////// +TrackedVehicle::TrackedVehicle() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +TrackedVehicle::~TrackedVehicle() +{ +} + +////////////////////////////////////////////////// +void TrackedVehicle::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "TrackedVehicle plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + const auto& modelName = this->dataPtr->model.Name(_ecm); + + // Get the canonical link + std::vector links = _ecm.ChildrenByComponents( + _entity, components::CanonicalLink()); + if (!links.empty()) + this->dataPtr->canonicalLink = Link(links[0]); + + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + std::unordered_map tracks; + + if (_sdf->HasElement("body_link")) + this->dataPtr->bodyLinkName = _sdf->Get("body_link"); + + // Get params from SDF + sdf::ElementPtr sdfElem = ptr->GetElement("left_track"); + while (sdfElem) + { + const auto& linkName = sdfElem->Get("link"); + this->dataPtr->leftTrackNames.push_back(linkName); + tracks[linkName] = sdfElem; + sdfElem = sdfElem->GetNextElement("left_track"); + } + sdfElem = ptr->GetElement("right_track"); + while (sdfElem) + { + const auto& linkName = sdfElem->Get("link"); + this->dataPtr->rightTrackNames.push_back(linkName); + tracks[linkName] = sdfElem; + sdfElem = sdfElem->GetNextElement("right_track"); + } + + for (const auto &[linkName, elem] : tracks) + { + const auto prefix = "/model/" + modelName + "/link/" + linkName; + + auto topic = validTopic({elem->Get( + "velocity_topic", prefix + "/track_cmd_vel").first}); + this->dataPtr->velPublishers[linkName] = + this->dataPtr->node.Advertise(topic); + + topic = validTopic({elem->Get("center_of_rotation_topic", + prefix + "/track_cmd_center_of_rotation").first}); + this->dataPtr->corPublishers[linkName] = + this->dataPtr->node.Advertise(topic); + } + + this->dataPtr->tracksSeparation = _sdf->Get("tracks_separation", + this->dataPtr->tracksSeparation).first; + this->dataPtr->steeringEfficiency = _sdf->Get("steering_efficiency", + this->dataPtr->steeringEfficiency).first; + + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); + + std::map limits = { + {"linear_velocity", this->dataPtr->limiterLin.get()}, + {"angular_velocity", this->dataPtr->limiterAng.get()}, + }; + + for (auto& [tag, limiter] : limits) + { + if (!_sdf->HasElement(tag)) + continue; + + auto sdf = ptr->GetElement(tag); + + // Parse speed limiter parameters. + bool hasVelocityLimits = false; + bool hasAccelerationLimits = false; + bool hasJerkLimits = false; + double minVel = std::numeric_limits::lowest(); + double maxVel = std::numeric_limits::max(); + double minAccel = std::numeric_limits::lowest(); + double maxAccel = std::numeric_limits::max(); + double minJerk = std::numeric_limits::lowest(); + double maxJerk = std::numeric_limits::max(); + + if (sdf->HasElement("min_velocity")) + { + minVel = sdf->Get("min_velocity"); + hasVelocityLimits = true; + } + if (sdf->HasElement("max_velocity")) + { + maxVel = sdf->Get("max_velocity"); + hasVelocityLimits = true; + } + if (sdf->HasElement("min_acceleration")) + { + minAccel = sdf->Get("min_acceleration"); + hasAccelerationLimits = true; + } + if (sdf->HasElement("max_acceleration")) + { + maxAccel = sdf->Get("max_acceleration"); + hasAccelerationLimits = true; + } + if (sdf->HasElement("min_jerk")) + { + minJerk = sdf->Get("min_jerk"); + hasJerkLimits = true; + } + if (sdf->HasElement("max_jerk")) + { + maxJerk = sdf->Get("max_jerk"); + hasJerkLimits = true; + } + + if (hasVelocityLimits) + { + limiter->SetMinVelocity(minVel); + limiter->SetMaxVelocity(maxVel); + } + + if (hasAccelerationLimits) + { + limiter->SetMinAcceleration(minAccel); + limiter->SetMaxAcceleration(maxAccel); + } + + if (hasJerkLimits) + { + limiter->SetMinJerk(minJerk); + limiter->SetMaxJerk(maxJerk); + } + } + + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; + if (odomFreq > 0) + { + std::chrono::duration odomPer{1 / odomFreq}; + this->dataPtr->odomPubPeriod = + std::chrono::duration_cast(odomPer); + } + + // Setup odometry. + this->dataPtr->odom.SetWheelParams(this->dataPtr->tracksSeparation, + this->dataPtr->trackHeight/2, this->dataPtr->trackHeight/2); + + // Subscribe to commands + const auto topicPrefix = "/model/" + this->dataPtr->model.Name(_ecm); + + const auto kDefaultCmdVelTopic {topicPrefix + "/cmd_vel"}; + const auto topic = validTopic({ + _sdf->Get("topic", kDefaultCmdVelTopic).first, + kDefaultCmdVelTopic}); + + this->dataPtr->node.Subscribe(topic, &TrackedVehiclePrivate::OnCmdVel, + this->dataPtr.get()); + + const auto kDefaultOdomTopic {topicPrefix + "/odometry"}; + const auto odomTopic = validTopic({ + _sdf->Get("odom_topic", kDefaultOdomTopic).first, + kDefaultOdomTopic}); + + this->dataPtr->odomPub = this->dataPtr->node.Advertise( + odomTopic); + + const auto kDefaultTfTopic {topicPrefix + "/tf"}; + const auto tfTopic = validTopic({ + _sdf->Get("tf_topic", kDefaultTfTopic).first, + kDefaultTfTopic}); + + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopic); + + const auto kDefaultSeTopic {topicPrefix + "/steering_efficiency"}; + const auto seTopic = validTopic({ + _sdf->Get("steering_efficiency_topic", kDefaultSeTopic).first, + kDefaultSeTopic}); + + this->dataPtr->node.Subscribe(seTopic, + &TrackedVehiclePrivate::OnSteeringEfficiency, this->dataPtr.get()); + + if (_sdf->HasElement("frame_id")) + this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); + + if (_sdf->HasElement("child_frame_id")) + this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); + + ignmsg << "TrackedVehicle [" << modelName << "] loaded:" << std::endl; + ignmsg << "- tracks separation: " << this->dataPtr->tracksSeparation + << " m" << std::endl; + ignmsg << "- track height (for odometry): " << this->dataPtr->trackHeight + << " m" << std::endl; + ignmsg << "- initial steering efficiency: " + << this->dataPtr->steeringEfficiency << std::endl; + ignmsg << "- subscribing to twist messages on [" << topic << "]" << std::endl; + ignmsg << "- subscribing to steering efficiency messages on [" + << seTopic << "]" << std::endl; + ignmsg << "- publishing odometry on [" << odomTopic << "]" << std::endl; + ignmsg << "- publishing TF on [" << tfTopic << "]" << std::endl; + + // Initialize debugging helpers if needed + this->dataPtr->debug = _sdf->Get("debug", false).first; + if (this->dataPtr->debug) + { + this->dataPtr->debugMarker.set_ns( + this->dataPtr->model.Name(_ecm) + "/cor"); + this->dataPtr->debugMarker.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->debugMarker.set_type(ignition::msgs::Marker::SPHERE); + this->dataPtr->debugMarker.set_visibility(ignition::msgs::Marker::GUI); + this->dataPtr->debugMarker.mutable_lifetime()->set_sec(0); + this->dataPtr->debugMarker.mutable_lifetime()->set_nsec(4000000); + this->dataPtr->debugMarker.set_id(1); + + // Set material properties + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_material()->mutable_ambient(), + ignition::math::Color(0, 0, 1, 1)); + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_material()->mutable_diffuse(), + ignition::math::Color(0, 0, 1, 1)); + + // Set marker scale + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_scale(), + ignition::math::Vector3d(0.1, 0.1, 0.1)); + } +} + +////////////////////////////////////////////////// +void TrackedVehicle::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrackedVehicle::PreUpdate"); + + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. Resetting odometry." << std::endl; + this->dataPtr->odom.Init( + std::chrono::steady_clock::time_point(_info.simTime)); + } + + // If the links haven't been identified yet, look for them + static std::set warnedModels; + auto modelName = this->dataPtr->model.Name(_ecm); + + if (this->dataPtr->bodyLink == kNullEntity) + { + if (!this->dataPtr->bodyLinkName.empty()) + this->dataPtr->bodyLink = + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->bodyLinkName); + else + this->dataPtr->bodyLink = this->dataPtr->canonicalLink.Entity(); + + if (this->dataPtr->bodyLink == kNullEntity) + { + static bool warned {false}; + if (!warned) + { + ignwarn << "Failed to find body link [" << this->dataPtr->bodyLinkName + << "] for model [" << modelName << "]" << std::endl; + warned = true; + } + return; + } + } + + if (this->dataPtr->leftTracks.empty() || + this->dataPtr->rightTracks.empty()) + { + bool warned{false}; + for (const std::string &name : this->dataPtr->leftTrackNames) + { + Entity track = this->dataPtr->model.LinkByName(_ecm, name); + if (track != kNullEntity) + this->dataPtr->leftTracks.push_back(track); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find left track [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + + for (const std::string &name : this->dataPtr->rightTrackNames) + { + Entity track = this->dataPtr->model.LinkByName(_ecm, name); + if (track != kNullEntity) + this->dataPtr->rightTracks.push_back(track); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find right track [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + if (warned) + { + warnedModels.insert(modelName); + } + } + + if (this->dataPtr->leftTracks.empty() || this->dataPtr->rightTracks.empty()) + return; + + if (warnedModels.find(modelName) != warnedModels.end()) + { + ignmsg << "Found tracks for model [" << modelName + << "], plugin will start working." << std::endl; + warnedModels.erase(modelName); + } +} + +////////////////////////////////////////////////// +void TrackedVehicle::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrackedVehicle::PostUpdate"); + // Nothing left to do if paused. + if (_info.paused) + return; + + this->dataPtr->UpdateVelocity(_info, _ecm); + this->dataPtr->UpdateOdometry(_info, _ecm); +} + +////////////////////////////////////////////////// +void TrackedVehiclePrivate::UpdateOdometry( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrackedVehicle::UpdateOdometry"); + // Initialize, if not already initialized. + if (!this->odom.Initialized()) + { + this->odom.Init(std::chrono::steady_clock::time_point(_info.simTime)); + return; + } + + if (this->leftTracks.empty() || this->rightTracks.empty()) + return; + + this->odom.Update(this->odomLeftWheelPos, this->odomRightWheelPos, + std::chrono::steady_clock::time_point(_info.simTime)); + + // Throttle publishing + auto diff = _info.simTime - this->lastOdomPubTime; + if (diff > std::chrono::steady_clock::duration::zero() && + diff < this->odomPubPeriod) + { + return; + } + this->lastOdomPubTime = _info.simTime; + + // Construct the odometry message and publish it. + msgs::Odometry msg; + msg.mutable_pose()->mutable_position()->set_x(this->odom.X()); + msg.mutable_pose()->mutable_position()->set_y(this->odom.Y()); + + math::Quaterniond orientation(0, 0, *this->odom.Heading()); + msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); + + msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity()); + msg.mutable_twist()->mutable_angular()->set_z(*this->odom.AngularVelocity()); + + // Set the time stamp in the header + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame id. + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + if (this->sdfFrameId.empty()) + { + frame->add_value(this->model.Name(_ecm) + "/odom"); + } + else + { + frame->add_value(this->sdfFrameId); + } + + if (this->sdfChildFrameId.empty()) + { + if (!this->bodyLinkName.empty()) + { + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(this->model.Name(_ecm) + "/" + this->bodyLinkName); + } + } + else + { + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(this->sdfChildFrameId); + } + + // Construct the Pose_V/tf message and publish it. + msgs::Pose_V tfMsg; + auto *tfMsgPose = tfMsg.add_pose(); + tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); + tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); + tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); + + // Publish the messages + this->odomPub.Publish(msg); + this->tfPub.Publish(tfMsg); +} + +////////////////////////////////////////////////// +void TrackedVehiclePrivate::UpdateVelocity( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrackedVehicle::UpdateVelocity"); + + // Read values protected by the mutex + double linVel; + double angVel; + double steeringEfficiencyCopy; + bool hadNewCommand; + { + std::lock_guard lock(this->mutex); + linVel = this->targetVel.linear().x(); + angVel = this->targetVel.angular().z(); + steeringEfficiencyCopy = this->steeringEfficiency; + hadNewCommand = this->hasNewCommand; + this->hasNewCommand = false; + } + + const auto dt = std::chrono::duration(_info.dt).count(); + + // Limit the target velocity if needed. + this->limiterLin->Limit( + linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt); + this->limiterAng->Limit( + angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt); + + // decide whether commands to tracks should be sent + bool sendCommandsToTracks{hadNewCommand}; + if (!hadNewCommand) + { + // if the speed limiter has been limiting the speed (or acceleration), + // we let it saturate first and will stop publishing to tracks after that + if (std::abs(linVel - this->last0Cmd.lin) > 1e-6) + { + sendCommandsToTracks = true; + } + else if (std::abs(angVel - this->last0Cmd.ang) > 1e-6) + { + sendCommandsToTracks = true; + } + } + + // Update history of commands. + this->last1Cmd = last0Cmd; + this->last0Cmd.lin = linVel; + this->last0Cmd.ang = angVel; + + // only update and publish the following values when tracks should be + // commanded with updated commands; none of these values changes when + // linVel and angVel stay the same + if (sendCommandsToTracks) + { + // Convert the target velocities to track velocities. + this->rightSpeed = (linVel + angVel * this->tracksSeparation / + (2.0 * steeringEfficiencyCopy)); + this->leftSpeed = (linVel - angVel * this->tracksSeparation / + (2.0 * steeringEfficiencyCopy)); + + // radius of the turn the robot is doing + this->desiredRotationRadiusSigned = + (fabs(angVel) < 0.1) ? + // is driving straight + math::INF_D : + ( + (fabs(linVel) < 0.1) ? + // is rotating about a single point + 0 : + // general movement + linVel / angVel); + + const auto bodyPose = worldPose(this->bodyLink, _ecm); + const auto bodyYAxisGlobal = + bodyPose.Rot().RotateVector(ignition::math::Vector3d(0, 1, 0)); + // centerOfRotation may be +inf + this->centerOfRotation = + (bodyYAxisGlobal * desiredRotationRadiusSigned) + bodyPose.Pos(); + + for (const auto& track : this->leftTrackNames) + { + msgs::Double vel; + vel.set_data(this->leftSpeed); + this->velPublishers[track].Publish(vel); + + this->corPublishers[track].Publish( + msgs::Convert(this->centerOfRotation)); + } + + for (const auto& track : this->rightTrackNames) + { + msgs::Double vel; + vel.set_data(this->rightSpeed); + this->velPublishers[track].Publish(vel); + + this->corPublishers[track].Publish( + msgs::Convert(this->centerOfRotation)); + } + } + + // Odometry is computed as if the vehicle were a diff-drive vehicle with + // wheels as high as the tracks are. + this->odomLeftWheelPos += this->leftSpeed / (this->trackHeight / 2) * dt; + this->odomRightWheelPos += this->rightSpeed / (this->trackHeight / 2) * dt; + + if (this->debug) + { + igndbg << "Tracked Vehicle " << this->model.Name(_ecm) << ":" << std::endl; + igndbg << "- cmd vel v=" << linVel << ", w=" << angVel + << (hadNewCommand ? " (new command)" : "") << std::endl; + igndbg << "- left v=" << this->leftSpeed + << ", right v=" << this->rightSpeed + << (sendCommandsToTracks ? " (sent to tracks)" : "") << std::endl; + + ignition::msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( + this->centerOfRotation.X(), + this->centerOfRotation.Y(), + this->centerOfRotation.Z(), + 0, 0, 0)); + this->node.Request("/marker", this->debugMarker); + } +} + +////////////////////////////////////////////////// +void TrackedVehiclePrivate::OnCmdVel(const msgs::Twist &_msg) +{ + std::lock_guard lock(this->mutex); + this->targetVel = _msg; + this->hasNewCommand = true; +} + +////////////////////////////////////////////////// +void TrackedVehiclePrivate::OnSteeringEfficiency( + const ignition::msgs::Double& _msg) +{ + std::lock_guard lock(this->mutex); + this->steeringEfficiency = _msg.data(); + this->hasNewCommand = true; +} + +IGNITION_ADD_PLUGIN(TrackedVehicle, + ignition::gazebo::System, + TrackedVehicle::ISystemConfigure, + TrackedVehicle::ISystemPreUpdate, + TrackedVehicle::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(TrackedVehicle, + "ignition::gazebo::systems::TrackedVehicle") diff --git a/src/systems/tracked_vehicle/TrackedVehicle.hh b/src/systems/tracked_vehicle/TrackedVehicle.hh new file mode 100644 index 0000000000..878c5defc9 --- /dev/null +++ b/src/systems/tracked_vehicle/TrackedVehicle.hh @@ -0,0 +1,168 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_TRACKEDVEHICLE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_TRACKEDVEHICLE_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class TrackedVehiclePrivate; + + /// \brief Tracked vehicle controller which can be attached to a model + /// with any number of left and right tracks. The system should be attached + /// to a model. Each track has to have a TrackController system configured and + /// running. + /// + /// So far, this system only supports tracks that are parallel along a common + /// axis (other designs are possible, but not implemented). + /// + /// # Examples + /// + /// See example usage in world example/tracked_vehicle_simple.sdf . + /// + /// # System Parameters + /// + /// ``: Configuration of a left track link. This element can + /// appear multiple times, and must appear at least once. + /// + /// ``: Configuration of a right track link. This element can + /// appear multiple times, and must appear at least once. + /// + /// ``, `` subelements: + /// - ``: The link representing the track. Required parameter. + /// - ``: The topic on which the track accepts velocity + /// commands (defaults to + /// `/model/${model_name}/link/${link_name}/track_cmd_vel`). + /// - ``: The topic on which the track accepts + /// center of rotation commands (defaults to + /// `/model/${model_name}/link/${link_name}/track_cmd_center_of_rotation`) + /// + /// ``: Distance between tracks, in meters. Required + /// parameter. + /// + /// ``: Height of the tracks, in meters (used for computing + /// odometry). Required parameter. + /// + /// ``: Initial steering efficiency. Defaults to 0.5. + /// + /// `` If 1, the system will output debugging info and visualizations. + /// Defaults to 0. + /// + /// ``: Limiter of linear velocity of the vehicle. Please + /// note that the tracks can each have their own speed limitations. If the + /// element is not specified, the velocities etc. have no implicit limits. + /// - ``/`` Min/max velocity of the vehicle (m/s). + /// If not specified, the velocity is not limited (however the physics + /// will, in the end, have some implicit limit). + /// - ``/`` Min/max acceleration of the + /// vehicle (m/s^2). If not specified, the acceleration is not limited + /// (however the physics will, in the end, have some implicit limit). + /// - ``/`` Min/max jerk of the vehicle (m/s^3). If not + /// specified, the acceleration is not limited (however the physics will, + /// in the end, have some implicit limit). + /// + /// ``: Limiter of angular velocity of the vehicle. Please + /// note that the tracks can each have their own speed limitations. If the + /// element is not specified, the velocities etc. have no implicit limits. + /// - ``/`` Min/max velocity of the vehicle + /// (rad/s). If not specified, the velocity is not limited (however the + /// physics will, in the end, have some implicit limit). + /// - ``/`` Min/max acceleration of the + /// vehicle (rad/s^2). If not specified, the velocity is not limited + /// (however the physics will, in the end, have some implicit limit). + /// - ``/`` Min/max jerk of the vehicle (rad/s^3). If not + /// specified, the velocity is not limited (however the physics + /// will, in the end, have some implicit limit). + /// + /// ``: Odometry publication frequency. This + /// element is optional, and the default value is 50Hz. + /// + /// ``: Custom topic that this system will subscribe to in order to + /// receive command velocity messages. This element is optional, and the + /// default value is `/model/{model_name}/cmd_vel`. + /// + /// ``: Custom topic that this system will + /// subscribe to in order to receive steering efficiency messages. + /// This element is optional, and the default value is + /// `/model/{model_name}/steering_efficiency`. + /// + /// ``: Custom topic on which this system will publish odometry + /// messages. This element is optional, and the default value is + /// `/model/{model_name}/odometry`. + /// + /// ``: Custom topic on which this system will publish the + /// transform from `frame_id` to `child_frame_id`. This element is optional, + /// and the default value is `/model/{model_name}/tf`. + /// + /// ``: Custom `frame_id` field that this system will use as the + /// origin of the odometry transform in both the `` + /// `ignition.msgs.Pose_V` message and the `` + /// `ignition.msgs.Odometry` message. This element if optional, and the + /// default value is `{model_name}/odom`. + /// + /// ``: Custom `child_frame_id` that this system will use as + /// the target of the odometry trasnform in both the `` + /// `ignition.msgs.Pose_V` message and the `` + /// `ignition.msgs.Odometry` message. This element if optional, + /// and the default value is `{model_name}/{link_name}`. + class TrackedVehicle + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: TrackedVehicle(); + + /// \brief Destructor + public: ~TrackedVehicle() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 922069f596..5655134556 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -41,6 +41,7 @@ set(tests sdf_include.cc thermal_system.cc touch_plugin.cc + tracked_vehicle_system.cc triggered_publisher.cc user_commands.cc velocity_control_system.cc @@ -92,3 +93,10 @@ if (DART_FOUND) target_include_directories(INTEGRATION_physics_system SYSTEM PRIVATE ${DART_INCLUDE_DIRS}) target_compile_definitions(INTEGRATION_physics_system PRIVATE HAVE_DART) endif() + +target_link_libraries(INTEGRATION_tracked_vehicle_system + ignition-physics${IGN_PHYSICS_VER}::core + ignition-plugin${IGN_PLUGIN_VER}::loader +) +# The default timeout (240s) doesn't seem to be enough for this test. +set_tests_properties(INTEGRATION_tracked_vehicle_system PROPERTIES TIMEOUT 300) diff --git a/test/integration/tracked_vehicle_system.cc b/test/integration/tracked_vehicle_system.cc new file mode 100644 index 0000000000..ea53742d3b --- /dev/null +++ b/test/integration/tracked_vehicle_system.cc @@ -0,0 +1,582 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include +#include +#include +#include +#include "ignition/gazebo/components/PhysicsEnginePlugin.hh" +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +#define tol 10e-4 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +#define EXPECT_ANGLE_NEAR(a1, a2, tol) \ + EXPECT_LT(std::abs(math::Angle((a1) - (a2)).Normalized().Radian()), (tol)) \ + << (a1) << " vs. " << (a2) + +// Verify that a model's world pose is near a specified pose. +void verifyPose(const math::Pose3d& pose1, const math::Pose3d& pose2) +{ + EXPECT_NEAR(pose1.Pos().X(), pose2.Pos().X(), 1e-1); + EXPECT_NEAR(pose1.Pos().Y(), pose2.Pos().Y(), 1e-1); + EXPECT_NEAR(pose1.Pos().Z(), pose2.Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(pose1.Rot().Roll(), pose2.Rot().Roll(), 1e-2); + EXPECT_ANGLE_NEAR(pose1.Rot().Pitch(), pose2.Rot().Pitch(), 1e-2); + EXPECT_ANGLE_NEAR(pose1.Rot().Yaw(), pose2.Rot().Yaw(), 1e-1); +} + +/// \brief Test TrackedVehicle system. This test drives a tracked robot over a +/// course of obstacles and verifies that it is able to climb on/over them. +class TrackedVehicleTest : public InternalFixture<::testing::Test> +{ + public: void SkipTestIfNotSupported(const EntityComponentManager &_ecm, + bool &_shouldSkip) + { +#if __APPLE__ + // until https://github.com/ignitionrobotics/ign-gazebo/issues/806 is fixed + _shouldSkip = true; +#else + _shouldSkip = false; + auto pluginLib = + _ecm.ComponentData(worldEntity(_ecm)); + ASSERT_TRUE(pluginLib.has_value()) + << "PhysicsEnginePlugin component not found"; + + // Find physics plugin (copied from the Physics system with some + // modifications) + common::SystemPaths systemPaths; + systemPaths.SetPluginPathEnv("IGN_GAZEBO_PHYSICS_ENGINE_PATH"); + systemPaths.AddPluginPaths({IGNITION_PHYSICS_ENGINE_INSTALL_DIR}); + + auto pathToLib = systemPaths.FindSharedLibrary(*pluginLib); + ASSERT_FALSE(pathToLib.empty()) + << "Failed to find plugin [" << *pluginLib << "]"; + + // Load engine plugin + ignition::plugin::Loader pluginLoader; + auto plugins = pluginLoader.LoadLib(pathToLib); + ASSERT_FALSE(plugins.empty()) + << "Unable to load the [" << pathToLib << "] library"; + + // Check that we do have a valid physics engine. Otherwise, this should be a + // failure not a skip. + auto classNames = pluginLoader.PluginsImplementing< + physics::ForwardStep::Implementation< + physics::FeaturePolicy3d>>(); + ASSERT_FALSE(classNames.empty()) + << "No physics plugins found in library [" << pathToLib << "]"; + + // Check if there are any plugins implementing + // SetContactPropertiesCallbackFeature. If not, skip the test. + auto contactProperties = pluginLoader.PluginsImplementing< + physics::SetContactPropertiesCallbackFeature::Implementation< + physics::FeaturePolicy3d>>(); + if (contactProperties.empty()) + { + _shouldSkip = true; + } +#endif + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + /// \param[in] _odomTopic Odometry topic. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic, + const std::string &_odomTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay ecmGetterSystem; + EntityComponentManager* ecm {nullptr}; + ecmGetterSystem.OnPreUpdate([&ecm](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (ecm == nullptr) + ecm = &_ecm; + }); + server.AddSystem(ecmGetterSystem.systemPtr); + // Get ECM + server.Run(true, 1, false); + + ASSERT_NE(nullptr, ecm); + bool shouldSkipTest = false; + this->SkipTestIfNotSupported(*ecm, shouldSkipTest); + if (shouldSkipTest) + { + // Skip test if the ContactProperties feature is not available + GTEST_SKIP() << "Skipping test because physics engine does not support " + "SetContactPropertiesCallbackFeature"; + } + + test::Relay testSystem; + Entity modelEntity {kNullEntity}; + std::vector poses; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + modelEntity = _ecm.EntityByComponents( + components::Model(), + components::Name("simple_tracked")); + EXPECT_NE(kNullEntity, modelEntity); + + auto poseComp = _ecm.Component(modelEntity); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (size_t i = 101; i < poses.size(); ++i) + { + SCOPED_TRACE(i); + verifyPose(poses[100], poses[i]); + } + + poses.clear(); + + // Get odometry messages + double period{1.0 / 50.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + node.Subscribe(_odomTopic, odomCb); + + msgs::Twist msg; + msg.mutable_linear()->set_x(1.0); + + pub.Publish(msg); + + server.Run(true, 1000, false); + + // Poses for 1s + ASSERT_EQ(1000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 50 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(100ms); + } + ASSERT_NE(maxSleep, sleep); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(50u, odomPoses.size()); + + EXPECT_LT(poses[0].Pos().X(), poses[999].Pos().X()); + EXPECT_NEAR(poses[0].Pos().Y(), poses[999].Pos().Y(), tol); + EXPECT_NEAR(poses[0].Pos().Z(), poses[999].Pos().Z(), tol); + EXPECT_ANGLE_NEAR(poses[0].Rot().X(), poses[999].Rot().X(), tol); + EXPECT_ANGLE_NEAR(poses[0].Rot().Y(), poses[999].Rot().Y(), tol); + EXPECT_ANGLE_NEAR(poses[0].Rot().Z(), poses[999].Rot().Z(), tol); + + // The robot starts at (3,0,0), so odom will have this shift. + EXPECT_NEAR(poses[0].Pos().X(), odomPoses[0].Pos().X() + 3.0, 3e-2); + EXPECT_NEAR(poses[0].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().X(), odomPoses.back().Pos().X() + 3, 1e-1); + EXPECT_NEAR(poses.back().Pos().Y(), odomPoses.back().Pos().Y(), 1e-2); + + // Max velocities/accelerations expectations. + // Moving time. + double t = 1.0; + double d = poses[999].Pos().Distance(poses[0].Pos()); + double v = d / t; + EXPECT_LT(v, 1); + + poses.clear(); + + gazebo::Model model(modelEntity); + + // Move the robot somewhere to free space without obstacles. + model.SetWorldPoseCmd(*ecm, math::Pose3d(10, 10, 0.1, 0, 0, 0)); + + // Let the models settle down. + server.Run(true, 300, false); + + // Test straight driving - 1 sec driving, should move 1 meter forward. + + const auto startPose = poses.back(); + + const double linearSpeed = 1.0; + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 1000, false); + + EXPECT_NEAR(poses.back().Pos().X(), startPose.Pos().X() + linearSpeed, 0.1); + EXPECT_NEAR(poses.back().Pos().Y(), startPose.Pos().Y(), 1e-1); + EXPECT_NEAR(poses.back().Pos().Z(), startPose.Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), startPose.Rot().Roll(), 1e-2); + EXPECT_ANGLE_NEAR( + poses.back().Rot().Pitch(), startPose.Rot().Pitch(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), startPose.Rot().Yaw(), 1e-1); + + // Test rotation in place - 1 sec rotation, should turn 0.25 rad. + + const auto middlePose = poses.back(); + + // Take care when changing this value - if too high, it could get restricted + // by the max speed of the tracks. + const double rotationSpeed = 0.25; + msgs::Set(msg.mutable_linear(), math::Vector3d(0, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, rotationSpeed)); + pub.Publish(msg); + server.Run(true, 1000, false); + + EXPECT_NEAR(poses.back().Pos().X(), middlePose.Pos().X(), 1e-1); + EXPECT_NEAR(poses.back().Pos().Y(), middlePose.Pos().Y(), 1e-1); + EXPECT_NEAR(poses.back().Pos().Z(), middlePose.Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), middlePose.Rot().Roll(), 1e-2); + EXPECT_ANGLE_NEAR( + poses.back().Rot().Pitch(), middlePose.Rot().Pitch(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), + middlePose.Rot().Yaw() + rotationSpeed, 1e-1); + + // Test following a circular path. + + const auto lastPose = poses.back(); + + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0.2)); + pub.Publish(msg); + server.Run(true, 1000, false); + + EXPECT_NEAR(poses.back().Pos().X(), lastPose.Pos().X() + 0.4, 1e-1); + EXPECT_NEAR(poses.back().Pos().Y(), lastPose.Pos().Y() + 0.15, 1e-1); + EXPECT_NEAR(poses.back().Pos().Z(), lastPose.Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), lastPose.Rot().Roll(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), lastPose.Rot().Pitch(), 1e-2); + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), lastPose.Rot().Yaw() + 0.2, 1e-1); + + // Test driving on staircase - should climb to its middle part. + + const auto beforeStairsPose = math::Pose3d( + 3, 0, 0.1, + 0, 0, 0); + model.SetWorldPoseCmd(*ecm, beforeStairsPose); + + // Let the model settle down. + server.Run(true, 300, false); + + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 3500, false); + + EXPECT_NEAR(poses.back().Pos().X(), beforeStairsPose.X() + 3.4, 0.15); + EXPECT_LE(poses.back().Pos().Y(), 0.7); + EXPECT_GT(poses.back().Pos().Z(), 0.6); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), -0.4, 1e-1); + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), beforeStairsPose.Rot().Yaw(), 1e-1); + + // Test driving over a cylinder + + const auto beforeCylinderPose = math::Pose3d( + 1, 0, 0.1, + 0, 0, -math::Angle::Pi.Radian()); + model.SetWorldPoseCmd(*ecm, beforeCylinderPose); + + // Let the model settle down. + server.Run(true, 300, false); + + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 2000, false); + + // The cylinder is at (0, 0, 0), we start at (0, 1, 0), and want to pass + // at least a bit behind the cylinder (0, -1, 0). The driving is a bit wild, + // so we don't care much about the end Y position and yaw. + EXPECT_LT(poses.back().Pos().X(), -0.99); // The driving is wild + EXPECT_NEAR(poses.back().Pos().Y(), 0, 0.5); + EXPECT_NEAR(poses.back().Pos().Z(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0.0, 1e-1); + // The driving is wild + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), beforeCylinderPose.Rot().Yaw(), 0.5); + + // Test driving over an obstacle that requires flippers. Without them, the + // robot would get stuck in front of the obstacle. + + const auto beforeBoxPose = math::Pose3d( + 1, 2, 0.1, + 0, 0, -math::Angle::Pi.Radian()); + model.SetWorldPoseCmd(*ecm, beforeBoxPose); + + // Let the model settle down. + server.Run(true, 300, false); + + // we go backwards because we have the CoG in the back + msgs::Set(msg.mutable_linear(), math::Vector3d(-linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 4000, false); + + // The box is at (2, 2, 0), we start at (1, 2, 0), and want to pass + // at least a bit behind the box (3.5, 2, 0). The driving is a bit wild. + EXPECT_GT(poses.back().Pos().X(), 3.5); + EXPECT_NEAR(poses.back().Pos().Y(), 2, 0.1); // The driving is wild + EXPECT_NEAR(poses.back().Pos().Z(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0.0, 1e-1); + // The driving is wild + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), beforeBoxPose.Rot().Yaw(), 0.25); + // And we go back, which is a somewhat easier way + + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 4000, false); + + // We start at (3.5, 2, 0), we go back, and it should be a bit faster than + // the previous traversal, so we should end up beyond the starting point. + EXPECT_LT(poses.back().Pos().X(), 1); + EXPECT_NEAR(poses.back().Pos().Y(), 2, 0.1); // The driving is wild + EXPECT_NEAR(poses.back().Pos().Z(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0.0, 1e-1); + // The driving is wild + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), beforeBoxPose.Rot().Yaw(), 0.25); + + // Test that disabling the contact surface customization makes the vehicle + // immobile + + ecm->Each( + [&](const Entity & _entity, + const components::Collision */*_collision*/) -> bool + { + ecm->SetComponentData( + _entity, false); + return true; + }); + + model.SetWorldPoseCmd(*ecm, beforeCylinderPose); + + // Let the model settle down. + server.Run(true, 300, false); + + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 500, false); + + // Verify that the vehicle did not move + poses.back().SetZ(beforeCylinderPose.Pos().Z()); // ignore Z offset + verifyPose(poses.back(), beforeCylinderPose); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestConveyor(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay ecmGetterSystem; + EntityComponentManager* ecm {nullptr}; + ecmGetterSystem.OnPreUpdate([&ecm](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (ecm == nullptr) + ecm = &_ecm; + }); + server.AddSystem(ecmGetterSystem.systemPtr); + // Get ECM + server.Run(true, 1, false); + + ASSERT_NE(nullptr, ecm); + bool shouldSkipTest = false; + this->SkipTestIfNotSupported(*ecm, shouldSkipTest); + if (shouldSkipTest) + { + // Skip test if the ContactProperties feature is not available + GTEST_SKIP() << "Skipping test because physics engine does not support " + "SetContactPropertiesCallbackFeature"; + } + + test::Relay testSystem; + Entity boxEntity {kNullEntity}; + std::vector poses; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + boxEntity = _ecm.EntityByComponents( + components::Model(), + components::Name("box")); + EXPECT_NE(kNullEntity, boxEntity); + + auto poseComp = _ecm.Component(boxEntity); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + server.Run(true, 1000, false); + + // Poses for 1s + ASSERT_EQ(1000u, poses.size()); + + // check that the box has not moved in X and Y directions (it will move in + // Z as it falls down on the conveyor) + EXPECT_NEAR(poses[0].Pos().X(), poses.back().Pos().X(), 1e-6); + EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-6); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + + poses.clear(); + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + + // In this test, there is a long conveyor and a small box at its center. + // The conveyor has max_velocity 0.5, max_acceleration 0.25 and command + // timeout 2 seconds. So we expect the box will move slowly in the first + // second, it will reach 0.5 meters in 2 seconds, and it will slow down + // for the third second (deceleration is limited even for command timeout). + + msgs::Double msg; + msg.set_data(1.0); + + pub.Publish(msg); + + server.Run(true, 1000, false); + + // Poses for 1s + ASSERT_EQ(1000u, poses.size()); + + EXPECT_NEAR(0.125, poses.back().Pos().X(), 1e-1); + EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); + EXPECT_NEAR(poses[0].Pos().Z(), poses.back().Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + + server.Run(true, 1000, false); + + // Poses for 2s + ASSERT_EQ(2000u, poses.size()); + + EXPECT_NEAR(0.5, poses.back().Pos().X(), 1e-1); + EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); + EXPECT_NEAR(poses[0].Pos().Z(), poses.back().Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + + server.Run(true, 1000, false); + + // Poses for 3s + ASSERT_EQ(3000u, poses.size()); + + EXPECT_NEAR(0.875, poses.back().Pos().X(), 1e-1); + EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); + EXPECT_NEAR(poses[0].Pos().Z(), poses.back().Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + + poses.clear(); + } +}; + +///////////////////////////////////////////////// +TEST_F(TrackedVehicleTest, PublishCmd) +{ + this->TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/tracked_vehicle_simple.sdf", + "/model/simple_tracked/cmd_vel", + "/model/simple_tracked/odometry"); +} + +///////////////////////////////////////////////// +TEST_F(TrackedVehicleTest, Conveyor) +{ + this->TestConveyor( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/conveyor.sdf", + "/model/conveyor/link/base_link/track_cmd_vel"); +} diff --git a/test/worlds/conveyor.sdf b/test/worlds/conveyor.sdf new file mode 100644 index 0000000000..f4cd000ad8 --- /dev/null +++ b/test/worlds/conveyor.sdf @@ -0,0 +1,284 @@ + + + + + + 0.001 + 10.0 + + + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 1 + + 0 0 0 0 0 0 + + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0 0 0 0 + + + 5 0.2 0.1 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0 0 0 0 + + + 5 0.2 0.1 + + + + + 2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + -2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + 1 + 0 + + + + base_link + 2.0 + 0.5 + 0.25 + -0.25 + + + + + 0 0 1 0 0 0 + + + 1.06 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + + + 0 0 0 0 0 0 + + + 0.1 0.1 0.1 + + + + 1 1 1 1 + + + + + + 0.1 0.1 0.1 + + + 0 0 0 0 0 0 + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + Transform control + + + + + false + 230 + 50 + floating + false + #666666 + + + + + + + + + + + false + 200 + 50 + floating + false + #666666 + + + + + diff --git a/test/worlds/tracked_vehicle_simple.sdf b/test/worlds/tracked_vehicle_simple.sdf new file mode 100644 index 0000000000..5a54f56958 --- /dev/null +++ b/test/worlds/tracked_vehicle_simple.sdf @@ -0,0 +1,1621 @@ + + + + + 0.001 + 1.0 + 1000 + + + + + + 1 1 1 1 + 0.8 0.8 0.8 1 + 1 + + + 1 + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 3 0 0.1 0 0 0 + + 0 0 0 0 0 0 + + -0.122 0 0.118 1.5708 0 0 + 13.14 + + 0.10019 + 0 + 0 + 0.345043 + 0 + 0.302044 + + + + -0.122 0 0.118 0 0 0 + + + 0.50017 0.24093 0.139 + + + + + -0.122 0 0.118 0 0 0 + + + 0.50017 0.24093 0.139 + + + + 0 + 1 + 0 + + + 0 0.1985 0 0 0 0 + + 0 0 0.0141 0 0 0 + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + 1 + 0 + + + left_track + base_link + + + 0 -0.1985 0 0 0 0 + + 0 0 0.0141 0 0 0 + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + 1 + 0 + + + right_track + base_link + + + 0.25 0.272 0.0195 0 -0.5 0 + + 0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + front_left_flipper + left_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + -0.25 0.272 0.0195 3.14159 -0.5 3.14159 + + 0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + rear_left_flipper + left_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + 0.25 -0.272 0.0195 3.14159 0.5 3.14159 + + -0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + front_right_flipper + right_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + -0.25 -0.272 0.0195 0 0.5 0 + + -0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + rear_right_flipper + right_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + + left_track + front_left_flipper + rear_left_flipper + right_track + front_right_flipper + rear_right_flipper + 0.4 + 0.18094 + 0.5 + + + left_track + -1.0 + 1.0 + + + right_track + -1.0 + 1.0 + + + front_left_flipper + -1.0 + 1.0 + + + rear_left_flipper + -1.0 + 1.0 + + + front_right_flipper + -1.0 + 1.0 + + + rear_right_flipper + -1.0 + 1.0 + + + + 7 0 0 0 0 1.5708 + + + 0 1.6625 0.0375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.6625 0.0375 0 0 0 + + + 0 1.4875 0.1125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.4875 0.1125 0 0 0 + + + 0 1.3125 0.1875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.3125 0.1875 0 0 0 + + + 0 1.1375 0.2625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.1375 0.2625 0 0 0 + + + 0 0.9625 0.3375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.9625 0.3375 0 0 0 + + + 0 0.7875 0.4125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.7875 0.4125 0 0 0 + + + 0 0.6125 0.4875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.6125 0.4875 0 0 0 + + + 0 0.4375 0.5625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.4375 0.5625 0 0 0 + + + 0 0.2625 0.6375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.2625 0.6375 0 0 0 + + + 0 0.0875 0.7125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.0875 0.7125 0 0 0 + + + 0 -0.0875 0.7875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.0875 0.7875 0 0 0 + + + 0 -0.2625 0.8625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.2625 0.8625 0 0 0 + + + 0 -0.4375 0.9375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.4375 0.9375 0 0 0 + + + 0 -0.6125 1.0125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.6125 1.0125 0 0 0 + + + 0 -0.7875 1.0875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.7875 1.0875 0 0 0 + + + 0 -0.9625 1.1625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.9625 1.1625 0 0 0 + + + 0 -1.1375 1.2375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.1375 1.2375 0 0 0 + + + 0 -1.3125 1.3125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.3125 1.3125 0 0 0 + + + 0 -1.4875 1.3875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.4875 1.3875 0 0 0 + + + 0 -1.6625 1.4625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.6625 1.4625 0 0 0 + + + 1 + + + 1 + 0 0 -0.06 0 0 1.5708 + + + 0 0 0 0 1.5708 0 + + + 0.15 + 0.8 + + + + + + 1 + + + + + + 0 0 0 0 1.5708 0 + + + 0.15 + 0.8 + + + + + + + 1 + 2 2 0.028 1.7821 0 1.5708 + + + 0 0 0 0 0 0 + + + 0.85 0.15 0.5 + + + + + + 1 + + + + + + 0 0 0 0 0 0 + + + 0.85 0.15 0.5 + + + + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + +