Merge branch 'humble' of github.com:luxonis/rae-ros into spectacular_sdk

.dockerignore

@@ -0,0 +1,3 @@
+.git
+.github
+.devcontainer

Dockerfile

@@ -8,12 +8,33 @@ ARG SPECTACULARAI_ROS_VERSION=v0.0.2
 
 ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update \
-   && apt-get -y install --no-install-recommends software-properties-common git libusb-1.0-0-dev wget zsh python3-colcon-common-extensions python3-rosdep build-essential neovim tmux htop net-tools iputils-ping gpiod gstreamer1.0-plugins-bad gstreamer1.0-alsa libasound2-dev busybox
+   && apt-get -y install --no-install-recommends \
+    software-properties-common \
+    git \
+    nano \
+    libusb-1.0-0-dev \
+    wget \
+    zsh \
+    python3-colcon-common-extensions \
+    python3-rosdep \
+    build-essential \
+    neovim \
+    tmux \
+    htop \
+    net-tools \
+    iputils-ping \
+    gpiod \
+    gstreamer1.0-plugins-bad \
+    gstreamer1.0-alsa \
+    libasound2-dev \
+    busybox
 
 ENV WS=/ws
 RUN mkdir -p $WS/src
 COPY ./ .$WS/src/rae-ros
 
+RUN cp -R .$WS/src/rae-ros/assets/. /usr/share
+RUN rm -rf .$WS/src/rae-ros/assets
 RUN rm -rf .$WS/src/rae-ros/rae_gazebo
 
 RUN cd  .$WS/ && apt update && rosdep update && rosdep install --from-paths src --ignore-src  -y --skip-keys depthai --skip-keys depthai_bridge --skip-keys depthai_ros_driver --skip-keys audio_msgs --skip-keys laserscan_kinect --skip-keys ira_laser_tools

README.md

@@ -1,6 +1,7 @@
 # RAE ROS
 
-This repository contains rae [ROS](https://www.ros.org/) integration files.
+This repository contains rae [ROS](https://www.ros.org/) integration files. 
+**Note** The RAE project is under active development so you can expect changes in the API and improvements in the performance in near future. Please report problems on Github issues as it's important for the development efforts.
 
 ### Setting up procedure
 
@@ -9,7 +10,8 @@ This repository contains rae [ROS](https://www.ros.org/) integration files.
 1. Connect via USB cable or wifi `rae-<ID>`, password `wifiwifi@` (See [rae getting started documentation](https://docs.luxonis.com/projects/hardware/en/latest/pages/rae/#getting-started)).
 2. To use SHH without typing password each time - `ssh-copy-id [email protected]`
 3. Currently date resets after each startup to set current - ssh [email protected] sudo date -s @`( date -u +"%s" )`
-4. If you want to run ROS packages while bypassing RobotHub it would be advised to stop RH agent before starting docker containers, otherwise you can easily run into conflicts as they would be competing for same hardware resources - `robothub-ctl stop`
+4. If you want to run ROS packages while bypassing RobotHub it would be advised to stop RH agent before starting docker containers, otherwise you can easily run into conflicts as they would be competing for same hardware resources - `robothub-ctl stop`. **Keep in mind** that since `wpa_supplicant` is a subproccess of the RH agent, the WiFi connection will get killed along with the agent. To resolve this we recommend you manually setup the WiFi connection as done in [this guide](https://docs-beta.luxonis.com/deploy/connect-device/rae/?v=Advanced+%28manual%29).
+
 
 #### Generating docker image
 
@@ -196,4 +198,5 @@ Microphone node expects audio messages and example of how to use that data (alon
             audio_data = audio_data.reshape((msg.frames, msg.channels))
 ```
 
-Speakers operate similarly, in that they output audio messages. In bringup package in scripts folder sound_test.py offers a decent example of how you can create audio messages. We will shortly create more demos for speakers and microphone. 
+
+Speakers operate similarly, in that they output audio messages. In bringup package in scripts folder sound_test.py offers a decent example of how you can create audio messages. We will shortly create more demos for speakers and microphone. 

entrypoint.sh

@@ -1,9 +1,18 @@
 #!/bin/bash
 set -e
 
-# setup ros environment
-echo 2 > /sys/class/pwm/pwmchip0/export && echo 1 > /sys/class/pwm/pwmchip0/export && chmod -R a+rw /sys/class/pwm/pwmchip0/pwm1/ && chmod -R a+rw /sys/class/pwm/pwmchip0/pwm2/ && chmod -R a+rw /dev/gpiochip0
+# Check and export pwm channels if not already exported
+for channel in 1 2; do
+    pwm="/sys/class/pwm/pwmchip0/pwm${channel}/"
+    if [ ! -d ${pwm} ]; then
+        echo ${channel} > /sys/class/pwm/pwmchip0/export
+    fi
+    chmod -R a+rw ${pwm}
+done
+
+chmod -R a+rw /dev/gpiochip0
+
 source "/opt/ros/$ROS_DISTRO/setup.bash"
 source "/ws/install/setup.bash"
 source "$HOME/.bashrc"
-exec "$@"
+exec "$@"

rae_bringup/CMakeLists.txt

@@ -33,6 +33,7 @@ install(
   scripts/battery_status.py
   scripts/audio_spectrum.py
   scripts/sound_test.py
+  scripts/led_test.py
   DESTINATION lib/${PROJECT_NAME}
 )
 install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})

rae_bringup/launch/slam.launch.py

@@ -78,7 +78,7 @@ def generate_launch_description():
         DeclareLaunchArgument('sim`', default_value='False'),
         DeclareLaunchArgument('use_rviz', default_value='False'),
         DeclareLaunchArgument('rviz_config', default_value=rviz_config),
-        DeclareLaunchArgument('params_file', default_value=os.path.join(bringup_prefix, 'config', 'slam_param.yaml')),
+        DeclareLaunchArgument('slam_params_file', default_value=os.path.join(bringup_prefix, 'config', 'slam_param.yaml')),
     ]
 
     return LaunchDescription(

rae_bringup/scripts/battery_status.py

@@ -1,7 +1,8 @@
 #!/usr/bin/env python3
 import rclpy
+import signal
+import sys
 from rclpy.node import Node
-from rclpy.time import Time
 
 from sensor_msgs.msg import BatteryState, Image
 from std_msgs.msg import ColorRGBA
@@ -95,7 +96,7 @@ def listener_callback(self, msg):
         led_msg = LEDControl()
         led_msg.header.stamp = self.get_clock().now().to_msg()
         led_msg.data = [ColorRGBA(r=0.0, g=0.0, b=0.0, a=0.0)]*40
-        for i in range(40):
+        for i in range(39):
             led_msg.single_led_n = 0
             led_msg.control_type = 2  # assuming 0 means "set color"
 
@@ -106,17 +107,21 @@ def listener_callback(self, msg):
         self.publisher_led.publish(led_msg)
 
 
+def signal_handler(node):
+    def handle(sig, frame):
+        if rclpy.ok():
+            node.destroy_node()
+            rclpy.shutdown()
+        sys.exit(0)
+    return handle
+
+
 def main(args=None):
     rclpy.init(args=args)
-
     battery_status_node = BatteryStatusNode()
-
+    signal.signal(signal.SIGINT, signal_handler(battery_status_node))
     rclpy.spin(battery_status_node)
 
-    # Destroy the node explicitly
-    battery_status_node.destroy_node()
-    rclpy.shutdown()
-
 
 if __name__ == '__main__':
-    main()
+    main()

rae_bringup/scripts/led_test.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from rclpy.time import Time
+
+from std_msgs.msg import ColorRGBA
+from rae_msgs.msg import LEDControl
+from cv_bridge import CvBridge
+from geometry_msgs.msg import Twist
+
+blinking = True
+
+class CarDemoNode(Node):
+
+    def __init__(self):
+        super().__init__('car_demo_node')
+        self.subscription = self.create_subscription(
+            Twist,
+            'cmd_vel',
+            self.listener_callback,
+            10)
+        self.publisher_led = self.create_publisher(LEDControl, 'leds', 10)
+
+        self.bridge = CvBridge()
+
+    def listener_callback(self, msg):
+        global blinking
+        linear_speed = msg.linear.x
+        angular_speed = msg.angular.z
+
+
+        # Set LEDs based on battery level
+        # Define colors for LEDs
+        colors = {
+            "white": ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0),
+            "yellow": ColorRGBA(r=1.0, g=1.0, b=0.0, a=1.0),
+            "red": ColorRGBA(r=1.0, g=0.0, b=0.0, a=1.0),
+            "blue": ColorRGBA(r=0.0, g=0.0, b=1.0, a=1.0)
+        }
+
+
+
+        # Create and publish LEDControl message for each LED
+        led_msg = LEDControl()
+        led_msg.header.stamp = self.get_clock().now().to_msg()
+        led_msg.data = [ColorRGBA(r=0.0, g=0.0, b=0.0, a=0.0)]*40
+        for i in range(39):
+            led_msg.single_led_n = 0
+            led_msg.control_type = 2 
+            if i < 8:
+                color = "white"
+                led_msg.data[i]=(colors[color])
+            if i >9 and i < 14 and angular_speed > 0.0 and blinking==True:
+                color = "yellow"
+                led_msg.data[i]=(colors[color])
+            if i > 20 and i < 29 and linear_speed < 0.0:
+                color = "red"
+                led_msg.data[i]=(colors[color])
+            if i> 34 and i < 39 and angular_speed < 0.0 and blinking==True:
+                color = "yellow"
+                led_msg.data[i]=(colors[color])
+
+        blinking = not blinking
+        self.publisher_led.publish(led_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    car_demo_node = CarDemoNode()
+
+    rclpy.spin(car_demo_node)
+
+    # Destroy the node explicitly
+    car_demo_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

rae_camera/config/camera.yaml

@@ -11,6 +11,15 @@
       i_nn_config_path: /workspaces/rae_ws/src/rae-ros/rae_camera/config/yolo.json
       i_disable_resize: true
       i_enable_passthrough: true
+    imu:
+      i_batch_report_threshold: 5
+      i_max_batch_reports: 5
+      i_enable_rotation: true
+      i_message_type: IMU_WITH_MAG_SPLIT
+      i_sync_method: COPY
+      i_gyro_freq: 100
+      i_acc_freq: 100
+      i_rot_freq: 100
     rgb:
       i_output_isp: false
       i_set_isp_scale: false

rae_description/launch/rsp.launch.py

@@ -4,40 +4,26 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import Command, LaunchConfiguration
-from launch_ros.actions import LoadComposableNodes, ComposableNodeContainer
+from launch_ros.actions import Node
 from launch_ros.descriptions import ComposableNode
 from launch.conditions import IfCondition
 
 
 def launch_setup(context, *args, **kwargs):
-    name = LaunchConfiguration('name').perform(context)
     use_sim_time = LaunchConfiguration('use_sim_time')
-    run_container = LaunchConfiguration('run_container')
     pkg_path = os.path.join(get_package_share_directory('rae_description'))
     xacro_path = os.path.join(pkg_path, 'urdf', 'rae.urdf.xacro')
 
     return [
-        ComposableNodeContainer(
-            condition=IfCondition(run_container),
-            name=name+"_container",
-            namespace="",
-            package="rclcpp_components",
-            executable="component_container",
-            composable_node_descriptions=[],
-            output="both",
-        ),
-        LoadComposableNodes(
-        target_container=name+"_container",
-        composable_node_descriptions=[
-            ComposableNode(
-                package='robot_state_publisher',
-                plugin='robot_state_publisher::RobotStatePublisher',
-                name=name+'_state_publisher',
-                parameters=[{
-                        'robot_description': Command(['xacro ', xacro_path, ' sim_mode:=', use_sim_time]),
-                        'use_sim_time': use_sim_time
-                }]
-            )])
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            parameters=[{
+                'robot_description': Command(['xacro ', xacro_path, ' sim_mode:=', use_sim_time]),
+                'use_sim_time': use_sim_time
+            }]
+        )
     ]
 
 

rae_description/urdf/rae_ros2_control.urdf.xacro

@@ -15,18 +15,18 @@
       <param name="enLB">43</param>
       <param name="enRA">46</param>
       <param name="enRB">47</param>
-      <param name="encTicsPerRevL">187</param>
-      <param name="encTicsPerRevR">187</param>
+      <param name="encTicsPerRevL">756</param>
+      <param name="encTicsPerRevR">756</param>
       <param name="maxVelL">32</param>
       <param name="maxVelR">32</param>
       <param name="closed_loopL">1</param>
-      <param name="PID_P_L">0.5</param>
-      <param name="PID_I_L">0.0</param>
-      <param name="PID_D_L">0.0</param>
+      <param name="PID_P_L">0.2</param>
+      <param name="PID_I_L">0.1</param>
+      <param name="PID_D_L">0.0005</param>
       <param name="closed_loopR">1</param>
-      <param name="PID_P_R">0.5</param>
-      <param name="PID_I_R">0.0</param>
-      <param name="PID_D_R">0.0</param>
+      <param name="PID_P_R">0.2</param>
+      <param name="PID_I_R">0.1</param>
+      <param name="PID_D_R">0.0005</param>
 
       <param name="loop_rate">30</param>
       <param name="chip_name">gpiochip0</param>

rae_hw/CMakeLists.txt

@@ -58,6 +58,13 @@ install(
   LIBRARY DESTINATION lib
 )
 
+add_executable(mic_node src/mic_node.cpp)
+ament_target_dependencies(mic_node rclcpp audio_msgs)
+target_link_libraries(mic_node ${PROJECT_NAME})
+
+add_executable(speakers_node src/speakers_node.cpp)
+ament_target_dependencies(speakers_node rclcpp audio_msgs)
+target_link_libraries(speakers_node ${PROJECT_NAME})
 
 add_executable(test_motors test/test_motors.cpp src/rae_motors.cpp)
 target_link_libraries(test_motors ${GPIOD_LIBRARY})
@@ -76,7 +83,7 @@ ament_target_dependencies(test_speed rclcpp geometry_msgs)
 
 install(TARGETS
 
-  test_motors test_encoders test_max_speed test_speed
+  test_motors test_encoders test_max_speed test_speed mic_node speakers_node
 
   DESTINATION lib/${PROJECT_NAME})
 

rae_hw/config/controller.yaml

@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 50  # Hz
+    update_rate: 100  # Hz
 
     diff_controller:
       type: diff_drive_controller/DiffDriveController

rae_hw/include/rae_hw/peripherals/lcd.hpp

@@ -2,11 +2,13 @@
 #include <linux/fb.h>
 #include <sys/mman.h>
 #include <sys/ioctl.h>
+#include <cstring>
 
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/image.hpp"
 #include "cv_bridge/cv_bridge.h"
 #include "opencv2/opencv.hpp"
+
 namespace rae_hw
 {
     class LCDNode : public rclcpp::Node
@@ -16,6 +18,7 @@ namespace rae_hw
         ~LCDNode();
 
         void image_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+        void display_image(const cv::Mat& img);
 
     private:
         rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
@@ -24,5 +27,6 @@ namespace rae_hw
         struct fb_var_screeninfo vinfo;
         struct fb_fix_screeninfo finfo;
         long screensize;
+        std::string default_logo_path;
     };
 }