diff --git a/libraries/SITL/examples/Webots_Python/controllers/ardupilot_vehicle_controller/webots_vehicle.py b/libraries/SITL/examples/Webots_Python/controllers/ardupilot_vehicle_controller/webots_vehicle.py index acd6f5641f..459cac2d89 100644 --- a/libraries/SITL/examples/Webots_Python/controllers/ardupilot_vehicle_controller/webots_vehicle.py +++ b/libraries/SITL/examples/Webots_Python/controllers/ardupilot_vehicle_controller/webots_vehicle.py @@ -155,14 +155,13 @@ class WebotsArduVehicle(): Args: port (int, optional): Port to listen for SITL on. Defaults to 9002. """ - # create a local UDP socket server to listen for SITL s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # SOCK_STREAM s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) s.bind(('0.0.0.0', port)) # wait for SITL to connect - print(f"Listening for ardupilot SITL (I{self._instance}) at 127.0.0.1:{port}") + print(f"Listening for ardupilot SITL (I{self._instance}) at {sitl_address}:{port}") self.robot.step(self._timestep) # flush print in webots console while not select.select([s], [], [], 0)[0]: # wait for socket to be readable @@ -241,12 +240,12 @@ class WebotsArduVehicle(): Args: command (tuple): tuple of motor speeds 0.0-1.0 where -1.0 is unused """ - # get only the number of motors we have command_motors = command[:len(self._motors)] if -1 in command_motors: print(f"Warning: SITL provided {command.index(-1)} motors " f"but model specifies {len(self._motors)} (I{self._instance})") + command_motors = [v if v != -1 else 0.0 for v in command_motors] # scale commands to -1.0-1.0 if the motors are bidirectional (ex rover wheels) if self._bidirectional_motors: diff --git a/libraries/SITL/examples/Webots_Python/params/heron.parm b/libraries/SITL/examples/Webots_Python/params/heron.parm new file mode 100644 index 0000000000..a5b6e26e8e --- /dev/null +++ b/libraries/SITL/examples/Webots_Python/params/heron.parm @@ -0,0 +1,13 @@ +FRAME_CLASS 2 # Boat + +SERVO1_FUNCTION 73 # Throttle Left (Boat throttle channel) +SERVO2_FUNCTION 74 # Throttle Right (Boat throttle channel) +SERVO3_FUNCTION 0 # Disabled (no steering required) +SERVO4_FUNCTION 0 # Disabled (no steering required) + +AHRS_EKF_TYPE 10 # Use sim AHRS +#ARMING_CHECK 1045534 # No RC requirements +ARMING_CHECK 0 # No RC requirements +MOT_THST_EXPO 0.3 # Small motor exponential curve for smoother throttle +SCHED_LOOP_RATE 300 +INS_GYR_CAL 0 # No gyro calibration (use simulated or fixed values) diff --git a/libraries/SITL/examples/Webots_Python/protos/Heron.proto b/libraries/SITL/examples/Webots_Python/protos/Heron.proto new file mode 100644 index 0000000000..b7e412a493 --- /dev/null +++ b/libraries/SITL/examples/Webots_Python/protos/Heron.proto @@ -0,0 +1,190 @@ +#VRML_SIM R2023a utf8 +# license: Copyright Cyberbotics Ltd. Licensed for use only with Webots. +# license url: https://cyberbotics.com/webots_assets_license +# documentation url: https://webots.cloud/run?url=https://github.com/cyberbotics/webots/blob/released/projects/robots/clearpath/heron/protos/Heron.proto +# keywords: robot/wheeled +# Clearpath Heron USV is an autonomous watercraft designed for various marine tasks. +# It features electric propulsion, advanced navigation systems, and integration capabilities for collaborative missions. +# With a streamlined hull and multiple sensor suites including cameras, +# it excels in environmental monitoring, surveillance, and research. + +PROTO Heron [ + field SFVec3f translation 0 0 0.0 # Is `Pose.translation`. + field SFRotation rotation 0 0 1 0 # Is `Pose.rotation`. + field SFString name "Heron USV" # Is `Solid.name`. + field SFString controller "void" # Is `Robot.controller`. + field MFString controllerArgs [] # Is `Robot.controllerArgs`. + field SFString window "" # Is `Robot.window`. + field SFBool synchronization TRUE # Is `Robot.synchronization`. + field MFNode bodySlot [] # Extends the robot with new nodes in the body slot. +] +{ + Robot { + translation IS translation + rotation IS rotation + name IS name + model "Clearpath Heron" + controller IS controller + controllerArgs IS controllerArgs + window IS window + synchronization IS synchronization + children [ + DEF BODY_SLOT Group { + children IS bodySlot + } + Transform { + translation -0.185 0 0 + rotation 0 0 1 1.01503e-06 + children [ + Shape { + castShadows FALSE + appearance PBRAppearance { + baseColor 0.964706 0.827451 0.176471 + metalness 0 + } + geometry Mesh { + url [ + "obj/plain-rear-plate.stl" + ] + } + } + ] + } + Transform { + translation 0 -0.35 0.05 + children [ + Shape { + castShadows FALSE + appearance PBRAppearance { + baseColor 0.960784 0.760784 0.0666667 + metalness 0 + } + geometry Mesh { + url [ + "obj/right_panel.stl" + ] + } + } + ] + } + Transform { + translation 0 0.35 0.05 + children [ + Shape { + castShadows FALSE + appearance PBRAppearance { + baseColor 0.960784 0.760784 0.0666667 + metalness 0 + } + geometry Mesh { + url [ + "obj/left_panel.stl" + ] + } + } + ] + } + Transform { + translation -0.73 0.38 -0.14 + children [ + Propeller { + device RotationalMotor { + name "left_motor" + maxVelocity 30 + } + fastHelix Solid { + rotation 0 1 0 1.57 + children [ + Shape { + castShadows FALSE + appearance PBRAppearance { + baseColor 0.368627 0.360784 0.392157 + } + geometry Cylinder { + height 0.1 + radius 0.05 + } + } + ] + } + } + ] + } + Transform { + translation -0.73 -0.38 -0.14 + children [ + Propeller { + device RotationalMotor { + name "right_motor" + maxVelocity 30 + } + fastHelix Solid { + rotation 0 1 0 1.5708 + children [ + Shape { + castShadows FALSE + appearance PBRAppearance { + baseColor 0.368627 0.360784 0.392157 + } + geometry Cylinder { + height 0.1 + radius 0.05 + } + } + ] + } + } + ] + } + DEF HERON Shape { + castShadows FALSE + appearance Appearance { + material Material { + diffuseColor 0.239216 0.219608 0.27451 + } + texture ImageTexture { + } + textureTransform TextureTransform { + } + } + geometry Mesh { + url [ + "obj/heron_base.stl" + ] + } + } + ] + name "heron_usv" + immersionProperties [ + ImmersionProperties { + fluidName "fluid" + dragForceCoefficients 0.01 0 0 + dragTorqueCoefficients 0.05 0 0 + viscousResistanceForceCoefficient 400 + viscousResistanceTorqueCoefficient 0.1 + } + ] + boundingObject Transform { + scale 0.1 0.1 0.1 + children [ + Mesh { + url [ + "obj/heron_collision.stl" + ] + } + ] + } + physics Physics { + density 400 + centerOfMass [ + 0 0 0 + ] + damping Damping { + linear 0.5 + angular 0.5 + } + } + linearVelocity -1.4502243780299133e-41 2.229921041216923e-42 0.0058372174879208726 + angularVelocity 0.00023835911050514857 0.001746443229709772 4.0940932765468614e-19 +} +} diff --git a/libraries/SITL/examples/Webots_Python/protos/obj/heron_base.stl b/libraries/SITL/examples/Webots_Python/protos/obj/heron_base.stl new file mode 100644 index 0000000000..808a9fd6c0 Binary files /dev/null and b/libraries/SITL/examples/Webots_Python/protos/obj/heron_base.stl differ diff --git a/libraries/SITL/examples/Webots_Python/protos/obj/heron_collision.stl b/libraries/SITL/examples/Webots_Python/protos/obj/heron_collision.stl new file mode 100644 index 0000000000..be9cf2a245 Binary files /dev/null and b/libraries/SITL/examples/Webots_Python/protos/obj/heron_collision.stl differ diff --git a/libraries/SITL/examples/Webots_Python/protos/obj/left_panel.stl b/libraries/SITL/examples/Webots_Python/protos/obj/left_panel.stl new file mode 100644 index 0000000000..fe00513032 Binary files /dev/null and b/libraries/SITL/examples/Webots_Python/protos/obj/left_panel.stl differ diff --git a/libraries/SITL/examples/Webots_Python/protos/obj/lidar-rear-plate.stl b/libraries/SITL/examples/Webots_Python/protos/obj/lidar-rear-plate.stl new file mode 100644 index 0000000000..89a5875ef9 Binary files /dev/null and b/libraries/SITL/examples/Webots_Python/protos/obj/lidar-rear-plate.stl differ diff --git a/libraries/SITL/examples/Webots_Python/protos/obj/plain-rear-plate.stl b/libraries/SITL/examples/Webots_Python/protos/obj/plain-rear-plate.stl new file mode 100644 index 0000000000..e809f832cc Binary files /dev/null and b/libraries/SITL/examples/Webots_Python/protos/obj/plain-rear-plate.stl differ diff --git a/libraries/SITL/examples/Webots_Python/protos/obj/right_panel.stl b/libraries/SITL/examples/Webots_Python/protos/obj/right_panel.stl new file mode 100644 index 0000000000..5de9251df6 Binary files /dev/null and b/libraries/SITL/examples/Webots_Python/protos/obj/right_panel.stl differ diff --git a/libraries/SITL/examples/Webots_Python/worlds/heron_ocean.wbt b/libraries/SITL/examples/Webots_Python/worlds/heron_ocean.wbt new file mode 100644 index 0000000000..f6385068ba --- /dev/null +++ b/libraries/SITL/examples/Webots_Python/worlds/heron_ocean.wbt @@ -0,0 +1,152 @@ +#VRML_SIM R2023a utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/obstacles/protos/OilBarrel.proto" +EXTERNPROTO "../protos/Heron.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/robots/nvidia/jetbot/protos/JetBotRaspberryPiCamera.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/devices/nvidia/protos/JetsonNano.proto" + +WorldInfo { +} +Viewpoint { + orientation 0.18802303494067113 0.008368575014304897 -0.9821289657086432 2.6567413787072653 + position -3.5173698357397702 0.9533488761989521 1.9238550170857296 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +DEF WATER Fluid { + children [ + Shape { + appearance PBRAppearance { + baseColor 0.0626841 0.552224 0.723339 + transparency 0.7 + roughness 1 + metalness 0 + } + geometry DEF WATER_BOX Box { + size 1000 1000 0.7 + } + } + ] + viscosity 0.01 + boundingObject Transform { + children [ + USE WATER_BOX + ] + } + locked TRUE +} +Heron { + hidden linearVelocity_0 0 0 -3.545123546997953e-14 + hidden angularVelocity_0 4.609404596900323e-14 6.687054702593327e-13 2.3099636435669426e-16 + translation -6.6784157308512615 -0.49028637658352825 0.4450058268441469 + rotation -0.010053065114475615 -0.041473403722977586 -0.9990890314008232 0.517597020428728 + controller "ardupilot_vehicle_controller" + controllerArgs [ + "--motors" + "left_motor, right_motor" + "--motor-cap" + "10" + "--bidirectional-motors" + "true" + "--camera" + "camera" + "--camera-port" + "5599" + "--sitl-address" + "localhost" + ] + bodySlot [ + JetsonNano { + translation 0.17 0.0700002 0.08 + rotation 0 0 1 -1.5707953071795862 + } + JetBotRaspberryPiCamera { + translation 0.32 0 0.22 + fieldOfView 0.7 + near 0.1 + } + InertialUnit { + } + GPS { + } + Accelerometer { + } + Gyro { + } + ] +} +OilBarrel { + hidden linearVelocity_0 0 0 1.4387456326881762e-15 + translation -5.769041604986175 5.7137363726152754 0.5595498919704248 + rotation -0.1699998685374228 -0.9047911400234424 0.39045235001513096 1.7528084979303689 + physics Physics { + density 100 + damping Damping { + linear 0.5 + angular 0.5 + } + } + immersionProperties [ + ImmersionProperties { + fluidName "fluid" + } + ] +} +OilBarrel { + hidden linearVelocity_0 0 0 1.4387456326881762e-15 + translation -10.052079249302796 2.4814946123187727 0.5595498919704248 + rotation -0.27511330388511257 -0.6601076264091592 0.6989782482895184 -2.843722039686051 + name "oil barrel(1)" + physics Physics { + density 100 + damping Damping { + linear 0.5 + angular 0.5 + } + } + immersionProperties [ + ImmersionProperties { + fluidName "fluid" + } + ] +} +OilBarrel { + hidden linearVelocity_0 0 0 -1.5261006141050254e-15 + translation -9.806804324970415 -3.2705951440882433 0.5595498919704249 + rotation 0.9914248557443196 0.045597095586811014 0.12246493492588871 1.5859460279251565 + name "oil barrel(2)" + physics Physics { + density 100 + damping Damping { + linear 0.5 + angular 0.5 + } + } + immersionProperties [ + ImmersionProperties { + fluidName "fluid" + } + ] +} +OilBarrel { + hidden linearVelocity_0 0 0 1.4387456326881762e-15 + translation -9.046348837161165 3.2463636605243056 0.5595498919704248 + rotation 0.7378366847241952 0.14393236835260417 -0.6594546989866846 -2.4506564948848384 + name "oil barrel(3)" + physics Physics { + density 100 + damping Damping { + linear 0.5 + angular 0.5 + } + } + immersionProperties [ + ImmersionProperties { + fluidName "fluid" + } + ] +}