Merge conflict

uf-mil · Jan 21, 2024 · c2ec9a1 · c2ec9a1
2 parents 740f752 + 97e7f7f
commit c2ec9a1
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 13 deletions.
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -11,15 +11,15 @@ ci:
 
 repos:
   - repo: https://github.com/adrienverge/yamllint.git
-    rev: v1.32.0
+    rev: v1.33.0
     hooks:
       - id: yamllint
   - repo: https://github.com/psf/black
-    rev: 23.10.1
+    rev: 23.12.1
     hooks:
       - id: black
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v17.0.4
+    rev: v17.0.6
     hooks:
       - id: clang-format
   - repo: https://github.com/PyCQA/autoflake
@@ -34,12 +34,13 @@ repos:
         exclude: ^docker|deprecated|NaviGator/simulation/VRX
         args: [--severity=warning, --exclude=SC1090]
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.7.0-1
+    rev: v3.7.0-4
     hooks:
       - id: shfmt
         exclude: ^docker|deprecated|NaviGator/simulation/VRX
   - repo: https://github.com/astral-sh/ruff-pre-commit
-    rev: 'v0.1.4'
+    # Ruff version.
+    rev: 'v0.1.9'
     hooks:
       - id: ruff
         args: [--fix, --exit-non-zero-on-fix]
@@ -77,6 +78,7 @@ repos:
       - id: check-added-large-files
       - id: check-case-conflict
       - id: check-merge-conflict
+        exclude_types: [markdown, rst]
       - id: check-executables-have-shebangs
       - id: check-symlinks
       - id: check-json

diff --git a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_tf.launch b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_tf.launch
@@ -1,5 +1,8 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
+    <!-- Silent unnecessary TF warnings about missing joint states or missing transforms (cred: osrf/vrx#340)-->
+    <rosparam param="silent_tf_failure">true</rosparam>
+
     <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher">
       <param name="use_tf_static" value="false" />
     </node>
@@ -11,9 +14,10 @@
       </rosparam>
     </node>
 
-  <!--Publishes connection between wamv/imu_wamv_link and wamv/base_link. Same for wamv/gps_wamv_link.-->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="imu_base_link_fix" args="0.3 -0.2 1.3 0 0 0 1 wamv/base_link wamv/imu_wamv_link" />
-  <node pkg="tf2_ros" type="static_transform_publisher" name="gps_base_link_fix" args="-0.85 0 1.3 0 0 1 wamv/base_link wamv/ins_link" />
-  <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_enu_fix" args="0 0 0 0 0 0 1 enu wamv/base_link" />
+
+    <!--Publishes connection between wamv/imu_wamv_link and wamv/base_link. Same for wamv/gps_wamv_link.-->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="imu_base_link_fix" args="0.3 -0.2 1.3 0 0 0 1 wamv/base_link wamv/imu_wamv_link" />
+    <node pkg="tf2_ros" type="static_transform_publisher" name="gps_base_link_fix" args="-0.85 0 1.3 0 0 1 wamv/base_link wamv/ins_link" />
+    <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_enu_fix" args="0 0 0 0 0 0 1 enu wamv/base_link" />
 
 </launch>
diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py
@@ -5,6 +5,7 @@
 from .vrx import Vrx
 
 ___author___ = "Alex Perez"
+# Optimized by Daniel Parra
 
 
 class VrxWayfinding2(Vrx):
@@ -42,12 +43,18 @@ async def run(self, parameters):
         poses = poses[:start_pose_index]
         path = path[1:]
 
-        # self.send_feedback('Sorted poses' + str(poses))
         await self.wait_for_task_such_that(lambda task: task.state in ["running"])
 
         # do movements
         for index in path:
             self.send_feedback(f"Going to {poses[index]}")
-
             # Go to goal
+            P = 0.85
+            part_way_point = [x * P for x in poses[index][0][:-1]]
+            part_way_point.append(poses[index][0][-1])
+            self.send_feedback(
+                f"\nPartway:\n{part_way_point}\nEndPoint:\n{poses[index][0]}",
+            )
+
+            await self.send_trajectory_without_path([part_way_point, poses[index][1]])
             await self.send_trajectory_without_path(poses[index])
diff --git a/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py
@@ -8,7 +8,7 @@
 
 
 @dataclass
-class TestPacket(Packet, msg_id=0x47, subclass_id=0x44, payload_format="BHf"):
+class TestPacket(Packet, msg_id=0x47, subclass_id=0x44, payload_format="?Hd"):
     example_bool: bool
     example_int: int
     example_float: float
@@ -23,7 +23,7 @@ def test_simple_packet(self):
         packet = TestPacket(False, 42, 3.14)
         self.assertEqual(packet.msg_id, 0x47)
         self.assertEqual(packet.subclass_id, 0x44)
-        self.assertEqual(packet.payload_format, "BHf")
+        self.assertEqual(packet.payload_format, "?Hd")
         self.assertEqual(packet.example_bool, False)
         self.assertEqual(packet.example_int, 42)
         self.assertEqual(packet.example_float, 3.14)
@@ -36,6 +36,13 @@ def test_assembled_packet(self):
         self.assertEqual(assembled[2], packet.msg_id)
         self.assertEqual(assembled[3], packet.subclass_id)
 
+    def test_format(self):
+        packet = TestPacket(False, 42, 3.14)
+        self.assertEqual(
+            TestPacket.from_bytes(TestPacket.__bytes__(packet)),
+            packet,
+        )
+
     def test_comparisons(self):
         packet = TestPacket(False, 42, 3.14)
         packet_two = TestPacket(False, 42, 3.14)