Lidar connection 624

.gitignore

@@ -49,4 +49,9 @@ compile_commands.json
 Power-Report*.csv
 
 in-house-deps/
-third-party-deps/
+third-party-deps/
+
+
+
+# Added by Marc
+space-env/

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "robot/rospackages/src/zed-ros2-wrapper"]
+	path = robot/rospackages/src/zed-ros2-wrapper
+	url = https://github.com/stereolabs/zed-ros2-wrapper.git
+[submodule "robot/rospackages/src/ouster-ros"]
+	path = robot/rospackages/src/ouster-ros
+	url = https://github.com/ouster-lidar/ouster-ros.git

README.md

@@ -1,7 +1,7 @@
 ### Setup
 **TL,DR**: Run the venv and compilation setups. If you want to run arm
 and wheels controls, or use the Aruco node, there are additional steps.
-Then compile with `colcon build --symlink-install --packages-skip usb_cam`.
+Then compile with `colcon build --symlink-install --packages-skip usb_cam LidarSlam --cmake-args -DCMAKE_BUILD_TYPE=Release`.
 Now, you can run a launch file. For instance, to run the simulation of
 IK, run `ros2 launch arm_ik local_ik.launch.py`. **NOTE**: VS Code's
 integrated terminal causes issues with RVIZ, so I recommend you use
@@ -15,19 +15,63 @@ to add it to your bashrc: `echo "source ${PWD}/space-env/bin/activate" >> ~/.bas
 
 #### Setup to compile repo
 The code in this repo was built around ROS Humble. First [install that](https://docs.ros.org/en/humble/Installation.html).
-Then, from this folder (`robotics-prototype`):
-- Install rosdep, colcon, catkin, and pip (if not already) (`sudo apt install python3-colcon-common-extensions python3-pip python3-rosdep2`)
-- Init rosdep (`sudo rosdep init`). An error of the form `ERROR: default sources list file already exists:` is
-expected, if you've already installed rosdep.
-- Go in the rospackages folder: `cd robot/rospackages`
-- Update rosdep and install `rosdep update && rosdep install --from-paths src --ignore-src -r -y`. Enter your password when prompted.
-- Run rosdep so it installs packages:- Install misc python deps (`pip install -r ../../requirements.txt`)
-- Install JetsonGPIO [from GitHub](https://github.com/pjueon/JetsonGPIO/blob/master/docs/installation_guide.md). This must be 
-manually installed. The default options will work.
-- Build: `colcon build --symlink-install --packages-skip usb_cam` (usb_cam can only compile on the Jetson).
-- Source this ros2 package by adding a command to your bashrc. You can do that by running this command:
-`echo "source ${PWD}/install/local_setup.bash" >> ~/.bashrc`
-- **Restart your terminal** so the above command runs and you can have access to the ros2 workspace.
+Then, from this folder:
+- I recommend you setup a [Python venv](https://docs.python.org/3/library/venv.html). See steps below.
+- Install rosdep, colcon, and pip (if not already) (`sudo apt install python3-colcon-common-extensions python3-pip python3-rosdep`)
+- Init and update rosdep (`sudo rosdep init && rosdep update`)
+- Run rosdep so it installs packages: `rosdep install --from-paths robot/rospackages/src --ignore-src -r -y`. Enter your password when prompted.
+Run this **from the robotics-prototype folder**
+- Install misc python deps (`pip install -r requirements.txt`)
+- Install JetsonGPIO [from GitHub](https://github.com/pjueon/JetsonGPIO/blob/master/docs/installation_guide.md). This must be manually installed. The default options will work.
+- `cd robot/rospackages`
+- If you want to use ZED2 stuff, [install CUDA](https://developer.nvidia.com/cuda-downloads) and [install the zed sdk](https://www.stereolabs.com/en-ca/developers/release). Pick the
+`ZED SDK for JetPack 6.0 GA (L4T 36.3) 4.1 (Jetson Orin, CUDA 12.2)` package.
+- Build. On the Jetson use this command: `colcon build --symlink-install --packages-skip usb_cam LidarSlam --cmake-args -DCMAKE_BUILD_TYPE=Release` (usb_cam can only compile on the Jetson, LidarSlam needs to be built separately). Run this **from the rospackages folder**.
+If you are on your laptop, you'll need to add more packages to `--packages-skip`, such as `zed-ros2-wrapper`, and 
+`outer-ros`
+
+#### Setup venv
+Run `python3 -m venv ./space-env` from the robotics-prototype folder. Then source it to activate it: `source space-env/bin/activate`. NOTE: it may be necessary to install a dependency: `sudo apt install python3.10-venv`.
+
+So you will use this frequently, add the source to your ~/.bashrc: `echo "source ${PWD}/space-env/bin/activate" >> ~/.bashrc`.
+That way it will run automatically.
+
+#### Compile LidarSlam
+Install nanoflann: `sudo apt-get install -y libnanoflann-dev`. Then, compile:
+`colcon build --base-paths src/slam/ros2_wrapping --cmake-args -DCMAKE_BUILD_TYPE=Release`
+NOTE: do this from the `robot/rospackages` folder.
+
+#### Setup the lidar
+Install additional packages:
+```
+sudo apt install -y             \
+    ros-$ROS_DISTRO-pcl-ros     \
+    ros-$ROS_DISTRO-tf2-eigen   \
+    ros-$ROS_DISTRO-rviz2
+```
+```
+sudo apt install -y         \
+    build-essential         \
+    libeigen3-dev           \
+    libjsoncpp-dev          \
+    libspdlog-dev           \
+    libcurl4-openssl-dev    \
+    cmake                   \
+    python3-colcon-common-extensions
+```
+Then, the lidar package will compile.
+
+
+Make sure that the ethernet interface the Lidar is set to is set to `link-local only`. This can be done within the 
+ubunutu GUI. The [Networking guide](https://static.ouster.dev/sensor-docs/image_route1/image_route2/networking_guide/networking_guide.html)
+can help, but this should be be only configuration that's necessary. The network config scripts **should**
+not be necessary anymore. To test if it's on the network, run the following command:
+`ping -c1 os1-992005000098.local`. Then, to *test* if it launches, run this command **in robot/rospackages**:
+`ros2 launch ouster_ros driver.launch.py  params_file:='src/beep_autonomy/config/ouster_driver_params.yaml'`
+To configure the network from a terminal (without GUI), use the `sudo nmtui` command.
+
+### Running Autonomy
+Run `ros2 launch beep_autonomy video.launch.py`
 
 ### Running arm and wheels controls
 First, run `sudo ./scripts/configure-can0.sh` and `sudo ./scripts/configure-arm.sh`. The 

os1-992005000098-metadata.json

@@ -0,0 +1,174 @@
+{
+    "beam_intrinsics": 
+    {
+        "beam_altitude_angles": 
+        [
+            16.37,
+            14.2,
+            12.06,
+            9.96,
+            7.87,
+            5.78,
+            3.7,
+            1.63,
+            -0.45,
+            -2.53,
+            -4.61,
+            -6.7,
+            -8.79,
+            -10.89,
+            -13.01,
+            -15.16
+        ],
+        "beam_azimuth_angles": 
+        [
+            0.93,
+            0.94,
+            0.94,
+            0.96,
+            0.97,
+            0.98,
+            1,
+            1.02,
+            1.04,
+            1.06,
+            1.08,
+            1.1,
+            1.12,
+            1.15,
+            1.2,
+            1.22
+        ],
+        "lidar_origin_to_beam_origin_mm": 12.163
+    },
+    "calibration_status": 
+    {
+        "reflectivity": 
+        {
+            "timestamp": "",
+            "valid": false
+        }
+    },
+    "config_params": 
+    {
+        "azimuth_window": 
+        [
+            0,
+            360000
+        ],
+        "columns_per_packet": 16,
+        "lidar_mode": "1024x10",
+        "multipurpose_io_mode": "OFF",
+        "nmea_baud_rate": "BAUD_9600",
+        "nmea_ignore_valid_char": 0,
+        "nmea_in_polarity": "ACTIVE_HIGH",
+        "nmea_leap_seconds": 0,
+        "operating_mode": "NORMAL",
+        "phase_lock_enable": false,
+        "phase_lock_offset": 0,
+        "signal_multiplier": 1,
+        "sync_pulse_in_polarity": "ACTIVE_HIGH",
+        "sync_pulse_out_angle": 360,
+        "sync_pulse_out_frequency": 1,
+        "sync_pulse_out_polarity": "ACTIVE_HIGH",
+        "sync_pulse_out_pulse_width": 10,
+        "timestamp_mode": "TIME_FROM_INTERNAL_OSC",
+        "udp_dest": "169.254.15.172",
+        "udp_port_imu": 41144,
+        "udp_port_lidar": 55406,
+        "udp_profile_imu": "LEGACY",
+        "udp_profile_lidar": "LEGACY"
+    },
+    "imu_intrinsics": 
+    {
+        "imu_to_sensor_transform": 
+        [
+            1,
+            0,
+            0,
+            6.253,
+            0,
+            1,
+            0,
+            -11.775,
+            0,
+            0,
+            1,
+            7.645,
+            0,
+            0,
+            0,
+            1
+        ]
+    },
+    "lidar_data_format": 
+    {
+        "column_window": 
+        [
+            0,
+            1023
+        ],
+        "columns_per_frame": 1024,
+        "columns_per_packet": 16,
+        "pixel_shift_by_row": 
+        [
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,
+            3
+        ],
+        "pixels_per_column": 16,
+        "udp_profile_imu": "LEGACY",
+        "udp_profile_lidar": "LEGACY"
+    },
+    "lidar_intrinsics": 
+    {
+        "lidar_to_sensor_transform": 
+        [
+            -1,
+            0,
+            0,
+            0,
+            0,
+            -1,
+            0,
+            0,
+            0,
+            0,
+            1,
+            36.18,
+            0,
+            0,
+            0,
+            1
+        ]
+    },
+    "ouster-sdk": 
+    {
+        "client_version": "ouster_client 0.11.1",
+        "output_source": "collect_metadata"
+    },
+    "sensor_info": 
+    {
+        "build_date": "2022-09-21T17:47:45Z",
+        "build_rev": "v2.4.0",
+        "image_rev": "ousteros-image-prod-aries-v2.4.0+20220921174636",
+        "initialization_id": 7109748,
+        "prod_line": "OS-1-16-A2",
+        "prod_pn": "840-101855-02",
+        "prod_sn": "992005000098",
+        "status": "RUNNING"
+    }
+}

requirements.txt

@@ -6,3 +6,6 @@ twisted==22.10.0
 Jetson.GPIO
 lark==1.1.5
 pytest==7.2.0
+empy==3.3.4
+numpy
+lxml