Update main with SAR code

src/athena_sensors/launch/sensors.launch.py

@@ -39,7 +39,7 @@ def generate_launch_description():
     )
     declare_enable_gnss = DeclareLaunchArgument(
         'enable_gnss',
-        default_value='false',
+        default_value='true',
         choices=['true', 'false'],
         description='Enable GNSS fusion in the ZED camera'
     )
@@ -61,7 +61,7 @@ def generate_launch_description():
             'publish_tf':      publish_odom, # this is really the odom tf, idk why its just called tf
             'publish_map_tf':  publish_map,
             'publish_urdf':   'false', # we take care of this elsewhere
-            'enable_gnss':     enable_gnss,
+            'enable_gnss':     'true',
         }.items(),
         condition=UnlessCondition(sim),
     )

src/config/cyclonedds/config_jetson.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config
+https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
+    <Domain Id="any">
+        <General>
+            <Interfaces>
+                <NetworkInterface name="enP8p1s0"/>
+            </Interfaces>
+            <AllowMulticast>false</AllowMulticast>
+        </General>
+        <Discovery>
+            <Peers>
+                <Peer address="192.168.1.1"/>
+                <Peer address="192.168.1.12"/>
+            </Peers>
+            <ParticipantIndex>auto</ParticipantIndex>
+            <MaxAutoParticipantIndex>1000</MaxAutoParticipantIndex>
+        </Discovery>
+        <Internal>
+            <SocketReceiveBufferSize min="10MB"/>
+        </Internal>
+    </Domain>
+</CycloneDDS>

src/config/cyclonedds/config_right_base.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config
+https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
+    <Domain Id="any">
+        <General>
+            <Interfaces>
+                <NetworkInterface name="eno1"/>
+            </Interfaces>
+            <AllowMulticast>false</AllowMulticast>
+        </General>
+        <Discovery>
+            <Peers>
+                <Peer address="192.168.1.12"/>
+                <Peer address="192.168.1.1"/>
+            </Peers>
+            <ParticipantIndex>auto</ParticipantIndex>
+            <MaxAutoParticipantIndex>1000</MaxAutoParticipantIndex>
+        </Discovery>
+        <Internal>
+            <SocketReceiveBufferSize min="10MB"/>
+        </Internal>
+    </Domain>
+</CycloneDDS>

src/description/ros2_control/arm/arm.rmd.ros2_control.xacro

@@ -4,7 +4,7 @@
     <ros2_control name="${name}" type="system">
       <hardware>
         <plugin>rmd_ros2_control/RMDHardwareInterface</plugin>
-        <param name="update_rate">10</param> <!-- Hz -->
+        <param name="update_rate">2</param> <!-- Hz -->
         <param name="logger_rate">5</param> <!-- Hz -->
         <param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
         <param name="can_interface">can0</param>
@@ -33,4 +33,4 @@
       </joint>
     </ros2_control>
   </xacro:macro>
-</robot>
+</robot>

src/description/ros2_control/arm/arm.servo.ros2_control.xacro

@@ -4,7 +4,7 @@
         <ros2_control name="${name}" type="system">
             <hardware>
                 <plugin>servo_ros2_control/SERVOHardwareInterface</plugin>
-                <param name="update_rate">10</param> <!-- Hz -->
+                <param name="update_rate">1</param> <!-- Hz -->
                 <param name="logger_rate">5</param> <!-- Hz -->
                 <param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
                 <param name="can_interface">can0</param>

src/description/ros2_control/arm/arm.smc.ros2_control.xacro

@@ -4,15 +4,15 @@
     <ros2_control name="${name}" type="system">
       <hardware>
         <plugin>smc_ros2_control/SMCHardwareInterface</plugin>
-        <param name="update_rate">10</param> <!-- Hz -->
+        <param name="update_rate">2</param> <!-- Hz -->
         <param name="logger_rate">5</param> <!-- Hz -->
         <param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
         <param name="can_interface">can0</param>
       </hardware>
 
       <joint name="base_yaw">
-        <param name="node_id">0x145</param>
-        <param name="gear_ratio">40</param>
+        <param name="node_id">0x148</param>
+        <param name="gear_ratio">-100</param>
         <param name="operating_velocity">300</param> <!-- motor dps -->
         <command_interface name="position"/>
         <command_interface name="velocity"/>
@@ -23,7 +23,7 @@
       </joint>
 
       <joint name="wrist_pitch">
-        <param name="node_id">0x148</param>
+        <param name="node_id">0x145</param>
         <param name="gear_ratio">40</param>
         <param name="operating_velocity">1000</param> <!-- motor dps -->
         <command_interface name="position"/>

src/description/urdf/athena_arm.urdf.xacro

@@ -45,7 +45,19 @@
   </xacro:if>
   <xacro:unless value="$(arg use_mock_hardware)"> <!-- HARDWARE -->
     <xacro:smc_ros2_control name="smc"/>
-    <xacro:talon_ros2_control name="talon"/> 
+    <ros2_control name="talon_mock" type="system">
+      <hardware>
+        <plugin>mock_components/GenericSystem</plugin>
+      </hardware>
+      <joint name="gripper_claw">
+        <command_interface name="position"/>
+        <command_interface name="velocity"/>
+        <state_interface name="position">
+          <param name="initial_value">-0.0</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+      </joint>
+    </ros2_control>
     <xacro:rmd_ros2_control name="rmd"/>
     <xacro:servo_ros2_control name="servo"/>
     <!-- <xacro:odrive_ros2_control name="odrive"/> -->

src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml

@@ -114,7 +114,7 @@ manual_arm_joint_by_joint_controller:
       - actuator
     # interface_name: velocity
     joint_max_velocities:
-      - 62.83185307 # 10 rot/s, please change dawg
+      - 0.08       # base_yaw
       - 0.05148721  # 2.95 dps
       - 0.05148721
       - 0.785398    # 45 dps

src/subsystems/navigation/aruco_bt/package.xml

@@ -13,7 +13,7 @@
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>cv_bridge</depend>
-  <depend>OpenCV</depend>
+  <depend>libopencv-dev</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
 

src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp

@@ -230,10 +230,7 @@ class PixhawkNode : public rclcpp::Node
     void raw_gps_callback(const Telemetry::RawGps raw_gps) {
         auto msg = sensor_msgs::msg::NavSatFix();
 
-        // convert microsections to nanoseconds for ros time
-        auto unix_epoch_time = std::chrono::microseconds(raw_gps.timestamp_us);
-        auto ros_time = rclcpp::Time(unix_epoch_time.count() * 1000);
-        msg.header.stamp = ros_time;
+        msg.header.stamp = this->now();
         msg.header.frame_id = gps_frame_id_;
 
         msg.latitude = raw_gps.latitude_deg;

src/subsystems/navigation/gps_goal/config/gps_goal_params.yaml

@@ -1,7 +1,7 @@
 gps_goal_server:
   ros__parameters:
-    origin_lat: 38.42391162772634
-    origin_lon: -110.78490558433397
-    origin_alt: 0.0
     map_frame: "map"
+    robot_frame: "base_link"
     default_position_tolerance: 2.0
+    gps_topic: "gps/fix"
+    heading_topic: "heading"