diff --git a/drone_assets/models/asphalt_plane/model.sdf b/drone_assets/models/asphalt_plane/model.sdf index 5840316..c4ffd75 100644 --- a/drone_assets/models/asphalt_plane/model.sdf +++ b/drone_assets/models/asphalt_plane/model.sdf @@ -6,7 +6,7 @@ - 200 200 .1 + 250 250 .1 @@ -14,7 +14,7 @@ false - 200 200 .1 + 250 250 .1 diff --git a/drone_assets/models/car_color_beacon/meshes/color_beacon.png b/drone_assets/models/car_color_beacon/meshes/color_beacon.png index 2e2b39d..2946b6b 100644 Binary files a/drone_assets/models/car_color_beacon/meshes/color_beacon.png and b/drone_assets/models/car_color_beacon/meshes/color_beacon.png differ diff --git a/drone_assets/models/cylinder1/meshes/pared4.dae b/drone_assets/models/cylinder1/meshes/pared4.dae new file mode 100644 index 0000000..b5a0b99 --- /dev/null +++ b/drone_assets/models/cylinder1/meshes/pared4.dae @@ -0,0 +1,93 @@ + + + + + Blender User + Blender 2.79.0 + + 2018-03-20T11:39:29 + 2018-03-20T11:39:29 + + Z_UP + + + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.003006209 0 1 1 + + + 1 + + + + + + + + + + + + + + + + 1 1 -1 1 -1 -1 -1 -0.9999998 -1 -0.9999997 1 -1 1 0.9999995 1 0.9999994 -1.000001 1 -1 -0.9999997 1 -1 1 1 + + + + + + + + + + 0 0 -1 0 0 1 1 0 -2.38419e-7 0 -1 -4.76837e-7 -1 2.38419e-7 -1.49012e-7 2.68221e-7 1 2.38419e-7 0 0 -1 0 0 1 1 -5.96046e-7 3.27825e-7 -4.76837e-7 -1 0 -1 2.38419e-7 -1.19209e-7 2.08616e-7 1 0 + + + + + + + + + + + + + + +

0 0 2 0 3 0 7 1 5 1 4 1 4 2 1 2 0 2 5 3 2 3 1 3 2 4 7 4 3 4 0 5 7 5 4 5 0 6 1 6 2 6 7 7 6 7 5 7 4 8 5 8 1 8 5 9 6 9 2 9 2 10 6 10 7 10 0 11 3 11 7 11

+ + + + + + + + + 1.027213 0 0 -1.084647 0 0.1027213 0 -6 0 0 1.54082 1.6 0 0 0 1 + + + + + + + + + + + + + + \ No newline at end of file diff --git a/drone_assets/models/cylinder1/model.config b/drone_assets/models/cylinder1/model.config new file mode 100644 index 0000000..08bdc28 --- /dev/null +++ b/drone_assets/models/cylinder1/model.config @@ -0,0 +1,16 @@ + + + + cylinder1 + 1.0 + model.sdf + + + Alvaro Villamil + a.villamil@hotmail.com + + + + The F1 Monaco track without elevations. + + diff --git a/drone_assets/models/cylinder1/model.sdf b/drone_assets/models/cylinder1/model.sdf new file mode 100644 index 0000000..af66cf2 --- /dev/null +++ b/drone_assets/models/cylinder1/model.sdf @@ -0,0 +1,107 @@ + + + + + 0 0 2.133 0 0 0 + + 10000 + + 99999999999 + 0 + 0 + 99999999999 + 0 + 99999999999 + + 0 0 0 0 -0 0 + + 0 + 0 + 1 + + + + 0.5 + 1 + + + + + + 0.9 0.6 0.3 1 + 0.698039 0.698039 0.698039 1 + 0.898039 0.898039 0.898039 1 20 + 0 0 0 1 + + __default__ + + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.5 + 1 + + + + + + 100 + 100 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 1 + + + diff --git a/drone_assets/models/cylinder2/meshes/pared4.dae b/drone_assets/models/cylinder2/meshes/pared4.dae new file mode 100644 index 0000000..b5a0b99 --- /dev/null +++ b/drone_assets/models/cylinder2/meshes/pared4.dae @@ -0,0 +1,93 @@ + + + + + Blender User + Blender 2.79.0 + + 2018-03-20T11:39:29 + 2018-03-20T11:39:29 + + Z_UP + + + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.003006209 0 1 1 + + + 1 + + + + + + + + + + + + + + + + 1 1 -1 1 -1 -1 -1 -0.9999998 -1 -0.9999997 1 -1 1 0.9999995 1 0.9999994 -1.000001 1 -1 -0.9999997 1 -1 1 1 + + + + + + + + + + 0 0 -1 0 0 1 1 0 -2.38419e-7 0 -1 -4.76837e-7 -1 2.38419e-7 -1.49012e-7 2.68221e-7 1 2.38419e-7 0 0 -1 0 0 1 1 -5.96046e-7 3.27825e-7 -4.76837e-7 -1 0 -1 2.38419e-7 -1.19209e-7 2.08616e-7 1 0 + + + + + + + + + + + + + + +

0 0 2 0 3 0 7 1 5 1 4 1 4 2 1 2 0 2 5 3 2 3 1 3 2 4 7 4 3 4 0 5 7 5 4 5 0 6 1 6 2 6 7 7 6 7 5 7 4 8 5 8 1 8 5 9 6 9 2 9 2 10 6 10 7 10 0 11 3 11 7 11

+ + + + + + + + + 1.027213 0 0 -1.084647 0 0.1027213 0 -6 0 0 1.54082 1.6 0 0 0 1 + + + + + + + + + + + + + + \ No newline at end of file diff --git a/drone_assets/models/cylinder2/model.config b/drone_assets/models/cylinder2/model.config new file mode 100644 index 0000000..d06f96d --- /dev/null +++ b/drone_assets/models/cylinder2/model.config @@ -0,0 +1,16 @@ + + + + cylinder2 + 1.0 + model.sdf + + + Alvaro Villamil + a.villamil@hotmail.com + + + + The F1 Monaco track without elevations. + + diff --git a/drone_assets/models/cylinder2/model.sdf b/drone_assets/models/cylinder2/model.sdf new file mode 100644 index 0000000..e9dcb7c --- /dev/null +++ b/drone_assets/models/cylinder2/model.sdf @@ -0,0 +1,107 @@ + + + + + 0 0 2.133 0 0 0 + + 10000 + + 99999999999 + 0 + 0 + 99999999999 + 0 + 99999999999 + + 0 0 0 0 -0 0 + + 0 + 0 + 1 + + + + 0.5 + 1 + + + + + + 0.4 0.6 0.3 1 + 0.698039 0.698039 0.698039 1 + 0.898039 0.898039 0.898039 1 20 + 0.1 0 0.1 1 + + __default__ + + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.5 + 1 + + + + + + 100 + 100 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 1 + + + diff --git a/drone_assets/models/cylinder3/meshes/pared4.dae b/drone_assets/models/cylinder3/meshes/pared4.dae new file mode 100644 index 0000000..b5a0b99 --- /dev/null +++ b/drone_assets/models/cylinder3/meshes/pared4.dae @@ -0,0 +1,93 @@ + + + + + Blender User + Blender 2.79.0 + + 2018-03-20T11:39:29 + 2018-03-20T11:39:29 + + Z_UP + + + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.003006209 0 1 1 + + + 1 + + + + + + + + + + + + + + + + 1 1 -1 1 -1 -1 -1 -0.9999998 -1 -0.9999997 1 -1 1 0.9999995 1 0.9999994 -1.000001 1 -1 -0.9999997 1 -1 1 1 + + + + + + + + + + 0 0 -1 0 0 1 1 0 -2.38419e-7 0 -1 -4.76837e-7 -1 2.38419e-7 -1.49012e-7 2.68221e-7 1 2.38419e-7 0 0 -1 0 0 1 1 -5.96046e-7 3.27825e-7 -4.76837e-7 -1 0 -1 2.38419e-7 -1.19209e-7 2.08616e-7 1 0 + + + + + + + + + + + + + + +

0 0 2 0 3 0 7 1 5 1 4 1 4 2 1 2 0 2 5 3 2 3 1 3 2 4 7 4 3 4 0 5 7 5 4 5 0 6 1 6 2 6 7 7 6 7 5 7 4 8 5 8 1 8 5 9 6 9 2 9 2 10 6 10 7 10 0 11 3 11 7 11

+ + + + + + + + + 1.027213 0 0 -1.084647 0 0.1027213 0 -6 0 0 1.54082 1.6 0 0 0 1 + + + + + + + + + + + + + + \ No newline at end of file diff --git a/drone_assets/models/cylinder3/model.config b/drone_assets/models/cylinder3/model.config new file mode 100644 index 0000000..00c2e5c --- /dev/null +++ b/drone_assets/models/cylinder3/model.config @@ -0,0 +1,16 @@ + + + + cylinder3 + 1.0 + model.sdf + + + Alvaro Villamil + a.villamil@hotmail.com + + + + The F1 Monaco track without elevations. + + diff --git a/drone_assets/models/cylinder3/model.sdf b/drone_assets/models/cylinder3/model.sdf new file mode 100644 index 0000000..d2ebd1c --- /dev/null +++ b/drone_assets/models/cylinder3/model.sdf @@ -0,0 +1,107 @@ + + + + + 0 0 2.133 0 0 0 + + 10000 + + 99999999999 + 0 + 0 + 99999999999 + 0 + 99999999999 + + 0 0 0 0 -0 0 + + 0 + 0 + 1 + + + + 0.5 + 1 + + + + + + 0.698039 0.698039 0.698039 1 + 0.698039 0.698039 0.698039 1 + 0.898039 0.898039 0.898039 1 20 + 1 1 0 1 + + __default__ + + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.5 + 1 + + + + + + 100 + 100 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 1 + + + diff --git a/drone_assets/models/package_box/materials/scripts/package_box.material b/drone_assets/models/package_box_01/materials/scripts/package_box_01.material similarity index 65% rename from drone_assets/models/package_box/materials/scripts/package_box.material rename to drone_assets/models/package_box_01/materials/scripts/package_box_01.material index 8b07c6c..f41f984 100644 --- a/drone_assets/models/package_box/materials/scripts/package_box.material +++ b/drone_assets/models/package_box_01/materials/scripts/package_box_01.material @@ -1,17 +1,17 @@ -material package_box +material package_box_01 { receive shadows off technique { pass { - ambient 0.8 1.0 0.8 1.0 - diffuse 0.8 1.0 0.8 1.0 + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 specular 0.1 0.1 0.1 1.0 12.5 texture_unit { - texture brown.png + texture package_box_01.png filtering anistropic max_anisotropy 16 } diff --git a/drone_assets/models/package_box/materials/textures/brown.png b/drone_assets/models/package_box_01/materials/textures/brown.png similarity index 100% rename from drone_assets/models/package_box/materials/textures/brown.png rename to drone_assets/models/package_box_01/materials/textures/brown.png diff --git a/drone_assets/models/package_box_01/materials/textures/package_box_01.png b/drone_assets/models/package_box_01/materials/textures/package_box_01.png new file mode 100644 index 0000000..7afb35d Binary files /dev/null and b/drone_assets/models/package_box_01/materials/textures/package_box_01.png differ diff --git a/drone_assets/models/package_box/model.config b/drone_assets/models/package_box_01/model.config similarity index 89% rename from drone_assets/models/package_box/model.config rename to drone_assets/models/package_box_01/model.config index c4403cd..631e84a 100644 --- a/drone_assets/models/package_box/model.config +++ b/drone_assets/models/package_box_01/model.config @@ -1,6 +1,6 @@ - package_box + package_box_01 1.0 model.sdf diff --git a/drone_assets/models/package_box/model.sdf b/drone_assets/models/package_box_01/model.sdf similarity index 74% rename from drone_assets/models/package_box/model.sdf rename to drone_assets/models/package_box_01/model.sdf index 1f4eee3..e4f615c 100644 --- a/drone_assets/models/package_box/model.sdf +++ b/drone_assets/models/package_box_01/model.sdf @@ -1,7 +1,7 @@ - - + + 0.1 @@ -15,7 +15,7 @@ 0 0 0.055 0 -0 0 1 - + 0.14 0.12 0.11 @@ -23,13 +23,13 @@ - + 0.14 0.12 0.11 diff --git a/drone_assets/models/package_box_02/materials/scripts/package_box_02.material b/drone_assets/models/package_box_02/materials/scripts/package_box_02.material new file mode 100644 index 0000000..4129770 --- /dev/null +++ b/drone_assets/models/package_box_02/materials/scripts/package_box_02.material @@ -0,0 +1,21 @@ +material package_box_02 +{ + receive shadows off + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.1 0.1 0.1 1.0 12.5 + + texture_unit + { + texture package_box_02.png + filtering anistropic + max_anisotropy 16 + } + + } + } +} \ No newline at end of file diff --git a/drone_assets/models/package_box_02/materials/textures/brown.png b/drone_assets/models/package_box_02/materials/textures/brown.png new file mode 100644 index 0000000..57a8480 Binary files /dev/null and b/drone_assets/models/package_box_02/materials/textures/brown.png differ diff --git a/drone_assets/models/package_box_02/materials/textures/package_box_02.png b/drone_assets/models/package_box_02/materials/textures/package_box_02.png new file mode 100644 index 0000000..65ff824 Binary files /dev/null and b/drone_assets/models/package_box_02/materials/textures/package_box_02.png differ diff --git a/drone_assets/models/package_box_02/model.config b/drone_assets/models/package_box_02/model.config new file mode 100644 index 0000000..17a8957 --- /dev/null +++ b/drone_assets/models/package_box_02/model.config @@ -0,0 +1,11 @@ + + + package_box_02 + 1.0 + model.sdf + + Arka + + + Package Box for Package Delivery exercise in RoboticsAcademy + diff --git a/drone_assets/models/package_box_02/model.sdf b/drone_assets/models/package_box_02/model.sdf new file mode 100644 index 0000000..8644f93 --- /dev/null +++ b/drone_assets/models/package_box_02/model.sdf @@ -0,0 +1,47 @@ + + + + + + 0.1 + + 0.000220833 + 0 + 0 + 0.000264167 + 0 + 0.000283333 + + + 0 0 0.055 0 -0 0 + 1 + + + + 0.14 0.12 0.11 + + + + + + + + + + 0.14 0.12 0.11 + + + + + 0x01 + + + + + 0 + + diff --git a/drone_assets/models/package_box_03/materials/scripts/package_box_03.material b/drone_assets/models/package_box_03/materials/scripts/package_box_03.material new file mode 100644 index 0000000..a7873ea --- /dev/null +++ b/drone_assets/models/package_box_03/materials/scripts/package_box_03.material @@ -0,0 +1,21 @@ +material package_box_03 +{ + receive shadows off + technique + { + pass + { + ambient 1.0 1.0 1.0 1.0 + diffuse 1.0 1.0 1.0 1.0 + specular 0.1 0.1 0.1 1.0 12.5 + + texture_unit + { + texture package_box_03.png + filtering anistropic + max_anisotropy 16 + } + + } + } +} \ No newline at end of file diff --git a/drone_assets/models/package_box_03/materials/textures/brown.png b/drone_assets/models/package_box_03/materials/textures/brown.png new file mode 100644 index 0000000..57a8480 Binary files /dev/null and b/drone_assets/models/package_box_03/materials/textures/brown.png differ diff --git a/drone_assets/models/package_box_03/materials/textures/package_box_03.png b/drone_assets/models/package_box_03/materials/textures/package_box_03.png new file mode 100644 index 0000000..bb41076 Binary files /dev/null and b/drone_assets/models/package_box_03/materials/textures/package_box_03.png differ diff --git a/drone_assets/models/package_box_03/model.config b/drone_assets/models/package_box_03/model.config new file mode 100644 index 0000000..6a97619 --- /dev/null +++ b/drone_assets/models/package_box_03/model.config @@ -0,0 +1,11 @@ + + + package_box_03 + 1.0 + model.sdf + + Arka + + + Package Box for Package Delivery exercise in RoboticsAcademy + diff --git a/drone_assets/models/package_box_03/model.sdf b/drone_assets/models/package_box_03/model.sdf new file mode 100644 index 0000000..7b017c9 --- /dev/null +++ b/drone_assets/models/package_box_03/model.sdf @@ -0,0 +1,47 @@ + + + + + + 0.1 + + 0.000220833 + 0 + 0 + 0.000264167 + 0 + 0.000283333 + + + 0 0 0.055 0 -0 0 + 1 + + + + 0.14 0.12 0.11 + + + + + + + + + + 0.14 0.12 0.11 + + + + + 0x01 + + + + + 0 + + diff --git a/drone_assets/models/simple_labyrinth_green/materials/scripts/simple_labyrinth_green.material b/drone_assets/models/simple_labyrinth_green/materials/scripts/simple_labyrinth_green.material new file mode 100644 index 0000000..993db1f --- /dev/null +++ b/drone_assets/models/simple_labyrinth_green/materials/scripts/simple_labyrinth_green.material @@ -0,0 +1,17 @@ +material simple_labyrinth_green +{ + technique + { + pass + { + lighting off + texture_unit + { + texture simple_labyrinth_green.png + filtering none none none + scale 1.0 1.0 + } + } + } +} + diff --git a/drone_assets/models/simple_labyrinth_green/materials/textures/simple_labyrinth_green.png b/drone_assets/models/simple_labyrinth_green/materials/textures/simple_labyrinth_green.png new file mode 100644 index 0000000..1aec414 Binary files /dev/null and b/drone_assets/models/simple_labyrinth_green/materials/textures/simple_labyrinth_green.png differ diff --git a/drone_assets/models/simple_labyrinth_green/model.sdf b/drone_assets/models/simple_labyrinth_green/model.sdf index 55fa9c6..f5e64cc 100644 --- a/drone_assets/models/simple_labyrinth_green/model.sdf +++ b/drone_assets/models/simple_labyrinth_green/model.sdf @@ -21,8 +21,9 @@ @@ -50,8 +51,9 @@ @@ -79,8 +81,9 @@ @@ -108,8 +111,9 @@ @@ -137,8 +141,9 @@ @@ -166,8 +171,9 @@ @@ -195,8 +201,9 @@ @@ -224,8 +231,9 @@ @@ -253,8 +261,9 @@ @@ -282,8 +291,9 @@ @@ -311,8 +321,9 @@ @@ -340,8 +351,9 @@ @@ -369,8 +381,9 @@ @@ -398,8 +411,9 @@ @@ -427,8 +441,9 @@ @@ -456,8 +471,9 @@ @@ -485,8 +501,9 @@ diff --git a/drone_assets/models/wall1/model.sdf b/drone_assets/models/wall1/model.sdf index cb671fb..fcc5b80 100644 --- a/drone_assets/models/wall1/model.sdf +++ b/drone_assets/models/wall1/model.sdf @@ -29,6 +29,7 @@ Gazebo/Grey file://media/materials/scripts/gazebo.material + 0 0 1 1 0.3 0.3 0.8 1 0.01 0.1 0.01 1 diff --git a/drone_assets/models/wall2/model.sdf b/drone_assets/models/wall2/model.sdf index 259401d..b1e55e4 100644 --- a/drone_assets/models/wall2/model.sdf +++ b/drone_assets/models/wall2/model.sdf @@ -30,10 +30,11 @@ Gazebo/Grey file://media/materials/scripts/gazebo.material - 0 0 1 1 - 0.3 0.3 0.8 1 - 0.1 0.1 0.005 1 - 0 0 0 1 + + 0.3 0.1 0.1 1 + 0.8 0.05 0.05 1 + 0.01 0.1 0.01 1 + 0.1 0 0.1 1 __default__ diff --git a/drone_assets/models/wall3/model.sdf b/drone_assets/models/wall3/model.sdf index eb31853..28b4524 100644 --- a/drone_assets/models/wall3/model.sdf +++ b/drone_assets/models/wall3/model.sdf @@ -29,10 +29,11 @@ Gazebo/Grey file://media/materials/scripts/gazebo.material - 0 0 1 1 - 0.3 0.3 0.8 1 - 0.1 0.01 0.01 1 - 0 0 0 1 + + 0.698039 0.698039 0.698039 1 + 0.698039 0.698039 0.698039 1 + 0.898039 0.898039 0.898039 1 20 + 0 0.945098 0.980392 1 __default__ diff --git a/drone_assets/models/wall4/model.sdf b/drone_assets/models/wall4/model.sdf index f5fbf9e..48c0ede 100644 --- a/drone_assets/models/wall4/model.sdf +++ b/drone_assets/models/wall4/model.sdf @@ -20,19 +20,21 @@ 1 - - 2.5 0.150783 4.266 - + + 0.5 + 1 + - 0 0 1 1 - 0.3 0.3 0.8 1 - 0.1 0.01 0.01 1 - 0 0 0 1 + + 0.698039 0.698039 0.698039 1 + 0.698039 0.698039 0.698039 1 + 0.898039 0.898039 0.898039 1 20 + 1 1 0 1 __default__ @@ -46,9 +48,10 @@ 10 0 0 0 0 -0 0 - - 2.5 0.150783 4.266 - + + 0.5 + 1 + diff --git a/drone_assets/urdf/marker0.material b/drone_assets/urdf/marker0.material new file mode 100644 index 0000000..a5e7479 --- /dev/null +++ b/drone_assets/urdf/marker0.material @@ -0,0 +1,17 @@ +material marker0 +{ + receive shadows off + technique + { + pass + { + texture_unit + { + texture marker0.png + filtering anistropic + max_anisotropy 16 + } + + } + } +} \ No newline at end of file diff --git a/drone_assets/urdf/marker0.png b/drone_assets/urdf/marker0.png new file mode 100755 index 0000000..686090d Binary files /dev/null and b/drone_assets/urdf/marker0.png differ diff --git a/drone_assets/urdf/turtlebot3_waffle_pi_colored_plate.xacro b/drone_assets/urdf/turtlebot3_waffle_pi_colored_plate.xacro index 7c31f58..d7f561a 100644 --- a/drone_assets/urdf/turtlebot3_waffle_pi_colored_plate.xacro +++ b/drone_assets/urdf/turtlebot3_waffle_pi_colored_plate.xacro @@ -7,7 +7,7 @@ - + @@ -21,7 +21,7 @@ - Gazebo/Green + marker0 diff --git a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py index 20e8995..c71ab60 100755 --- a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py +++ b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py @@ -42,7 +42,7 @@ def get_frame(self, frame): elif str(frame).lower() in ["frd", "body_frd", "base_link_frd"]: return "base_link_frd" else: - raise self.UnknownFrameError + return def get_frame_id(self, frame): if str(frame).lower() in ["enu", "local_enu", "map", "world"]: @@ -54,7 +54,7 @@ def get_frame_id(self, frame): elif str(frame).lower() in ["frd", "body_frd", "base_link_frd"]: return 12 else: - raise self.UnknownFrameError + return def id2frame(self, id_): if id_ == self.LOCAL_ENU: @@ -66,7 +66,7 @@ def id2frame(self, id_): elif id_ == self.BODY_FRD: return "base_link_frd" else: - raise self.UnknownFrameError + return def __transform_pose(self, from_, to, pose): if from_ == to: @@ -263,6 +263,18 @@ def cam_ventral_cb(self, msg): rospy.logdebug('Ventral image updated') self.rqt_cam_ventral_publisher.publish(self.ventral_image) + + def depth_cam_frontal_cb(self, msg): + self.depth_frontal_image = msg + rospy.logdebug('Frontal depth image updated') + + self.rqt_depth_cam_frontal_publisher.publish(self.depth_frontal_image) + + def depth_cam_ventral_cb(self, msg): + self.depth_ventral_image = msg + rospy.logdebug('Ventral depth image updated') + + self.rqt_depth_cam_ventral_publisher.publish(self.depth_ventral_image) def stay_armed_stay_offboard_cb(self, event): try: @@ -280,6 +292,12 @@ def get_frontal_image(self): def get_ventral_image(self): return self.bridge.imgmsg_to_cv2(self.ventral_image) + + def get_depth_frontal_image(self): + return self.bridge.imgmsg_to_cv2(self.depth_frontal_image) + + def get_depth_ventral_image(self): + return self.bridge.imgmsg_to_cv2(self.depth_ventral_image) def get_position(self, frame="map"): ps = self.frames_tf.transform(self.pose_stamped.header.frame_id, frame, @@ -532,22 +550,61 @@ def takeoff(self, h=3, precision=0.05): rospy.loginfo('Drone is already flying!') return - self.set_cmd_pos(0, 0, 0, 0) - self.hold_setpoint_raw() - self.arm(True) - self.stay_armed_stay_offboard_timer = rospy.Timer(rospy.Duration(3), self.stay_armed_stay_offboard_cb) - while True: - while not (self.state.armed and self.state.mode == 'OFFBOARD'): - self.rate.sleep() - rospy.loginfo('Sleeping 1 secs to confirm change') - rospy.sleep(1) - if self.state.mode == 'OFFBOARD': - break - self.set_cmd_mix(z=h) - rospy.loginfo('Taking off!!!') - while True: - if abs(self.pose_stamped.pose.position.z - h) < precision: - break + if self.drone_model == 'iris' or self.drone_model == 'iris1': #This condition is specifically for solving Rotors sim bug so dev + #who are using drone_model name as iris, please try to change it or vary the script accordingly for rotors + + self.arm(True) + self.stay_armed_stay_offboard_timer = rospy.Timer(rospy.Duration(3), self.stay_armed_stay_offboard_cb) + pose = Pose(position=Point(x=self.pose_stamped.pose.position.x, + y=self.pose_stamped.pose.position.y, z=h)) + pose = self.frames_tf.transform("map", "map", pose).pose + + twist = Twist(linear=Vector3(x=0.0, y=0.0, z=0), + angular=Vector3(x=0, y=0, z=0.0)) + twist = self.frames_tf.transform("flu", "flu", twist).twist + + self.coord_frame = 8 + self.setpoint_raw.header.frame_id = self.frames_tf.get_frame("flu") + self.setpoint_raw.coordinate_frame = self.coord_frame + + self.posx = self.pose_stamped.pose.position.x + self.posy = self.pose_stamped.pose.position.y + self.height = pose.position.z + self.vx = twist.linear.x + self.vy = twist.linear.y + self.vz = 0 + + self.setpoint_raw.position.z = pose.position.z + self.setpoint_raw.velocity.x = twist.linear.x + self.setpoint_raw.velocity.y = twist.linear.y + self.setpoint_raw.yaw_rate = twist.angular.z + + global CMD + CMD = 2 # MIX + self.setpoint_raw.type_mask = 1987 # vx vy vz z yaw_rate + + self.setpoint_raw_publisher.publish(self.setpoint_raw) + + rospy.loginfo('Taking off!!!') + + + else: + self.set_cmd_pos(0, 0, 0, 0) + self.hold_setpoint_raw() + self.arm(True) + self.stay_armed_stay_offboard_timer = rospy.Timer(rospy.Duration(3), self.stay_armed_stay_offboard_cb) + while True: + while not (self.state.armed and self.state.mode == 'OFFBOARD'): + self.rate.sleep() + rospy.loginfo('Sleeping 1 secs to confirm change') + rospy.sleep(1) + if self.state.mode == 'OFFBOARD': + break + self.set_cmd_mix(z=h) + rospy.loginfo('Taking off!!!') + while True: + if abs(self.pose_stamped.pose.position.z - h) < precision: + break self.set_cmd_vel() # NOT USED @@ -591,7 +648,7 @@ def __init__(self, name='drone', ns='', verbose=False): rospy.on_shutdown(self.shutdown) - drone_model = rospy.get_param('drone_model', 'iris') # default --> iris + self.drone_model = rospy.get_param('drone_model', 'iris') # default --> iris self.state = State() self.extended_state = ExtendedState() @@ -657,15 +714,28 @@ def __init__(self, name='drone', ns='', verbose=False): # cam_frontal_topic = rospy.get_param('cam_frontal_topic', None) # cam_frontal_topic = rospy.get_param('cam_frontal_topic', '/iris/cam_frontal/image_raw') - drone_model = rospy.get_param('drone_model', 'iris') # default --> iris + self.drone_model = rospy.get_param(self.ns +'drone_model', 'iris') # default --> iris + enable_depth = rospy.get_param(self.ns +'enable_depth', False) self.frontal_image = Image() self.ventral_image = Image() - cam_frontal_topic = '/' + drone_model + '/cam_frontal/image_raw' - cam_ventral_topic = '/' + drone_model + '/cam_ventral/image_raw' + cam_frontal_topic = '/' + self.drone_model + '/cam_frontal/image_raw' + cam_ventral_topic = '/' + self.drone_model + '/cam_ventral/image_raw' rospy.Subscriber(cam_frontal_topic, Image, self.cam_frontal_cb) rospy.Subscriber(cam_ventral_topic, Image, self.cam_ventral_cb) + if enable_depth: + self.rqt_depth_cam_frontal_publisher = rospy.Publisher(self.ns + 'drone_wrapper/depth_cam_frontal/image_raw', Image, + queue_size=1) + self.rqt_depth_cam_ventral_publisher = rospy.Publisher(self.ns + 'drone_wrapper/depth_cam_ventral/image_raw', Image, + queue_size=1) + self.depth_frontal_image = Image() + self.depth_ventral_image = Image() + depth_cam_frontal_topic = '/' + self.drone_model + '/depth_cam_frontal/image_raw' + depth_cam_ventral_topic = '/' + self.drone_model + '/depth_cam_ventral/image_raw' + rospy.Subscriber(depth_cam_frontal_topic, Image, self.depth_cam_frontal_cb) + rospy.Subscriber(depth_cam_ventral_topic, Image, self.depth_cam_ventral_cb) + self.setpoint_raw_publisher = rospy.Publisher(self.ns + 'mavros/setpoint_raw/local', PositionTarget, queue_size=1) diff --git a/rotors_driver/src/rotors_driver_node.py b/rotors_driver/src/rotors_driver_node.py index d2cd1af..030dcd3 100755 --- a/rotors_driver/src/rotors_driver_node.py +++ b/rotors_driver/src/rotors_driver_node.py @@ -39,10 +39,12 @@ class RotorsDriver(): CONSTRAINTS = RotorsConstraints() def __init__(self): - self.sample_time = rospy.get_param('sample_time', 1.0) - self.mav_name = rospy.get_param('drone_model', 'firefly') - self.drone_state_upd_freq = rospy.get_param('drone_state_timer_frequency', 0.01) - self.misc_state_upd_freq = rospy.get_param('misc_state_timer_frequency', 1.0) + self.ns = rospy.get_namespace() + self.sample_time = rospy.get_param(self.ns +'sample_time', 1.0) + self.mav_name = rospy.get_param(self.ns +'drone_model', 'iris') + self.drone_state_upd_freq = rospy.get_param(self.ns +'drone_state_timer_frequency', 0.01) + self.misc_state_upd_freq = rospy.get_param(self.ns +'misc_state_timer_frequency', 1.0) + self.enable_depth = rospy.get_param(self.ns +'enable_depth', False) self.drone_flight_state = States.LANDED self.current_state = Odometry() self.current_x=self.current_state.pose.pose.position.x @@ -82,25 +84,31 @@ def __init__(self): queue_size=1) self.velocity_body_publisher = rospy.Publisher('mavros/local_position/velocity_body', TwistStamped, queue_size=1) - self.cam_frontal_publisher = rospy.Publisher('/' + self.mav_name + '/cam_frontal/image_raw', Image, + self.cam_frontal_publisher = rospy.Publisher(self.ns + 'cam_frontal/image_raw', Image, queue_size=1) - self.cam_ventral_publisher = rospy.Publisher('/' + self.mav_name + '/cam_ventral/image_raw', Image, + self.cam_ventral_publisher = rospy.Publisher(self.ns + 'cam_ventral/image_raw', Image, queue_size=1) - - - - rospy.Subscriber("mavros/setpoint_raw/local", PositionTarget, self.publish_position_desired) - + if self.enable_depth: + self.depth_frontal_image = Image() + self.depth_ventral_image = Image() + self.depth_cam_frontal_publisher = rospy.Publisher(self.ns + 'depth_cam_frontal/image_raw', Image, + queue_size=1) + self.depth_cam_ventral_publisher = rospy.Publisher(self.ns + 'depth_cam_ventral/image_raw', Image, + queue_size=1) + rospy.Subscriber(self.ns + 'vi_sensor_frontal/camera_depth/camera/image_raw', Image, self.depth_cam_frontal) + rospy.Subscriber(self.ns + 'vi_sensor_ventral/camera_depth/camera/image_raw', Image, self.depth_cam_ventral) - rospy.Subscriber('/' + self.mav_name +"/ground_truth/odometry", Odometry, self.odom_callback) - rospy.Subscriber('/' + self.mav_name +"/frontal_cam/camera_nadir/image_raw", Image, self.cam_frontal) - rospy.Subscriber('/' + self.mav_name +"/ventral_cam/camera_nadir/image_raw", Image, self.cam_ventral) - self.firefly_command_publisher = rospy.Publisher('/firefly/command/trajectory', MultiDOFJointTrajectory, queue_size=10) + rospy.Subscriber(self.ns + 'ground_truth/odometry', Odometry, self.odom_callback) + rospy.Subscriber(self.ns + 'frontal_cam/camera_nadir/image_raw', Image, self.cam_frontal) + rospy.Subscriber(self.ns + 'ventral_cam/camera_nadir/image_raw', Image, self.cam_ventral) + time.sleep(1.0) rospy.Timer(rospy.Duration(self.drone_state_upd_freq), self.drone_state_update_callback) rospy.Timer(rospy.Duration(self.misc_state_upd_freq), self.misc_state_update_callback) + self.iris_command_publisher = rospy.Publisher(self.ns + 'command/trajectory', MultiDOFJointTrajectory, queue_size=10) + rospy.Subscriber("mavros/setpoint_raw/local", PositionTarget, self.publish_position_desired) def drone_state_update_callback(self, event=None): @@ -117,6 +125,10 @@ def drone_state_update_callback(self, event=None): self.cam_frontal_publisher.publish(self.frontal_image) self.cam_ventral_publisher.publish(self.ventral_image) + + if self.enable_depth: + self.depth_cam_frontal_publisher.publish(self.depth_frontal_image) + self.depth_cam_ventral_publisher.publish(self.depth_ventral_image) def misc_state_update_callback(self, event=None): landed_state = 2 if self.drone_flight_state == 4 else 1 @@ -148,6 +160,14 @@ def cam_frontal(self, msg): def cam_ventral(self, msg): self.ventral_image = msg rospy.logdebug('Ventral image updated') + + def depth_cam_frontal(self, msg): + self.depth_frontal_image = msg + rospy.logdebug('Frontal depth image updated') + + def depth_cam_ventral(self, msg): + self.depth_ventral_image = msg + rospy.logdebug('Ventral depth image updated') def odom_callback(self,msg): self.current_state = msg @@ -198,7 +218,7 @@ def rotors_landing(self,req): while not(0.0<=self.current_z<0.1): - self.firefly_command_publisher.publish(traj) + self.iris_command_publisher.publish(traj) if not req : return True, 0 @@ -213,13 +233,13 @@ def rotors_arm(self, req): # bool success # uint8 result if req.value: - rospy.loginfo("Firefly Arming") + rospy.loginfo("iris Arming") self.drone_flight_state = States.ARMING time.sleep(0.01) self.drone_flight_state = States.ARMED return True, 1 else: - rospy.loginfo("Firefly Disarming") + rospy.loginfo("iris Disarming") self.drone_flight_state = States.DISARMING time.sleep(0.01) self.drone_flight_state = States.DISARMED @@ -290,13 +310,13 @@ def publish_position_desired(self, msg): self.drone_flight_state = States.TAKINGOFF - if (desired_x_to_go ==0.0 and desired_y_to_go ==0.0 and yaw_des == 0.0) and (vx_des >0.0 or vy_des >0.0 or yaw_rate_des>0) and vz_des >=0.0: + if (desired_x_to_go ==0.0 and desired_y_to_go ==0.0 and yaw_des == 0.0 and desired_z_to_go == 0.0) and (vx_des >0.0 or vy_des >0.0 or yaw_rate_des>0) and vz_des >=0.0: desired_x_to_go=self.current_x+(vx_des*self.sample_time) desired_y_to_go=self.current_y+(vy_des*self.sample_time) desired_z_to_go=self.current_z+(vz_des*self.sample_time) desired_yaw_to_go = self.current_yaw +(yaw_rate_des*self.sample_time) print("Vel control") - elif (desired_x_to_go ==0.0 and desired_y_to_go ==0.0 and vz_des ==0.0 and yaw_des == 0.0) and ((vx_des >0.0 or vy_des >0.0 or yaw_rate_des>0)and desired_z_to_go>0.0 ): + elif (desired_x_to_go ==0.0 and desired_y_to_go ==0.0 and vz_des ==0.0 and yaw_des == 0.0) and ((vx_des >0.0 or vy_des >0.0 or yaw_rate_des>0)and desired_z_to_go>=0.0 ): desired_x_to_go=self.current_x+(vx_des*self.sample_time) desired_y_to_go=self.current_y+(vy_des*self.sample_time) desired_z_to_go= desired_z_to_go @@ -335,7 +355,7 @@ def publish_position_desired(self, msg): traj.points.append(point) # time.sleep(0.1) #commented out for vel control - self.firefly_command_publisher.publish(traj) + self.iris_command_publisher.publish(traj) # Deleting (Calling destructor) def __del__(self):