Humble

.github/workflows/ros_build.yaml

@@ -1,4 +1,4 @@
-name: ROS2 CI
+name: ROS2 Iron
 
 on:
   pull_request:
@@ -15,26 +15,28 @@ jobs:
       fail-fast: false
       matrix:
         ros_distribution:
-          - humble
           - iron 
-          - rolling
         include:
-          # Humble Hawksbill (May 2022 - May 2027)
-          - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest
-            ros_distribution: humble
-            ros_version: 2
           - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest
             ros_distribution: iron
             ros_version: 2            
-          # Rolling Ridley  (June 2020 - Present)
-          - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest
-            ros_distribution: rolling
-            ros_version: 2
     container:
       image: ${{ matrix.docker_image }}
     steps:
       - name: checkout
         uses: actions/checkout@v2
+      - name: Install Ceres dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y \
+            libunwind-dev \
+            libgoogle-glog-dev \
+            libatlas-base-dev \
+            libsuitesparse-dev
+      - name: Install Ceres
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y libceres-dev
       - name: build and test
         uses: ros-tooling/action-ros-ci@master
         with:

.github/workflows/ros_build_humble.yaml

@@ -0,0 +1,45 @@
+name: ROS2 Humble
+
+on:
+  pull_request:
+    branches:
+      - 'humble'
+  push:
+    branches:
+      - 'humble'
+
+jobs:
+  test_environment:
+    runs-on: [ubuntu-latest]
+    strategy:
+      fail-fast: false
+      matrix:
+        ros_distribution:
+          - humble 
+        include:
+          - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest
+            ros_distribution: humble
+            ros_version: 2          
+    container:
+      image: ${{ matrix.docker_image }}
+    steps:
+      - name: checkout
+        uses: actions/checkout@v2
+      - name: Install Ceres dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y \
+            libunwind-dev \
+            libgoogle-glog-dev \
+            libatlas-base-dev \
+            libsuitesparse-dev
+      - name: Install Ceres
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y libceres-dev
+      - name: build and test
+        uses: ros-tooling/action-ros-ci@master
+        with:
+          target-ros2-distro: ${{ matrix.ros_distribution }}
+          vcs-repo-file-url: ""
+          skip-tests: true

README.md

@@ -69,31 +69,69 @@ Real time Go2 Air/PRO ROS2 topics
 
 ## System requirements
 Tested systems and ROS2 distro
-|systems|ROS2 distro|
-|--|--|
-|Ubuntu 22.04|humble|
-|Ubuntu 22.04|iron|
+|systems|ROS2 distro|Build status
+|--|--|--|
+|Ubuntu 22.04|iron|![example workflow](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg)
+|Ubuntu 22.04|humble|![example workflow](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build_humble.yaml/badge.svg)
+
+A single workspace can contain as many packages as you want, each in their own folder. You can also have packages of different build types in one workspace (CMake, Python, etc.). You cannot have nested packages.
+
+Best practice is to have a src folder within your workspace, and to create your packages in there. This keeps the top level of the workspace “clean”.
+
+Your workspace should look like:
+
+```
+workspace_folder/
+    src/
+      py_package_1/
+          package.xml
+          resource/py_package_1
+          setup.cfg
+          setup.py
+          py_package_1/
+
+      py_package_2/
+          package.xml
+          resource/py_package_2
+          setup.cfg
+          setup.py
+          py_package_2/
+```
+
+clone this repo to src folder of your own ros2_ws repo
 
-clone this rep and build it (put go2_interfaces and go2_robot_sdk to src folder of your own ros2_ws repo)
-install rust language support in your system (cargo build should work in terminal)
 ```
 git clone https://github.com/abizovnuralem/go2_ros2_sdk.git
+cd go2_ros2_sdk
 pip install -r requirements.txt
 sudo apt install ros-{ROS2_VER}-test-msgs
 sudo apt install ros-{ROS2_VER}-tf2-sensor-msgs
-source /opt/ros/<your_ros_ver>/setup.bash
-colcon build
+cd ..
+mkdir -p ros2_ws/src
+copy all files inside go2_ros2_sdk folder to ros2_ws/src folder
 ```
 
-don't forget to setup your GO2-robot in Wifi-mode and get IP
-then
+install rust language support in your system https://www.rust-lang.org/tools/install 
 
+cargo should work in terminal
 ```
-export ROBOT_IP="Your robot ip"
+cargo --version
+```
+
+build it
+
+```
+source /opt/ros/<your_ros_ver>/setup.bash
+cd ros2_ws
+colcon build
 ```
 
 ## Usage
+don't forget to setup your GO2-robot in Wifi-mode and get IP then
+
 ```
+export ROBOT_IP="Your robot ip"
+cd ros2_ws
 source install/setup.bash
 ros2 launch go2_robot_sdk robot.launch.py
 ```

nav2_amcl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.2.0</version>
+  <version>1.1.13</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_amcl/src/amcl_node.cpp

@@ -328,12 +328,10 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   nomotion_update_srv_.reset();
   initial_pose_sub_.reset();
   laser_scan_connection_.disconnect();
-  tf_listener_.reset();  //  listener may access lase_scan_filter_, so it should be reset earlier
   laser_scan_filter_.reset();
   laser_scan_sub_.reset();
 
   // Map
-  map_sub_.reset();  //  map_sub_ may access map_, so it should be reset earlier
   if (map_ != NULL) {
     map_free(map_);
     map_ = nullptr;
@@ -343,6 +341,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   // Transforms
   tf_broadcaster_.reset();
+  tf_listener_.reset();
   tf_buffer_.reset();
 
   // PubSub
@@ -809,15 +808,6 @@ bool AmclNode::updateFilter(
     get_logger(), "Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min,
     angle_increment);
 
-  // Check the validity of range_max, must > 0.0
-  if (laser_scan->range_max <= 0.0) {
-    RCLCPP_WARN(
-      get_logger(), "wrong range_max of laser_scan data: %f. The message could be malformed."
-      " Ignore this message and stop updating.",
-      laser_scan->range_max);
-    return false;
-  }
-
   // Apply range min/max thresholds, if the user supplied them
   if (laser_max_range_ > 0.0) {
     ldata.range_max = std::min(laser_scan->range_max, static_cast<float>(laser_max_range_));
@@ -1168,7 +1158,7 @@ AmclNode::dynamicParametersCallback(
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
       if (param_name == "alpha1") {
         alpha1_ = parameter.as_double();
-        // alpha restricted to be non-negative
+        //alpha restricted to be non-negative
         if (alpha1_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha1 to be negative,"
@@ -1178,7 +1168,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha2") {
         alpha2_ = parameter.as_double();
-        // alpha restricted to be non-negative
+        //alpha restricted to be non-negative
         if (alpha2_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha2 to be negative,"
@@ -1188,7 +1178,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha3") {
         alpha3_ = parameter.as_double();
-        // alpha restricted to be non-negative
+        //alpha restricted to be non-negative
         if (alpha3_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha3 to be negative,"
@@ -1198,7 +1188,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha4") {
         alpha4_ = parameter.as_double();
-        // alpha restricted to be non-negative
+        //alpha restricted to be non-negative
         if (alpha4_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha4 to be negative,"
@@ -1208,7 +1198,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha5") {
         alpha5_ = parameter.as_double();
-        // alpha restricted to be non-negative
+        //alpha restricted to be non-negative
         if (alpha5_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha5 to be negative,"

nav2_amcl/src/pf/eig3.c

@@ -43,7 +43,7 @@ static void tred2(double V[n][n], double d[n], double e[n])
 //  Fortran subroutine in EISPACK.
 
   int i, j, k;
-  double f, g, hh;
+  double f, g, h, hh;
   for (j = 0; j < n; j++) {
     d[j] = V[n - 1][j];
   }
@@ -122,7 +122,7 @@ static void tred2(double V[n][n], double d[n], double e[n])
   for (i = 0; i < n - 1; i++) {
     V[n - 1][i] = V[i][i];
     V[i][i] = 1.0;
-    const double h = d[i + 1];
+    h = d[i + 1];
     if (h != 0.0) {
       for (k = 0; k <= i; k++) {
         d[k] = V[k][i + 1] / h;