Feat/handheld mapping #79

DingoOz · 2025-01-16T11:36:52Z

Merged three branches into one. PR covers:
*adding rqt and tf2_tools to the flake.nix
*adding slam_toolbox and launch files for autonomy

updating autonomy ready for near future feature addition of using robot_localization
fixed IMU data's frame so it is separate to laser_scan data
correcting m2m2 lidar so that is produces 4096 interpolate points per scan

Upgraded m2m2_lidar to use simple-networking from the Perseus project Also amended the readme (typo re node) persues_sensors(m2m2_lidar): m2m2_lidar node uses simple-networking

perseus_sensors(m2m2_lidar): Improve error handling in constructor perseus_sensors(m2m2_lidar): Added networking namespace to constructor perseus_sensors(m2m2_lidar): removed erroneous rule of 5 perseus_sensors(m2m2_lidar): removed erroneous rule of 5 from cpp perseus_sensors(m2m2_lidar): used assert for networking client checks perseus_sensors(m2m2_lidar): removed try blocks perseus_sensors(m2m2_ldiar): put back try/catch

The code predated the current software standards and templates. Brought up to date.

Upgraded m2m2_lidar to use simple-networking from the Perseus project Also amended the readme (typo re node) persues_sensors(m2m2_lidar): m2m2_lidar node uses simple-networking perseus_sensors(m2m2_lidar): updated to use simple-networking perseus_sensors(m2m2_lidar): Improve error handling in constructor perseus_sensors(m2m2_lidar): Added networking namespace to constructor perseus_sensors(m2m2_lidar): removed erroneous rule of 5 perseus_sensors(m2m2_lidar): removed erroneous rule of 5 from cpp perseus_sensors(m2m2_lidar): used assert for networking client checks perseus_sensors(m2m2_lidar): removed try blocks perseus_sensors(m2m2_ldiar): put back try/catch perseus_sensors(m2m2_lidar): removed destructor

documentation(software): fixed typo

Fixed up laser_scan_matcher and added odom to slamtoolbox

Fixed launch to use correct yaml and added base_link tf Additional TF link added in launch

RandomSpaceship

Overall good work, although there are a few issues.

Some launch files are made obselete by work on gazebo-sim, and should probably be refactored to better split responsibilities
I haven't reviewed the parameter files due to a lack of time

RandomSpaceship · 2025-01-18T12:25:17Z

docs/source/architecture/software.md

+
+The simple-networking library provides a modern C++ implementation for handling network socket communications, with a primary focus on client-side operations.
+
+It offers an object-oriented wrapper around traditional POSIX socket operations, supporting both TCP and UDP protocols. The library implements RAII principles through its Client class, which manages socket creation, configuration, connection and cleanup while providing exception-based error handling for robust failure management.


You can use {expr}`Client` or {cpp:expr}`Client` (the cpp is optional, since it's set as the primary language) to automatically link to the generated documentation. However, I'd recommend adding the namespace in if you do this (networking::Client).

RandomSpaceship · 2025-01-18T13:49:47Z

docs/source/architecture/software.md

+The library distinguishes itself through flexible socket configuration using handler callbacks, support for custom bind addresses and a clean abstraction over low-level socket operations. It provides convenient methods for transmitting and receiving both string and binary data, with support for both blocking and non-blocking operations. Error handling is comprehensive, with descriptive error messages that include both the operation context and underlying system error details.
+:::{warning}
+This library is not used for ROS2 communications, it exists for scenarios such as a creating a ROS2 driver node which needs to interface with a specific sensor via ethernet.
+:::


Might rewrite this at some point, probably along with the Nix stuff, but it's fine. I'd rewrite it to emphasise slightly different points

RandomSpaceship · 2025-01-18T13:54:24Z

flake.nix

+            tf2-tools
+            rqt-gui
+            rqt-gui-py
+            rqt-graph
+            rqt-plot
+            rqt-reconfigure
+            rqt-common-plugins


It might be worth moving these into a separate package at some point (a perseus_visualisation meta-package?) but I think that probably doesn't make too much sense right now

RandomSpaceship · 2025-01-18T14:27:08Z

software/ros_ws/src/autonomy/config/ekf_config.yaml

Going to assume the config files are all fine, I don't have the time to read through the documentation to verify them right now.

RandomSpaceship · 2025-01-18T14:31:23Z

software/ros_ws/src/autonomy/launch/handheld_mapping.launch.py

+    # # Robot localization node for sensor fusion
+    # start_robot_localization_node = Node(
+    #     package="robot_localization",
+    #     executable="ekf_node",
+    #     name="ekf_filter_node",
+    #     output="screen",
+    #     parameters=[
+    #         {
+    #             # The frequency of the filter prediction step
+    #             "frequency": 30.0,
+    #             # Frame definitions
+    #             "publish_tf": True,
+    #             "map_frame": "map",
+    #             "odom_frame": "odom",
+    #             "base_link_frame": "base_link",
+    #             "world_frame": "odom",
+    #             "imu_frame": "imu_link",
+    #             # True if sensor provides relative measurements
+    #             "imu0_differential": False,
+    #             "imu0_relative": False,
+    #             # IMU sensor configuration
+    #             "imu0": "/imu",
+    #             "imu0_pose_rejection_threshold": 0.0,
+    #             "imu0_twist_rejection_threshold": 0.0,
+    #             "imu0_linear_acceleration_rejection_threshold": 0.0,
+    #             "ignore_orientation_covariance": True,
+    #             "ignore_angular_velocity_covariance": True,
+    #             "ignore_linear_acceleration_covariance": True,
+    #             "imu0_config": [
+    #                 False,
+    #                 False,
+    #                 False,  # x, y, z position
+    #                 True,
+    #                 True,
+    #                 True,  # roll, pitch, yaw
+    #                 False,
+    #                 False,
+    #                 False,  # x, y, z velocity
+    #                 True,
+    #                 True,
+    #                 True,  # roll, pitch, yaw rates
+    #                 True,
+    #                 True,
+    #                 True,  # x, y, z accelerations
+    #             ],
+    #             # Whether to publish the acceleration state
+    #             "imu0_remove_gravitational_acceleration": True,
+    #             "imu0_queue_size": 10,
+    #             "imu0_nodelay": True,
+    #             "imu_frame": "lidar_frame",  # Match the actual IMU frame_id
+    #             "publish_acceleration": False,
+    #             "print_diagnostics": True,
+    #             "debug": True,
+    #             "debug_out_file": "robot_localization_debug.txt",
+    #             "use_fixed_covariance": True,
+    #             "imu0_pose_covariance": [
+    #                 0.01,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.01,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.01,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.01,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.01,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.0,
+    #                 0.01,
+    #             ],
+    #         }
+    #     ],
+    # )


I'm not a fan of leaving commented code in the repo, should this be either removed or enabled? If you keep it, you should also use the yaml file for parameters rather than passing them in manually.

RandomSpaceship · 2025-01-18T14:41:48Z

software/ros_ws/src/autonomy/launch/mapping_using_slam_toolbox.launch.py

The launch file refactoring I did on the gazebo-sim branch means this has to be almost completely rewritten, sorry. IMO, it'd make more sense to isolate rover bringup to the perseus package, and keep this one to launching solely autonomy-related tasks, anyway - that goes for all the launch files here.

RandomSpaceship · 2025-01-18T14:49:52Z

software/ros_ws/src/perseus_description/urdf/perseus.urdf.xacro

+
+    <!--BASE_FOOTPRINT_LINK-->
+    <joint name="base_footprint_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="base_footprint"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+    </joint>
+
+    <link name="base_footprint">
+    </link>
+
+


What advantage is there to adding the base_footprint link, instead of just using base_link?

RandomSpaceship · 2025-01-18T14:54:26Z

software/ros_ws/src/perseus_sensors/include/m2m2_lidar/m2m2_lidar.hpp

+    static constexpr std::string_view REQUEST_DELIM{"\r\n\r\n"};
+    static constexpr size_t INTERPOLATED_POINTS = 4096;  // Number of points per scan
+    // for imu
+    static constexpr char const* IMU_COMMAND = "getimuinrobotcoordinate";


Suggested change

static constexpr char const* IMU_COMMAND = "getimuinrobotcoordinate";

static const std::string IMU_COMMAND = "getimuinrobotcoordinate";

There's no reason to use char* if it can be avoided, especially given the existence of the .c_str() method on std::string.

RandomSpaceship · 2025-01-18T15:15:53Z

software/ros_ws/src/perseus_sensors/src/m2m2_lidar/m2m2_lidar.cpp

+        while (angle < 0) angle += 2 * M_PI;
+        while (angle >= 2 * M_PI) angle -= 2 * M_PI;


Suggested change

while (angle < 0) angle += 2 * M_PI;

while (angle >= 2 * M_PI) angle -= 2 * M_PI;

const auto tau = 2 * std::numbers::pi;

angle -= std::trunc(angle / tau) * tau;

if (angle < 0) angle += tau;

While loops to process any kind of number are a bad idea, and especially so for floats.

If you use std::numbers::pi (yay C++20!) instead of M_PI from <cmath> you'll need to #include <numbers> too

RandomSpaceship · 2025-01-18T15:28:41Z

software/ros_ws/src/perseus_sensors/src/m2m2_lidar/m2m2_lidar.cpp

+        while (idx < points.size() && get<0>(points[idx]) < targetAngle)
+        {
+            idx++;
+        }


Again, a while loop is a bad idea. You've already sorted your points, and size(interpolated points) >> size(source points), so you can store idx outside the loop (though you should probably rename it to something like sourcePointIdx), and just do a check to see if you need to increment it once at the start of each loop.

DingoOz and others added 15 commits January 11, 2025 21:07

perseus_sensors(m2m2_lidar): Uses simple-networking now

2ef201c

Upgraded m2m2_lidar to use simple-networking from the Perseus project Also amended the readme (typo re node) persues_sensors(m2m2_lidar): m2m2_lidar node uses simple-networking

autonomy: bugfix to align autonomy launch with current project

dd13338

The code predated the current software standards and templates. Brought up to date.

perseus_sensors(m2m2_lidar): extract imu data and publish /imu

c273bae

Documentation(software): Section for simple-networking library

5d803fd

documentation(software): fixed typo

autonomy: started with the slam_toolbox launch file for handheld

da55b7e

Fixed up laser_scan_matcher and added odom to slamtoolbox

autonomy: Added tf2_tools to the nix environment

997fde0

Fixed launch to use correct yaml and added base_link tf Additional TF link added in launch

autonomy: update the description with base_footprint

7e86b04

autonomy: added rqt to the flake.nix

18bc8d9

perseus_sensors: fixed imu frame id

c3a741d

perseus_sensors(m2m2_lidar): interpolate raw data and output 4096 points

9a796bd

autonomy: Updated CMakeLists.txt for alignement

b71e5ab

perseus_sensors(m2m2_lidar): Fixed typos from merge

05b14dc

chore: Format and lint

6912288

DingoOz added the 21-ros Involves ROS code label Jan 16, 2025

DingoOz added this to the ARCh 2025 Competition milestone Jan 16, 2025

DingoOz requested a review from RandomSpaceship January 16, 2025 11:36

DingoOz self-assigned this Jan 16, 2025

RandomSpaceship reviewed Jan 18, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feat/handheld mapping #79

Feat/handheld mapping #79

DingoOz commented Jan 16, 2025

RandomSpaceship left a comment

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025

RandomSpaceship Jan 18, 2025


		The simple-networking library provides a modern C++ implementation for handling network socket communications, with a primary focus on client-side operations.

		It offers an object-oriented wrapper around traditional POSIX socket operations, supporting both TCP and UDP protocols. The library implements RAII principles through its Client class, which manages socket creation, configuration, connection and cleanup while providing exception-based error handling for robust failure management.

	static constexpr char const* IMU_COMMAND = "getimuinrobotcoordinate";
	static const std::string IMU_COMMAND = "getimuinrobotcoordinate";

		while (angle < 0) angle += 2 * M_PI;
		while (angle >= 2 * M_PI) angle -= 2 * M_PI;

-        while (angle < 0) angle += 2 * M_PI;
-        while (angle >= 2 * M_PI) angle -= 2 * M_PI;
+        const auto tau = 2 * std::numbers::pi;
+        angle -= std::trunc(angle / tau) * tau;
+        if (angle < 0) angle += tau;

Feat/handheld mapping #79

Are you sure you want to change the base?

Feat/handheld mapping #79

Conversation

DingoOz commented Jan 16, 2025

RandomSpaceship left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment