LSM303D does not have a gyroscope

Cartographer requires 6DOF IMU data: linear accel and angular vel.

.devcontainer/Dockerfile

@@ -4,12 +4,11 @@ FROM ros:humble-ros-base-jammy
 
 RUN apt-get update && apt-get install --yes --no-install-recommends \
         bash-completion \
-        curl \
+        curl wget \
         python3-pip \
         ros-$ROS_DISTRO-foxglove-bridge \
         ros-$ROS_DISTRO-cartographer-ros \
         ros-$ROS_DISTRO-hls-lfcd-lds-driver \
-        wget \
  && python3 -m pip install meson ninja \
  && rm -rf /var/lib/apt/lists/*
 

.devcontainer/devcontainer.json

@@ -33,7 +33,6 @@
                 "ms-python.vscode-pylance", // https://github.com/microsoft/vscode-python/issues/18073
                 "llvm-vs-code-extensions.vscode-clangd",
                 "mesonbuild.mesonbuild",
-                "twxs.cmake",
                 "rust-lang.rust-analyzer",
                 "tamasfe.even-better-toml",
                 "serayuzgur.crates",
@@ -48,10 +47,7 @@
                     "**/.git/objects/**": true,
                     "**/.git/subtree-cache/**": true,
                     "**/meson-build*/**": true,
-                    "**/target/**": true,
-                    "**/build/**": true,
-                    "**/install/**": true,
-                    "**/log/**": true
+                    "**/target/**": true
                 },
                 // https://github.com/microsoft/vscode-remote-release/issues/1671
                 "terminal.integrated.profiles.linux": {

cfg/2d.lua

@@ -5,10 +5,10 @@ options = {
   map_builder = MAP_BUILDER,
   trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
-  tracking_frame = "base_link",
+  tracking_frame = "imu_link",
   published_frame = "base_link",
   odom_frame = "odom",
-  provide_odom_frame = true,
+  provide_odom_frame = false,
   publish_frame_projected_to_2d = false,
   use_pose_extrapolator = true,
   use_odometry = false,

nodes/lsm303d.cc

@@ -0,0 +1,63 @@
+#include <chrono>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+#include <lsm303d.hh>
+
+using namespace std::chrono_literals;
+
+class LSM303DPublisher : public rclcpp::Node {
+public:
+  LSM303DPublisher()
+  : Node("lsm303d"), lsm303d(0, std::byte{0x1d}) {
+    lsm303d.control1_wr(std::byte{0b0101'0'1'1'1});
+    lsm303d.control2_wr(std::byte{0b00'000'0'0'0});
+    lsm303d.control3_wr(std::byte{0b0'0'0'0'0'0'0'0});
+    lsm303d.control4_wr(std::byte{0b0'0'0'0'0'0'0'0});
+    lsm303d.control5_wr(std::byte{0b0'11'100'0'0});
+    lsm303d.control6_wr(std::byte{0b0'00'00000});
+    lsm303d.control7_wr(std::byte{0b00'0'0'0'0'00});
+
+    publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu", 10);
+    timer_ = this->create_wall_timer(  // 100 Hz -> 10 ms
+      10ms, std::bind(&LSM303DPublisher::publish, this));
+  }
+
+private:
+  void
+  publish() {
+    auto acceleration = lsm303d.accelerometer_rd();
+
+    msg.header.frame_id = "imu_link";
+    msg.header.stamp = this->now();
+
+    msg.linear_acceleration.x = acceleration[0];
+    msg.linear_acceleration.y = acceleration[1];
+    msg.linear_acceleration.z = acceleration[2];
+
+    // LSM303D doesn't produce an orientation and angular velocity estimate
+    msg.orientation_covariance[0] = -1;
+    msg.angular_velocity_covariance[0] = -1;
+    // A zero covariance matrix will be interpreted as "covariance unknown"
+    msg.linear_acceleration_covariance[0] = 0;
+    msg.linear_acceleration_covariance[4] = 0;
+    msg.linear_acceleration_covariance[8] = 0;
+
+    publisher_->publish(msg);
+  }
+
+  LSM303D lsm303d;
+  sensor_msgs::msg::Imu msg;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
+};
+
+
+auto
+main(int argc, char * argv[]) -> int {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<LSM303DPublisher>());
+  rclcpp::shutdown();
+}

nodes/meson.build

@@ -1,8 +1,15 @@
 rclcpp = dependency('rclcpp', method : 'cmake')
 std_msgs = dependency('std_msgs', method : 'cmake')
+sensor_msgs = dependency('sensor_msgs', method : 'cmake')
 
 executable('example',
     sources : 'example.cc',
     # include_directories :
     dependencies : [rclcpp, std_msgs],
 )
+
+executable('lsm303d',
+    sources : 'lsm303d.cc',
+    include_directories : inc,
+    dependencies : [rclcpp, sensor_msgs],
+)

slam.py

@@ -21,11 +21,29 @@ def generate_launch_description():
     #     ],
     # )
 
+    # static_tf = Node(
+    #     package = "tf2_ros", 
+    #     executable = "static_transform_publisher",
+    #     arguments = ["0", "0", "0", "0", "0", "0", "odom", "laser"],
+    # )
+
+    base_tf_laser = Node(
+        package = "tf2_ros", 
+        executable = "static_transform_publisher",
+        arguments = ["0", "0", "0", "0", "0", "0", "base_link", "laser_link"],
+    )
+
+    laser_tf_imu = Node(
+        package = "tf2_ros", 
+        executable = "static_transform_publisher",
+        arguments = ["-0.048", "-0.036", "-0.025", "0", "0", "0", "laser_link", "imu_link"],
+    )
+
     hls_lfcd_lds_publisher_node = Node(
         package="hls_lfcd_lds_driver",
         executable="hlds_laser_publisher",
         name="hlds_laser_publisher",
-        parameters=[{"port": "/dev/ttyUSB0", "frame_id": "laser"}],
+        parameters=[{"port": "/dev/ttyUSB0", "frame_id": "laser_link"}],
         output="screen"
     )
 
@@ -52,7 +70,9 @@ def generate_launch_description():
 
     return LaunchDescription([
         # robot_state_publisher_node,
+        base_tf_laser,
+        laser_tf_imu,
         hls_lfcd_lds_publisher_node,
-        # cartographer_node,
-        # cartographer_occupancy_grid_node,
+        cartographer_node,
+        cartographer_occupancy_grid_node,
     ])

trials/Makefile

@@ -1,3 +1,6 @@
+csi_camera:
+	c++ csi_camera.cc -I/usr/include/opencv4/ -o csi_camera -lopencv_core -lopencv_videoio -lopencv_highgui
+
 ioctl_i2c_rdwr:
 	c++ -Wall -Wextra -Wpedantic -std=c++20 -g -o ioctl_i2c_rdwr ioctl_i2c_rdwr.cc
 
@@ -8,6 +11,7 @@ libi2c_smbus:
 	c++ -Wall -Wextra -Wpedantic -std=c++20 -g -o libi2c_smbus libi2c_smbus.cc -li2c
 
 clean:
+	rm -f csi_camera
 	rm -f ioctl_i2c_rdwr
 	rm -f plain_i2c_rdwr
 	rm -f libi2c_smbus

trials/csi_camera.cc

@@ -0,0 +1,60 @@
+#include <iostream>
+
+#include <opencv2/opencv.hpp>
+
+// int main()
+// {
+//   std::cout << cv::getBuildInformation() << std::endl;
+// }
+
+// Notice that nvarguscamerasrc is not avaliable inside a container; thus, access the CSI cam is not possible
+std::string gstreamer_pipeline (int capture_width, int capture_height, int display_width, int display_height, int framerate, int flip_method) {
+    return "nvarguscamerasrc ! video/x-raw(memory:NVMM), width=(int)" + std::to_string(capture_width) + ", height=(int)" +
+           std::to_string(capture_height) + ", framerate=(fraction)" + std::to_string(framerate) +
+           "/1 ! nvvidconv flip-method=" + std::to_string(flip_method) + " ! video/x-raw, width=(int)" + std::to_string(display_width) + ", height=(int)" +
+           std::to_string(display_height) + ", format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink";
+}
+
+int main()
+{
+    int capture_width = 640 ;
+    int capture_height = 480 ;
+    int display_width = 640 ;
+    int display_height = 480 ;
+    int framerate = 30 ;
+    int flip_method = 0 ;
+
+    std::string pipeline = gstreamer_pipeline(capture_width,
+        capture_height,
+        display_width,
+        display_height,
+        framerate,
+        flip_method);
+    std::cout << "Using pipeline: \n\t" << pipeline << "\n";
+
+    cv::VideoCapture cap(pipeline, cv::CAP_GSTREAMER);
+
+    if(!cap.isOpened()) {
+        std::cout<<"Failed to open camera."<<std::endl;
+        return (-1);
+    }
+
+    cv::namedWindow("CSI Camera", cv::WINDOW_AUTOSIZE);
+    cv::Mat img;
+
+    std::cout << "Hit ESC to exit" << "\n" ;
+    while(true)
+    {
+        if (!cap.read(img)) {
+                std::cout<<"Capture read error"<<std::endl;
+                break;
+        }
+
+        cv::imshow("CSI Camera",img);
+        int keycode = cv::waitKey(10) & 0xff ; 
+        if (keycode == 27) break ;
+    }
+
+    cap.release();
+    cv::destroyAllWindows() ;
+}

trials/opencv_buildinfo.txt

@@ -0,0 +1,108 @@
+General configuration for OpenCV 4.5.4 =====================================
+  Version control:               unknown
+
+  Extra modules:
+    Location (extra):            /build/opencv-Ro9xS7/opencv-4.5.4+dfsg/contrib/modules
+    Version control (extra):     unknown
+
+  Platform:
+    Timestamp:                   2022-02-02T14:55:11Z
+    Host:                        Linux 4.15.0-167-generic aarch64
+    CMake:                       3.22.1
+    CMake generator:             Ninja
+    CMake build tool:            /usr/bin/ninja
+    Configuration:               Release
+
+  CPU/HW features:
+    Baseline:                    NEON FP16
+
+  C/C++:
+    Built as dynamic libs?:      YES
+    C++ standard:                11
+    C++ Compiler:                /usr/bin/c++  (ver 11.2.0)
+    C++ flags (Release):         -g -O2 -ffile-prefix-map=/build/opencv-Ro9xS7/opencv-4.5.4+dfsg=. -flto=auto -ffat-lto-objects -flto=auto -ffat-lto-objects -fstack-protector-strong -Wformat -Werror=format-security -Wdate-time -D_FORTIFY_SOURCE=2   -fsigned-char -W -Wall -Werror=return-type -Werror=non-virtual-dtor -Werror=address -Werror=sequence-point -Wformat -Werror=format-security -Wmissing-declarations -Wundef -Winit-self -Wpointer-arith -Wshadow -Wsign-promo -Wuninitialized -Wsuggest-override -Wno-delete-non-virtual-dtor -Wno-comment -Wimplicit-fallthrough=3 -Wno-strict-overflow -fdiagnostics-show-option -pthread -fomit-frame-pointer -ffunction-sections -fdata-sections    -fvisibility=hidden -fvisibility-inlines-hidden -g -O2 -ffile-prefix-map=/build/opencv-Ro9xS7/opencv-4.5.4+dfsg=. -flto=auto -ffat-lto-objects -flto=auto -ffat-lto-objects -fstack-protector-strong -Wformat -Werror=format-security  -DNDEBUG
+    C++ flags (Debug):           -g -O2 -ffile-prefix-map=/build/opencv-Ro9xS7/opencv-4.5.4+dfsg=. -flto=auto -ffat-lto-objects -flto=auto -ffat-lto-objects -fstack-protector-strong -Wformat -Werror=format-security -Wdate-time -D_FORTIFY_SOURCE=2   -fsigned-char -W -Wall -Werror=return-type -Werror=non-virtual-dtor -Werror=address -Werror=sequence-point -Wformat -Werror=format-security -Wmissing-declarations -Wundef -Winit-self -Wpointer-arith -Wshadow -Wsign-promo -Wuninitialized -Wsuggest-override -Wno-delete-non-virtual-dtor -Wno-comment -Wimplicit-fallthrough=3 -Wno-strict-overflow -fdiagnostics-show-option -pthread -fomit-frame-pointer -ffunction-sections -fdata-sections    -fvisibility=hidden -fvisibility-inlines-hidden -g  -DDEBUG -D_DEBUG
+    C Compiler:                  /usr/bin/cc
+    C flags (Release):           -g -O2 -ffile-prefix-map=/build/opencv-Ro9xS7/opencv-4.5.4+dfsg=. -flto=auto -ffat-lto-objects -flto=auto -ffat-lto-objects -fstack-protector-strong -Wformat -Werror=format-security -Wdate-time -D_FORTIFY_SOURCE=2   -fsigned-char -W -Wall -Werror=return-type -Werror=address -Werror=sequence-point -Wformat -Werror=format-security -Wmissing-declarations -Wmissing-prototypes -Wstrict-prototypes -Wundef -Winit-self -Wpointer-arith -Wshadow -Wuninitialized -Wno-comment -Wimplicit-fallthrough=3 -Wno-strict-overflow -fdiagnostics-show-option -pthread -fomit-frame-pointer -ffunction-sections -fdata-sections    -fvisibility=hidden -g -O2 -ffile-prefix-map=/build/opencv-Ro9xS7/opencv-4.5.4+dfsg=. -flto=auto -ffat-lto-objects -flto=auto -ffat-lto-objects -fstack-protector-strong -Wformat -Werror=format-security  -DNDEBUG
+    C flags (Debug):             -g -O2 -ffile-prefix-map=/build/opencv-Ro9xS7/opencv-4.5.4+dfsg=. -flto=auto -ffat-lto-objects -flto=auto -ffat-lto-objects -fstack-protector-strong -Wformat -Werror=format-security -Wdate-time -D_FORTIFY_SOURCE=2   -fsigned-char -W -Wall -Werror=return-type -Werror=address -Werror=sequence-point -Wformat -Werror=format-security -Wmissing-declarations -Wmissing-prototypes -Wstrict-prototypes -Wundef -Winit-self -Wpointer-arith -Wshadow -Wuninitialized -Wno-comment -Wimplicit-fallthrough=3 -Wno-strict-overflow -fdiagnostics-show-option -pthread -fomit-frame-pointer -ffunction-sections -fdata-sections    -fvisibility=hidden -g  -DDEBUG -D_DEBUG
+    Linker flags (Release):      -Wl,-Bsymbolic-functions -flto=auto -ffat-lto-objects -flto=auto -Wl,-z,relro -Wl,-z,now  -Wl,--gc-sections -Wl,--as-needed -Wl,-Bsymbolic-functions -flto=auto -ffat-lto-objects -flto=auto -Wl,-z,relro -Wl,-z,now 
+    Linker flags (Debug):        -Wl,-Bsymbolic-functions -flto=auto -ffat-lto-objects -flto=auto -Wl,-z,relro -Wl,-z,now  -Wl,--gc-sections -Wl,--as-needed  
+    ccache:                      NO
+    Precompiled headers:         NO
+    Extra dependencies:          dl m pthread rt
+    3rdparty dependencies:
+
+  OpenCV modules:
+    To be built:                 alphamat aruco barcode bgsegm bioinspired calib3d ccalib core datasets dnn dnn_objdetect dnn_superres dpm face features2d flann freetype fuzzy hdf hfs highgui img_hash imgcodecs imgproc intensity_transform java line_descriptor mcc ml objdetect optflow phase_unwrapping photo plot python3 quality rapid reg rgbd saliency shape stereo stitching structured_light superres surface_matching text tracking video videoio videostab viz wechat_qrcode ximgproc xobjdetect xphoto
+    Disabled:                    world
+    Disabled by dependency:      sfm
+    Unavailable:                 cudaarithm cudabgsegm cudacodec cudafeatures2d cudafilters cudaimgproc cudalegacy cudaobjdetect cudaoptflow cudastereo cudawarping cudev cvv gapi julia matlab ovis python2 ts
+    Applications:                apps
+    Documentation:               doxygen python javadoc
+    Non-free algorithms:         NO
+
+  GUI:                           GTK3
+    GTK+:                        YES (ver 3.24.30)
+      GThread :                  YES (ver 2.71.1)
+      GtkGlExt:                  NO
+    OpenGL support:              NO
+    VTK support:                 YES (ver 9.1.0)
+
+  Media I/O: 
+    ZLib:                        /usr/lib/aarch64-linux-gnu/libz.so (ver 1.2.11)
+    JPEG:                        /usr/lib/aarch64-linux-gnu/libjpeg.so (ver 80)
+    WEBP:                        /usr/lib/aarch64-linux-gnu/libwebp.so (ver encoder: 0x020f)
+    PNG:                         /usr/lib/aarch64-linux-gnu/libpng.so (ver 1.6.37)
+    TIFF:                        /usr/lib/aarch64-linux-gnu/libtiff.so (ver 42 / 4.3.0)
+    JPEG 2000:                   OpenJPEG (ver 2.4.0)
+    OpenEXR:                     /usr/lib/aarch64-linux-gnu/libImath-2_5.so /usr/lib/aarch64-linux-gnu/libIlmImf-2_5.so /usr/lib/aarch64-linux-gnu/libIex-2_5.so /usr/lib/aarch64-linux-gnu/libHalf-2_5.so /usr/lib/aarch64-linux-gnu/libIlmThread-2_5.so (ver 2_5)
+    GDAL:                        YES (/lib/libgdal.so)
+    GDCM:                        YES (3.0.10)
+    HDR:                         YES
+    SUNRASTER:                   YES
+    PXM:                         YES
+    PFM:                         YES
+
+  Video I/O:
+    DC1394:                      YES (2.2.6)
+    FFMPEG:                      YES
+      avcodec:                   YES (58.134.100)
+      avformat:                  YES (58.76.100)
+      avutil:                    YES (56.70.100)
+      swscale:                   YES (5.9.100)
+      avresample:                NO
+    GStreamer:                   YES (1.19.90)
+    PvAPI:                       NO
+    v4l/v4l2:                    YES (linux/videodev2.h)
+    gPhoto2:                     YES
+
+  Parallel framework:            TBB (ver 2020.3 interface 11103)
+
+  Trace:                         YES (built-in)
+
+  Other third-party libraries:
+    Lapack:                      NO
+    Eigen:                       YES (ver 3.4.0)
+    Custom HAL:                  NO
+    Protobuf:                    /usr/lib/aarch64-linux-gnu/libprotobuf.so (3.12.4)
+
+  OpenCL:                        YES (no extra features)
+    Include path:                /usr/include/CL
+    Link libraries:              Dynamic load
+
+  Python 3:
+    Interpreter:                 /usr/bin/python3 (ver 3.10.2)
+    Libraries:                   /usr/lib/aarch64-linux-gnu/libpython3.10.so (ver 3.10.2)
+    numpy:                       /usr/lib/python3/dist-packages/numpy/core/include (ver 1.21.5)
+    install path:                lib/python3.10/dist-packages
+
+  Python (for build):            /usr/bin/python3
+
+  Java:                          
+    ant:                         /usr/bin/ant (ver 1.10.12)
+    JNI:                         /usr/lib/jvm/default-java/include /usr/lib/jvm/default-java/include/linux /usr/lib/jvm/default-java/include
+    Java wrappers:               YES
+    Java tests:                  NO
+
+  Install to:                    /usr
+-----------------------------------------------------------------