Merge pull request #50 from sskorol/GH-35-address-slam-issues

Addressed a bunch of issues as a part SLAM flow fixes

.dockerignore

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

Dockerfile

@@ -6,12 +6,33 @@ ARG BUILD_TYPE="RelWithDebInfo"
 
 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

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/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
@@ -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_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_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;
     };
 }

rae_hw/launch/peripherals.launch.py

@@ -34,6 +34,9 @@ def launch_setup(context, *args, **kwargs):
                     name='lcd_node',
                     package='rae_hw',
                     plugin='rae_hw::LCDNode',
+                    parameters=[{
+                        'default_logo_path': LaunchConfiguration('default_logo_path')
+                    }],
                 ),
                 ComposableNode(
                     name='led_node',
@@ -62,6 +65,7 @@ def generate_launch_description():
         DeclareLaunchArgument('name', default_value='rae'),
         DeclareLaunchArgument('run_container', default_value='true'),
         DeclareLaunchArgument('enable_battery_status', default_value='true'),
+        DeclareLaunchArgument('default_logo_path', default_value='/usr/share/rae-logo-white.jpg'),
     ]
 
     return LaunchDescription(

rae_hw/src/peripherals/lcd.cpp

@@ -15,6 +15,10 @@ namespace rae_hw
     LCDNode::LCDNode(const rclcpp::NodeOptions &options)
         : Node("lcd_node", options)
     {
+        std::string logo_key = "default_logo_path";
+        declare_parameter<std::string>(logo_key);
+        default_logo_path = get_parameter(logo_key).as_string();
+
         // Open the framebuffer device
         fbfd = open("/dev/fb0", O_RDWR);
         if (fbfd == -1)
@@ -52,16 +56,22 @@ namespace rae_hw
             "lcd", 10, std::bind(&LCDNode::image_callback, this, std::placeholders::_1));
         RCLCPP_INFO(this->get_logger(), "LCD node running!");
     }
+
     LCDNode::~LCDNode()
     {
+        // Load default image
+        cv::Mat default_img = cv::imread(default_logo_path);
+        if (!default_img.empty())
+        {
+            display_image(default_img);
+        }
+
         munmap(fbp, screensize);
         close(fbfd);
     }
 
-    void LCDNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
+    void LCDNode::display_image(const cv::Mat& img)
     {
-        // Convert ROS image message to OpenCV image
-        cv::Mat img = cv_bridge::toCvCopy(msg, "bgr8")->image;
         // Resize to match the screen dimensions
         cv::resize(img, img, cv::Size(vinfo.xres, vinfo.yres));
         // Iterate over the image pixels
@@ -77,8 +87,15 @@ namespace rae_hw
                 *((cv::Vec3b *)(fbp + location)) = pixel;
             }
         }
-    };
+    }
 
+    void LCDNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
+    {
+        // Convert ROS image message to OpenCV image
+        cv::Mat img = cv_bridge::toCvCopy(msg, "bgr8")->image;
+        display_image(img);
+    };
 }
+
 #include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(rae_hw::LCDNode);
+RCLCPP_COMPONENTS_REGISTER_NODE(rae_hw::LCDNode);