use rosdep to install ros2 dependencies (#12)

also, update readme accordingly

.gitignore

@@ -3,3 +3,4 @@ test*
 webots_spot/__pycache__*
 worlds/.*.wbproj
 .vscode
+.trunk

README.md

@@ -7,72 +7,62 @@ The world contains apriltags, a red line to test lane follower and objects for m
 
 ## Prerequisites
 
-    - Tested for ubuntu 22.04
-    - ROS 2 Humble
-    - Webots R2023b
-    - Webots ROS 2 interface
+    - Ubuntu 22.04
+    - ROS2 Humble https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html
+    - Webots 2023b https://github.com/cyberbotics/webots/releases/tag/R2023b
 
 ## Install
 
-### Install dependencies
-
-    sudo apt install ros-$ROS_DISTRO-nav* -y
-    sudo apt install ros-$ROS_DISTRO-pointcloud-to-laserscan -y
-    sudo apt install ros-$ROS_DISTRO-moveit* -y
-    sudo apt install ros-$ROS_DISTRO-vision-msgs* -y
-    sudo apt install python3-open3d -y # for Ubuntu 22.04
-    pip3 install scipy pupil-apriltags
-    pip3 install open3d # for Ubuntu 20.04
-
-### webots_ros2_spot
-
-    # Source ROS2
-    . /opt/ros/$ROS_DISTRO/setup.bash
-
-    mkdir ~/ros2_ws
-    cd ~/ros2_ws
-
-    # webots_ros2
-    git clone https://github.com/cyberbotics/webots_ros2 -b 2023.1.1 --recursive src/webots_ros2
-
-    # webots_spot
-    git clone https://github.com/MASKOR/webots_ros2_spot src/webots_spot
-
-    # webots_spot_msgs
-    git clone https://github.com/MASKOR/webots_spot_msgs src/webots_spot_msgs
-
-    # webots_spot_teleop (optional)
-    git clone https://github.com/MASKOR/webots_spot_teleop src/webots_spot_teleop
-
-    # Build everything
+1. Make sure that `colcon`, its extensions, and `vcs` are installed:
+    ```
+    sudo apt install python3-colcon-common-extensions python3-vcstool
+    ```
+
+    Also, rosdep is installed https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html#rosdep-installation.
+
+2. Create a new ROS2 workspace:
+    ```
+    export COLCON_WS=~/ros2_ws
+    mkdir -p $COLCON_WS/src
+    ```
+
+3. Pull relevant packages, install dependencies, compile, and source the workspace by using:
+    ```
+    cd $COLCON_WS
+    git clone https://github.com/MASKOR/webots_ros2_spot src/webots_ros2_spot
+    vcs import src --skip-existing --input src/webots_ros2_spot/webots_ros2_spot.repos
+    rosdep install --ignore-src --from-paths src -y -r
+    ```
+
+4. Build packages and source the workspace
+    ```
     colcon build --symlink-install
-    . install/setup.bash
-
-    # With '--symlink-install', following python file isn't marked as an executable, do it manually
-    chmod +x ~/ros2_ws/install/webots_ros2_driver/lib/webots_ros2_driver/ros2_supervisor.py
+    source install/setup.bash
+    ```
 
 ## Start
 Starting the simulation:
-
-    ros2 launch webots_spot spot_launch.py
-
-Starting MoveIt:
-
-    ros2 launch webots_spot moveit_launch.py
+```
+ros2 launch webots_spot spot_launch.py
+```
 
 To launch navigation with Rviz2:
-
-    ros2 launch webots_spot nav_launch.py set_initial_pose:=true
+```
+ros2 launch webots_spot nav_launch.py set_initial_pose:=true
+```
 
 To launch mapping with Slamtoolbox:
+```
+ros2 launch webots_spot slam_launch.py
+```
 
-    ros2 launch webots_spot slam_launch.py
-
-To launch mapping with RTABMAP: #https://github.com/introlab/rtabmap_ros
-
-    ros2 launch webots_spot rtabmap_launch.py
+Starting MoveIt:
+```
+ros2 launch webots_spot moveit_launch.py
+```
 
 Teleop keyboard:
-
-    ros2 run teleop_twist_keyboard teleop_twist_keyboard
-    # OR ros2 run spot_teleop spot_teleop_keyboard for body_pose control as well
+```
+ros2 run teleop_twist_keyboard teleop_twist_keyboard
+# OR ros2 run spot_teleop spot_teleop_keyboard for body_pose control as well
+```

package.xml

@@ -7,19 +7,40 @@
   <maintainer email="maximillian.kirsch@alumni.fh-aachen.de">max</maintainer>
   <license>TODO: License declaration</license>
 
-  <depend>rclpy</depend>
-  <depend>geometry_msgs</depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
 
   <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>webots_spot_msgs</exec_depend>
   <exec_depend>rviz2</exec_depend>
-
-  <exec_depend>webots_ros2_driver</exec_depend>
-  <exec_depend>webots_ros2_control</exec_depend>
+  <exec_depend>rviz_common</exec_depend>
+  <exec_depend>rviz_default_plugins</exec_depend>
+
+  <exec_depend>webots_ros2</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>pointcloud_to_laserscan</exec_depend>
+
+  <exec_depend>navigation2</exec_depend>
+  <exec_depend>nav2_bringup</exec_depend>
+
+  <exec_depend>moveit</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_kinematics</exec_depend>
+  <exec_depend>moveit_planners</exec_depend>
+  <exec_depend>moveit_simple_controller_manager</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
+  <exec_depend>moveit_ros_warehouse</exec_depend>
+  <exec_depend>moveit_setup_assistant</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

webots_ros2_spot.repos

@@ -0,0 +1,9 @@
+repositories:
+  webots_spot_msgs:
+    type: git
+    url: https://github.com/MASKOR/webots_spot_msgs.git
+    version: main
+  webots_spot_teleop:
+    type: git
+    url: https://github.com/MASKOR/webots_spot_teleop.git
+    version: main

webots_spot/spot_driver.py

@@ -10,7 +10,6 @@
 from tf2_ros.transform_broadcaster import TransformBroadcaster
 from std_srvs.srv import Empty
 
-from scipy.spatial.transform import Rotation as R
 import numpy as np
 import copy
 
@@ -104,12 +103,69 @@ def set_rod(robot, red=False):
         )
 
 
-def quat_from_aa(aa):
-    return R.from_rotvec(aa[3] * np.array(aa[:3])).as_quat()
+def quaternion_to_euler(q):
+    x, y, z, w = q
+
+    roll = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
+    pitch = np.arcsin(2 * (w * y - z * x))
+    yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
+
+    return roll, pitch, yaw
+
+
+def quaternion_from_euler(a):
+    ai, aj, ak = a
+    ai /= 2.0
+    aj /= 2.0
+    ak /= 2.0
+    ci = np.cos(ai)
+    si = np.sin(ai)
+    cj = np.cos(aj)
+    sj = np.sin(aj)
+    ck = np.cos(ak)
+    sk = np.sin(ak)
+    cc = ci * ck
+    cs = ci * sk
+    sc = si * ck
+    ss = si * sk
+
+    q = np.empty((4,))
+    q[0] = cj * sc - sj * cs
+    q[1] = cj * ss + sj * cc
+    q[2] = cj * cs - sj * sc
+    q[3] = cj * cc + sj * ss
+
+    return q
+
+
+def quat_from_angle_axis(aa):
+    angle = aa[3]
+    axis = np.array(aa[:3])
+    norm_axis = axis / np.linalg.norm(axis)
+    half_angle = 0.5 * angle
+    sin_half_angle = np.sin(half_angle)
+
+    x = sin_half_angle * norm_axis[0]
+    y = sin_half_angle * norm_axis[1]
+    z = sin_half_angle * norm_axis[2]
+    w = np.cos(half_angle)
+
+    return [x, y, z, w]
 
 
 def diff_quat(q2, q1):
-    return (R.from_quat(q2) * R.from_quat(q1).inv()).as_quat()
+    # Calculate the product of q2 and the inverse of q1
+    q1_inv = [-q1[0], -q1[1], -q1[2], q1[3]]
+
+    x1, y1, z1, w1 = q1_inv
+    x2, y2, z2, w2 = q2
+
+    x = x2 * w1 + y2 * z1 - z2 * y1 + w2 * x1
+    y = -x2 * z1 + y2 * w1 + z2 * x1 + w2 * y1
+    z = x2 * y1 - y2 * x1 + z2 * w1 + w2 * z1
+    w = w2 * w1 - x2 * x1 - y2 * y1 - z2 * z1
+
+    return [x, y, z, w]
 
 
 def shuffle_cubes(robot):
@@ -521,8 +577,8 @@ def handle_transforms_and_odometry(self):
             tf.transform.translation.z += HEIGHT + 0.095  # BASE_LINK To Ground at Rest
 
             r = diff_quat(
-                quat_from_aa(part.getField("rotation").getSFRotation()),
-                quat_from_aa(self.spot_rotation_initial),
+                quat_from_angle_axis(part.getField("rotation").getSFRotation()),
+                quat_from_angle_axis(self.spot_rotation_initial),
             )
             tf.transform.rotation.x = -r[0]
             tf.transform.rotation.y = -r[1]
@@ -548,15 +604,14 @@ def handle_transforms_and_odometry(self):
             tf_odom_base_link.translation.z,
         ]
 
-        r = R.from_quat(
+        rotation = quaternion_to_euler(
             [
                 tf_odom_base_link.rotation.x,
                 tf_odom_base_link.rotation.y,
                 tf_odom_base_link.rotation.z,
                 tf_odom_base_link.rotation.w,
             ]
         )
-        rotation = [r.as_euler("xyz")[0], r.as_euler("xyz")[1], r.as_euler("xyz")[2]]
 
         if not hasattr(self, "previous_rotation"):
             self.previous_rotation = rotation
@@ -582,11 +637,11 @@ def handle_transforms_and_odometry(self):
             odom.pose.pose.position.x = translation[0]
             odom.pose.pose.position.y = translation[1]
             odom.pose.pose.position.z = translation[2]
-            r = R.from_euler("xyz", [rotation[0], rotation[1], rotation[2]])
-            odom.pose.pose.orientation.x = r.as_quat()[0]
-            odom.pose.pose.orientation.y = r.as_quat()[1]
-            odom.pose.pose.orientation.z = r.as_quat()[2]
-            odom.pose.pose.orientation.w = r.as_quat()[3]
+            quat = quaternion_from_euler([rotation[0], rotation[1], rotation[2]])
+            odom.pose.pose.orientation.x = quat[0]
+            odom.pose.pose.orientation.y = quat[1]
+            odom.pose.pose.orientation.z = quat[2]
+            odom.pose.pose.orientation.w = quat[3]
             odom.twist.twist.linear.x = translation_twist[0]
             odom.twist.twist.linear.y = translation_twist[1]
             odom.twist.twist.linear.z = translation_twist[2]