Add some system configuration (#189)

* Add lovelace configuration

* Add cyclonedds configuration

* Fixed filepath to configure_lovelace.sh

* Update README.md

* Final cleaup

* Formatting

---------

Co-authored-by: Taylor Kessler Faulkner <[email protected]>

Author: amalnanavati

README.md

@@ -47,12 +47,20 @@ This README is the definitive source for downloading, installing, and running th
     - [`pyrealsense2` is not released for ARM systems](https://github.com/IntelRealSense/librealsense/issues/6449#issuecomment-650784066), so ARM users will have to [build from source](https://github.com/IntelRealSense/librealsense/blob/master/wrappers/python/readme.md#building-from-source). You may have to add the `-DPYTHON_EXECUTABLE=/usr/bin/python3` flag to the `cmake` command. When running `sudo make install`, pay close attention to which path `pyrealsense2` is installed to and add *that path* to the `PYTHONPATH` -- it should be `/use/local/lib` but may be `/usr/local/OFF`.
    
 7. Install the JACO SDK (real robot only). All SDKs are listed [here](https://www.kinovarobotics.com/resources?r=79301&s); PRL currently uses the [Gen2 SDK v1.5.1](https://drive.google.com/file/d/1UEQAow0XLcVcPCeQfHK9ERBihOCclkJ9/view). Note that although the latest version of that SDK is for Ubuntu 16.04, it still works on Ubuntu 22.04 (only for x86 systems, not ARM system).
-8. (Optional): We recommend using [FastRTPS](https://docs.ros.org/en/humble/Installation/DDS-Implementations/Working-with-eProsima-Fast-DDS.html) as ROS middleware, as [CycloneDDS unnecessarily sends packets over the network](https://github.com/ros2/rmw_cyclonedds/issues/489). Install it with `sudo apt install ros-humble-rmw-fastrtps-cpp`. Then, add the following line to your `~/bashrc`: `export RMW_IMPLEMENTATION=rmw_fastrtps_cpp`. Note that you have to [rebuild your workspace from scratch after doing this](https://docs.ros.org/en/humble/How-To-Guides/Working-with-multiple-RMW-implementations.html#adding-rmw-implementations-to-your-workspace).
-8. Build your workspace:
+8. (Optional): We recommend using CycloneDDS as your ROS2 midleware, with a custom configuration file that enables multicast on loopback and prioritizes loopback. Install it with `sudo apt install ros-humble-rmw-cyclonedds-cpp`. Then, add the following lines to your `~/bashrc`: `export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp; export CYCLONEDDS_URI=~/colcon_ws/src/ada_feeding/cyclonedds.xml`.
+    - Note that you may have to change the name of the fallback interface that CycloneDDS uses if `lo` does not work for that transmission. To do so, run `ifconfig` and either use the name of your ethernet network or WiFi network, depending on how your computer is connected to the internet.
+    - Note that you have to [rebuild your workspace from scratch after doing this](https://docs.ros.org/en/humble/How-To-Guides/Working-with-multiple-RMW-implementations.html#adding-rmw-implementations-to-your-workspace).
+9. Build your workspace:
 
         cd ~/colcon_ws
         colcon build --symlink-install # if sim-only, add '--packages-skip ada_hardware'
 
+### Setup (System Configuration)
+
+Our system includes two computers, `lovelace` (Lenovo Legion 5i) and `nano` (Nvidia Jetson Nano), where the former runs camera code and the latter runs everything else. This repository includes some system-specific bash scripts to configure those computers. Specifically, move `start_nano.sh`, `start_nano.py`, and `run_camera.sh` to the home directory of `nano`. On `lovelace`, add a cronjob to run `configure_lovelace.sh` on reboot (e.g., `sudo crontab -e`, then add the line `@reboot <path/to/ada_feeding>/configure_lovelace.sh`).
+
+Note that these scripts will likely need to be changed for your setup's computers; however, perhaps the commands in these scripts can provide you inspiration.
+
 ### Setup (Robot Hardware)
 
 If you change the e-stop button, the e-stop button's adapter(s), and/or the device this code is being run on, you may have to change several device-specific parameters for the e-stop to work. Read the long comment above `amixer_configuration_name` in `ada_feeding/config/ada_watchdog.yaml` for detailed instructions on changing these parameters.

ada_feeding/ada_feeding/watchdog/estop_condition.py

@@ -770,6 +770,10 @@ def __audio_callback(
 
         # Convert the data to a numpy array
         self.curr_data_arr = np.frombuffer(data, dtype=np.int16)
+        self._node.get_logger().debug(
+            f"Data min: {np.min(self.curr_data_arr)}, max: {np.max(self.curr_data_arr)}, "
+            f"mean: {np.mean(self.curr_data_arr)}, median: {np.median(self.curr_data_arr)}"
+        )
 
         # Check if the e-stop button has been pressed
         if EStopCondition.rising_edge_detector(

ada_feeding/config/ada_watchdog.yaml

@@ -119,7 +119,7 @@ ada_watchdog:
       amixer_mic_control_names:
         - "'Mic',0"
       amixer_mic_control_percentages:
-        - 100
+        - 40
       is_usb: true
       udev_id: 'usb-Plugable_Plugable_USB_Audio_Device_000000000000-00'
       device_name: 'Plugable USB Audio Device'

ada_feeding/scripts/create_action_servers.py

@@ -6,6 +6,7 @@
 
 # Standard imports
 import collections.abc
+import multiprocessing
 import os
 import threading
 import traceback
@@ -932,7 +933,7 @@ def main(args: List = None) -> None:
     create_action_servers = CreateActionServers()
 
     # Use a MultiThreadedExecutor to enable processing goals concurrently
-    executor = MultiThreadedExecutor()
+    executor = MultiThreadedExecutor(num_threads=multiprocessing.cpu_count() * 2)
 
     # pylint: disable=broad-exception-caught
     # All exceptions need printing at shutdown

ada_feeding_perception/ada_feeding_perception/segment_from_point.py

@@ -63,6 +63,7 @@ def __init__(self) -> None:
 
         # Check if cuda is available
         self.device = "cuda" if torch.cuda.is_available() else "cpu"
+        self.get_logger().info(f"Using device {self.device}")
 
         # Read the parameters
         # NOTE: These parameters are only read once. Any changes after the node

ada_planning_scene/config/ada_planning_scene.yaml

@@ -23,7 +23,7 @@ ada_planning_scene:
     namespaces:
       - seated
       - bedside
-    namespace_to_use: bedside # seated
+    namespace_to_use: seated # bedside # 
 
     ############################################################################
     # Parameters related to the PlanningSceneInitializer class

configure_lovelace.sh

@@ -0,0 +1,5 @@
+#!/bin/sh
+# export LOCAL_USER=ada
+ifconfig lo multicast
+route add -net 224.0.0.0 netmask 240.0.0.0 dev lo
+setfacl -m u:ada:rw /dev/snd/*

cyclonedds.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="utf-8"?>
+<CycloneDDS
+  xmlns="https://cdds.io/config"
+  xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+  xsi:schemaLocation="https://cdds.io/config https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd"
+>
+  <Domain Id="any">
+    <General>
+      <Interfaces>
+        <NetworkInterface name="lo" priority="default" multicast="true"/>
+        <NetworkInterface name="wlp0s20f3" priority="default" multicast="true"/>
+      </Interfaces>
+    </General>
+  </Domain>
+
+</CycloneDDS>

run_camera.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+source ~/ros2_humble/install/local_setup.bash
+source ~/Workspace/camera_ros2/install/setup.bash
+export ROS_DOMAIN_ID=42
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+ros2 launch realsense2_camera rs_launch.py rgb_camera.profile:=640,480,15 depth_module.profile:=640,480,15 align_depth.enable:=true initial_reset:=true publish_tf:=false
+
+

start.py

@@ -269,6 +269,7 @@ async def main(args: argparse.Namespace, pwd: str) -> None:
                 f"ros2 launch ada_planning_scene ada_moveit_launch.xml use_rviz:={'true' if args.dev else 'false'}",
             ],
             "feeding": [
+                "sudo ./src/ada_feeding/configure_lovelace.sh",
                 (
                     "ros2 launch ada_feeding ada_feeding_launch.xml "
                     f"use_estop:={'false' if args.dev else 'true'} run_web_bridge:=false"