fix: double reset segfault / fp error (Merge pull request #163 from utn-mi/krack/fix_double_reset)

Fixes and issue where calling reset twice in a row crashes the whole program

Author: pkrack

.github/workflows/cpp.yaml

@@ -26,7 +26,7 @@ jobs:
       - name: Clang build
         run: make clangcompile
       - name: Clang Tidy
-        run: clang-tidy-15 -p=build --warnings-as-errors='*' $(find src -name '*.cpp' -o -name '*.cc' -name '*.h')
+        run: clang-tidy -p=build --warnings-as-errors='*' $(find src -name '*.cpp' -o -name '*.cc' -name '*.h')
 
   build_gcc:
     runs-on: ubuntu-latest

.github/workflows/py.yaml

@@ -28,8 +28,8 @@ jobs:
   pythonpackage:
     runs-on: ubuntu-latest
     env:
-      CC: clang-15
-      CXX: clang++-15
+      CC: clang
+      CXX: clang++
       GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
     steps:
       - uses: actions/checkout@v4

python/tests/test_sim_envs.py

@@ -57,6 +57,20 @@ def assert_collision(self, info: dict):
 class TestSimEnvsTRPY(TestSimEnvs):
     """This class is for testing TRPY sim env functionalities"""
 
+    def test_reset(self, cfg, gripper_cfg, cam_cfg):
+        """
+        Test reset functionality.
+        """
+        # TODO:
+        # - test initial pose after reset.
+        # - test initial gripper config.
+        env = fr3_sim_env(
+            ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=None
+        )
+        # Test double reset. Regression test. A lot can go wrong when resetting.
+        env.reset()
+        env.reset()
+
     def test_zero_action_trpy(self, cfg):
         """
         Test that a zero action does not change the state significantly
@@ -169,6 +183,24 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg):
 class TestSimEnvsTquart(TestSimEnvs):
     """This class is for testing Tquart sim env functionalities"""
 
+    def test_reset(self, cfg, gripper_cfg, cam_cfg):
+        """
+        Test reset functionality.
+        """
+        # TODO:
+        # - test initial pose after reset.
+        # - test initial gripper config.
+        env = fr3_sim_env(
+            ControlMode.CARTESIAN_TQuart,
+            cfg,
+            gripper_cfg=gripper_cfg,
+            camera_set_cfg=cam_cfg,
+            max_relative_movement=None,
+        )
+        # Test double reset. Regression test. A lot can go wrong when resetting.
+        env.reset()
+        env.reset()
+
     def test_non_zero_action_tquart(self, cfg):
         """
         Test that a zero action does not change the state significantly in the tquart configuration
@@ -271,6 +303,20 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg):
 class TestSimEnvsJoints(TestSimEnvs):
     """This class is for testing Joints sim env functionalities"""
 
+    def test_reset(self, cfg, gripper_cfg, cam_cfg):
+        """
+        Test reset functionality.
+        """
+        # TODO:
+        # - test initial pose after reset.
+        # - test initial gripper config.
+        env = fr3_sim_env(
+            ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=None
+        )
+        # Test double reset. Regression test. A lot can go wrong when resetting.
+        env.reset()
+        env.reset()
+
     def test_zero_action_joints(self, cfg):
         """
         This is for testing that a certain action leads to the expected change in state

src/sim/camera.cpp

@@ -51,6 +51,8 @@ int SimCameraSet::buffer_size() {
 }
 void SimCameraSet::clear_buffer() {
   std::lock_guard<std::mutex> lock(buffer_lock);
+  // when we clear the buffer, there is no last image timestep
+  this->last_ts = 0;
   buffer.clear();
 }