diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..46d9a84
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,3 @@
+{
+ "ros.distro": "humble"
+}
\ No newline at end of file
diff --git a/docker/bashrc b/docker/bashrc
index a65ba2b..c55e3fc 100644
--- a/docker/bashrc
+++ b/docker/bashrc
@@ -2,3 +2,4 @@ source /opt/ros/humble/setup.bash
source /${DOCKERUSER}/ros2_ws/install/local_setup.bash
source /${DOCKERUSER}/ros2_libs_ws/install/local_setup.bash
alias cb='colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug'
+export USERNAME='default'
\ No newline at end of file
diff --git a/spesbot_description/urdf/mobile_base.xacro b/spesbot_description/urdf/mobile_base.xacro
index 139a01c..2998d27 100644
--- a/spesbot_description/urdf/mobile_base.xacro
+++ b/spesbot_description/urdf/mobile_base.xacro
@@ -8,6 +8,17 @@
filename="package://spesbot_description/urdf/meshes/mobile_base.stl" />
+
+
+
+
+
+
+
+
+
+
+
@@ -22,6 +33,17 @@
filename="package://spesbot_description/urdf/meshes/wheel.stl" />
+
+
+
+
+
+
+
+
+
+
+
@@ -37,6 +59,17 @@
+
+
+
+
+
+
+
+
+
+
+
@@ -51,6 +84,17 @@
filename="package://spesbot_description/urdf/meshes/caster_holder.stl" />
+
+
+
+
+
+
+
+
+
+
+
@@ -65,6 +109,21 @@
filename="package://spesbot_description/urdf/meshes/caster_wheel.stl" />
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
-
\ No newline at end of file
+
diff --git a/spesbot_webots/README.md b/spesbot_webots/README.md
new file mode 100644
index 0000000..712c391
--- /dev/null
+++ b/spesbot_webots/README.md
@@ -0,0 +1,7 @@
+# SpesBot Webots Simulation
+
+Regenerate the Webots model:
+```bash
+pip3 install urdf2webots
+xacro ${HOME}/ros2_ws/src/spesbot/spesbot_description/urdf/spesbot.xacro | python3 -m urdf2webots.importer --output=${HOME}/ros2_ws/src/spesbot/spesbot_webots/data/protos/SpesBot.proto --tool-slot=base_link
+```
diff --git a/spesbot_webots/data/protos/SpesBot.proto b/spesbot_webots/data/protos/SpesBot.proto
index 3a1800f..9fa318d 100644
--- a/spesbot_webots/data/protos/SpesBot.proto
+++ b/spesbot_webots/data/protos/SpesBot.proto
@@ -1,233 +1,259 @@
#VRML_SIM R2023b utf8
-# template language: javascript
-
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/devices/robotis/protos/RobotisLds01.proto"
+# license: Apache License 2.0
+# license url: http://www.apache.org/licenses/LICENSE-2.0
+# This is a proto file for Webots for the SpesBot
+# Extracted from a URDF content string
PROTO SpesBot [
- field SFVec3f translation 0 0 0
- field SFRotation rotation 0 1 0 0
+ field SFVec3f translation 0 0 0
+ field SFRotation rotation 0 0 1 0
+ field SFString name "SpesBot" # Is `Robot.name`.
+ field SFString controller "void" # Is `Robot.controller`.
+ field MFString controllerArgs [] # Is `Robot.controllerArgs`.
+ field SFString customData "" # Is `Robot.customData`.
+ field SFBool supervisor FALSE # Is `Robot.supervisor`.
+ field SFBool synchronization TRUE # Is `Robot.synchronization`.
+ field SFBool selfCollision FALSE # Is `Robot.selfCollision`.
+ field MFNode toolSlot [] # Extend the robot with new nodes at the end of the arm.
]
{
-Robot {
- translation IS translation
- rotation IS rotation
- children [
- Solid {
- translation 0 0 0.1
- children [
- Camera {
- translation 0.01 0 0.11
- rotation 0 1 0 0.4017996938995747
- locked TRUE
- width 1280
- height 720
- far 0.6
+ Robot {
+ translation IS translation
+ rotation IS rotation
+ controller IS controller
+ controllerArgs IS controllerArgs
+ customData IS customData
+ supervisor IS supervisor
+ synchronization IS synchronization
+ selfCollision IS selfCollision
+ children [
+ Shape {
+ appearance PBRAppearance {
+ baseColor 0.500000 0.500000 0.500000
+ roughness 1.000000
+ metalness 0
}
- RobotisLds01 {
- translation 0.05 0 0.05
- rotation 0 0 1 0
+ geometry DEF mobile_base Mesh {
+ url "/spesbot/ros2_ws/install/spesbot_description/share/spesbot_description/urdf/meshes/mobile_base.stl"
}
- Shape {
- appearance Appearance {
- material Material {
- diffuseColor 0.5 0.5 0.5
- }
+ }
+ HingeJoint {
+ jointParameters HingeJointParameters {
+ axis 0.000000 1.000000 0.000000
+ anchor 0.000000 0.145000 0.000000
+ }
+ device [
+ RotationalMotor {
+ name "left_motor"
+ maxTorque 10000
}
- geometry DEF CAMERA_SUPPORT Box {
- size 0.02 0.05 0.2
+ PositionSensor {
+ name "left_motor_sensor"
}
- }
- ]
- name "camera_support"
- boundingObject USE CAMERA_SUPPORT
- physics Physics {
- }
- }
- DEF CENTRAL_WHEEL Solid {
- translation -0.28 -3.44968e-07 -0.02
- rotation 0 0 1 3.14159
- children [
- DEF wheel_suport Solid {
- translation -0.0010778898115112734 3.647104313117544e-05 -0.0051394172830611096
- rotation -0.010125424020987739 0.9999349777505068 0.005245575229558288 0.05571206851003739
+ ]
+ endPoint Solid {
+ translation 0.000000 0.145000 0.000000
children [
- Solid {
- translation 0 0 -0.04
- children [
- DEF sphere Shape {
- appearance Appearance {
- material Material {
- diffuseColor 0.1 0.1 0.1
- }
- }
- geometry Sphere {
- radius 0.02
- }
- }
- ]
- boundingObject USE sphere
- physics Physics {
+ Shape {
+ appearance PBRAppearance {
+ baseColor 0.500000 0.500000 0.500000
+ roughness 1.000000
+ metalness 0
}
- }
- DEF WHEEL_SUPORT Shape {
- appearance Appearance {
- material Material {
- diffuseColor 0.5 0.5 0.5
- }
- }
- geometry Box {
- size 0.01 0.015 0.11
+ geometry DEF wheel Mesh {
+ url "/spesbot/ros2_ws/install/spesbot_description/share/spesbot_description/urdf/meshes/wheel.stl"
}
}
]
- name "wheel_suport"
- boundingObject USE WHEEL_SUPORT
+ name "left_wheel_link"
+ boundingObject Pose {
+ translation 0.000000 0.030000 0.000000
+ rotation 1.000000 0.000000 0.000000 1.570700
+ children [
+ Cylinder {
+ radius 0.085
+ height 0.03
+ }
+ ]
+ }
physics Physics {
+ density -1
+ mass 2.000000
+ centerOfMass [ 0.000000 0.000000 0.000000 ]
+ inertiaMatrix [
+ 1.000000e-04 1.000000e-04 1.000000e-04
+ 0.000000e+00 0.000000e+00 0.000000e+00
+ ]
}
}
- ]
- name "central_wheel_solid"
- physics Physics {
}
- }
- DEF CENTRAL_PROFILE Solid {
- translation -0.275 -0.000495055 0.03
- rotation 0 0 1 -1.5708053071795867
- children [
- DEF CENTER Shape {
- appearance Appearance {
- material Material {
- diffuseColor 0.5 0.5 0.5
- }
- }
- geometry Box {
- size 0.095 0.02 0.02
- }
+ HingeJoint {
+ jointParameters HingeJointParameters {
+ axis 0.000000 1.000000 0.000000
+ anchor 0.000000 -0.145000 0.000000
}
- ]
- name "center"
- boundingObject USE CENTER
- physics Physics {
- }
- }
- DEF RIGHT_PROFILE Solid {
- translation -0.219965 -0.089017 0.03
- rotation 0 0 -1 0.6545063061004254
- children [
- DEF RIGHT Shape {
- appearance Appearance {
- material Material {
- diffuseColor 0.5 0.5 0.5
- }
+ device [
+ RotationalMotor {
+ name "right_motor"
+ maxTorque 10000
}
- geometry Box {
- size 0.15 0.02 0.02
+ PositionSensor {
+ name "right_motor_sensor"
}
- }
- ]
- name "right_profile"
- boundingObject USE RIGHT
- physics Physics {
- }
- }
- DEF LEFT_PROFILE Solid {
- translation -0.219957 0.0890142 0.03
- rotation 0 0 -1 -0.654506
- children [
- DEF RIGHT Shape {
- appearance Appearance {
- material Material {
- diffuseColor 0.5 0.5 0.5
+ ]
+ endPoint Solid {
+ translation 0.000000 -0.145000 0.000000
+ children [
+ Pose {
+ rotation 0.000000 0.000000 1.000000 3.141590
+ children [
+ Shape {
+ appearance PBRAppearance {
+ baseColor 0.500000 0.500000 0.500000
+ roughness 1.000000
+ metalness 0
+ }
+ geometry USE wheel
+ }
+ ]
}
+ ]
+ name "right_wheel_link"
+ boundingObject Pose {
+ translation 0.000000 -0.030000 0.000000
+ rotation 1.000000 0.000000 0.000000 1.570700
+ children [
+ Cylinder {
+ radius 0.085
+ height 0.03
+ }
+ ]
}
- geometry Box {
- size 0.15 0.02 0.02
+ physics Physics {
+ density -1
+ mass 2.000000
+ centerOfMass [ 0.000000 0.000000 0.000000 ]
+ inertiaMatrix [
+ 1.000000e-04 1.000000e-04 1.000000e-04
+ 0.000000e+00 0.000000e+00 0.000000e+00
+ ]
}
}
- ]
- name "left"
- boundingObject USE RIGHT
- physics Physics {
}
- }
- DEF BASE_BODY Transform {
- translation 0 0 0.03
- children [
- Shape {
- appearance Appearance {
- material Material {
- diffuseColor 0.5 0.5 0.5
- }
+ HingeJoint {
+ jointParameters HingeJointParameters {
+ axis 0.000000 0.000000 1.000000
+ anchor -0.310000 0.000000 0.000000
+ }
+ device [
+ RotationalMotor {
+ name "caster_steering_joint"
+ maxTorque 10000
}
- geometry Box {
- size 0.33 0.29 0.02
+ PositionSensor {
+ name "caster_steering_joint_sensor"
}
- }
- ]
- }
- DEF LEFT_JOINT HingeJoint {
- jointParameters HingeJointParameters {
- position 8.128370060178053e-07
- axis 0 1 0
- }
- device [
- DEF LEFT_MOTOR RotationalMotor {
- name "left_motor"
- }
- PositionSensor {
- }
- ]
- endPoint DEF WHELL_LEFT Solid {
- translation 0 0.16 0
- rotation 0.9999999999998349 -4.0641701015308036e-07 4.064185030088579e-07 1.5708
- children [
- DEF WHEEL_SHAPE Shape {
- appearance Appearance {
- material Material {
- diffuseColor 0.1 0.1 0.1
+ ]
+ endPoint Solid {
+ translation -0.310000 0.000000 0.000000
+ children [
+ Shape {
+ appearance PBRAppearance {
+ baseColor 0.500000 0.500000 0.500000
+ roughness 1.000000
+ metalness 0
+ }
+ geometry DEF caster_holder Mesh {
+ url "/spesbot/ros2_ws/install/spesbot_description/share/spesbot_description/urdf/meshes/caster_holder.stl"
}
}
- geometry Cylinder {
- height 0.03
- radius 0.085
+ HingeJoint {
+ jointParameters HingeJointParameters {
+ axis 0.000000 1.000000 0.000000
+ anchor -0.030000 0.000000 -0.050000
+ }
+ device [
+ RotationalMotor {
+ name "caster_wheel_joint"
+ maxTorque 10000
+ }
+ PositionSensor {
+ name "caster_wheel_joint_sensor"
+ }
+ ]
+ endPoint Solid {
+ translation -0.030000 0.000000 -0.050000
+ children [
+ Shape {
+ appearance PBRAppearance {
+ baseColor 0.500000 0.500000 0.500000
+ roughness 1.000000
+ metalness 0
+ }
+ geometry DEF caster_wheel Mesh {
+ url "/spesbot/ros2_ws/install/spesbot_description/share/spesbot_description/urdf/meshes/caster_wheel.stl"
+ }
+ }
+ ]
+ name "caster_wheel_link"
+ boundingObject Pose {
+ rotation 1.000000 0.000000 0.000000 1.570700
+ children [
+ Cylinder {
+ radius 0.033
+ height 0.03
+ }
+ ]
+ }
+ physics Physics {
+ density -1
+ mass 2.000000
+ centerOfMass [ 0.000000 0.000000 0.000000 ]
+ inertiaMatrix [
+ 1.000000e-04 1.000000e-04 1.000000e-04
+ 0.000000e+00 0.000000e+00 0.000000e+00
+ ]
+ }
+ }
}
+ ]
+ name "caster_holder_link"
+ boundingObject Cylinder {
+ radius 0.03
+ height 0.01
+ }
+ physics Physics {
+ density -1
+ mass 0.300000
+ centerOfMass [ 0.000000 0.000000 0.000000 ]
+ inertiaMatrix [
+ 1.000000e-04 1.000000e-04 1.000000e-04
+ 0.000000e+00 0.000000e+00 0.000000e+00
+ ]
}
- ]
- boundingObject USE WHEEL_SHAPE
- physics Physics {
}
}
- }
- DEF RIGHT_JOINT HingeJoint {
- jointParameters HingeJointParameters {
- position 8.112865025542439e-07
- axis 0 1 0
+ Group {
+ children IS toolSlot
}
- device [
- PositionSensor {
- }
- DEF RIGHT_MOTOR RotationalMotor {
- name "right_motor"
+ ]
+ name IS name
+ boundingObject Pose {
+ translation -0.150000 0.000000 0.000000
+ children [
+ Box {
+ size 0.400000 0.300000 0.020000
}
]
- endPoint DEF WHELL_RIGHT Solid {
- translation 0 -0.16 0
- rotation 0.9999999999998354 -4.056011991208921e-07 4.056432512770775e-07 1.5709
- children [
- USE WHEEL_SHAPE
- ]
- name "right_motor"
- boundingObject USE WHEEL_SHAPE
- physics Physics {
- }
- }
}
- ]
- name "spesbot"
- boundingObject USE BASE_BODY
- physics Physics {
+ physics Physics {
+ density -1
+ mass 5.000000
+ centerOfMass [ 0.000000 0.000000 0.000000 ]
+ inertiaMatrix [
+ 1.000000e-01 1.000000e-01 1.000000e-01
+ 0.000000e+00 0.000000e+00 0.000000e+00
+ ]
+ }
}
- controller ""
- supervisor TRUE
-}
}
diff --git a/spesbot_webots/data/worlds/.sample.wbproj b/spesbot_webots/data/worlds/.sample.wbproj
index 5baab60..9eef1c7 100644
--- a/spesbot_webots/data/worlds/.sample.wbproj
+++ b/spesbot_webots/data/worlds/.sample.wbproj
@@ -1,5 +1,5 @@
Webots Project File version R2023b
-perspectives: 000000ff00000000fd00000002000000010000011c00000267fc0200000001fb0000001400540065007800740045006400690074006f00720100000013000002670000003f00ffffff0000000300000640000000d9fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000006400000006900ffffff000005220000026700000001000000020000000100000008fc00000000
+perspectives: 000000ff00000000fd00000002000000010000011c000002f4fc0200000001fb0000001400540065007800740045006400690074006f00720100000013000002f40000003f00ffffff0000000300000780000000d9fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000662000002f400000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000101000005610100000002010000000100
sceneTreePerspectives: 000000ff00000001000000030000001c0000022a000000fa0100000002010000000200
maximizedDockId: -1
diff --git a/spesbot_webots/data/worlds/sample.wbt b/spesbot_webots/data/worlds/sample.wbt
index 2cf8bcf..8c17cf6 100644
--- a/spesbot_webots/data/worlds/sample.wbt
+++ b/spesbot_webots/data/worlds/sample.wbt
@@ -6,6 +6,7 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/project
EXTERNPROTO "../protos/SpesBot.proto"
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/drinks/protos/Can.proto"
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/kitchen/utensils/protos/Glass.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/devices/robotis/protos/RobotisLds01.proto"
WorldInfo {
basicTimeStep 20
@@ -45,6 +46,22 @@ Can {
}
SpesBot {
translation 0 0 0.09
+ controller ""
+ name "spesbot"
+ toolSlot [
+ Camera {
+ translation 0.01 0 0.11
+ rotation 0 1 0 0.4017996938995747
+ locked TRUE
+ width 1280
+ height 720
+ far 0.6
+ }
+ RobotisLds01 {
+ translation -0.05 0 0.2
+ rotation 0 0 1 0
+ }
+ ]
}
DEF GLASS Solid {
translation 0.37 0 0