Merge remote-tracking branch 'origin/integration' into camera-id

umrover · Apr 27, 2024 · 1c203f1 · 1c203f1
2 parents c295415 + 7dcfcc7
commit 1c203f1
Show file tree

Hide file tree

Showing 60 changed files with 1,147 additions and 496 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -67,15 +67,19 @@ set(MROVER_ROS_DEPENDENCIES
         tf2
         tf2_ros
         tf2_geometry_msgs
+        actionlib_msgs
 )
 
 extract_filenames(msg/*.msg MROVER_MESSAGE_FILES)
 
 extract_filenames(srv/*.srv MROVER_SERVICE_FILES)
 
+extract_filenames(action/*.action MROVER_ACTION_FILES)
+
 set(MROVER_MESSAGE_DEPENDENCIES
         std_msgs
         sensor_msgs
+        actionlib_msgs
 )
 
 set(MROVER_PARAMETERS
@@ -98,7 +102,7 @@ if (ZED_FOUND)
     set(CMAKE_CUDA_STANDARD 17)
     set(CMAKE_CUDA_STANDARD_REQUIRED ON)
     set(CMAKE_CUDA_SEPARABLE_COMPILATION ON)
-    set(CMAKE_CUDA_FLAGS "--diag-suppress 108,68")
+    set(CMAKE_CUDA_FLAGS "--diag-suppress 20012")
     # Jetson Xavier NX/AGX has Volta 7.2 architecture
     # Perception Laptop (A4000, Quadro version of RTX 3080) has Ampere 8.6 architecture
     set(CMAKE_CUDA_ARCHITECTURES 72 86)
@@ -123,6 +127,8 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES})
 
 add_service_files(FILES ${MROVER_SERVICE_FILES})
 
+add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES})
+
 generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES})
 
 generate_dynamic_reconfigure_options(${MROVER_PARAMETERS})

diff --git a/action/ArmAction.action b/action/ArmAction.action
@@ -0,0 +1,3 @@
+string name
+---
+---
diff --git a/ansible/roles/build/files/profiles/ci/config.yaml b/ansible/roles/build/files/profiles/ci/config.yaml
@@ -5,9 +5,9 @@ catkin_make_args: [ ]
 cmake_args:
   - -DCMAKE_BUILD_TYPE=Release
   - -DCMAKE_C_COMPILER=clang-16
-  - -DCMAKE_CXX_FLAGS=-pipe
+  - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0
   - -DCMAKE_CXX_COMPILER=clang++-16
-  - -DCMAKE_CUDA_HOST_COMPILER=clang++-16
+  - -DCMAKE_CUDA_HOST_COMPILER=g++-9
   - -DMROVER_IS_CI=ON
   - -Wno-dev
 devel_layout: linked

diff --git a/ansible/roles/build/files/profiles/debug/config.yaml b/ansible/roles/build/files/profiles/debug/config.yaml
@@ -5,11 +5,11 @@ catkin_make_args: [ ]
 cmake_args:
   - -DCMAKE_BUILD_TYPE=Debug
   - -DCMAKE_C_COMPILER=clang-16
-  - -DCMAKE_CXX_FLAGS=-pipe
+  - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0
   - -DCMAKE_CXX_COMPILER=clang++-16
   - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
   - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache
-  - -DCMAKE_CUDA_HOST_COMPILER=clang++-16
+  - -DCMAKE_CUDA_HOST_COMPILER=g++-9
   - -Wno-dev
 devel_layout: linked
 devel_space: devel

diff --git a/ansible/roles/build/files/profiles/release/config.yaml b/ansible/roles/build/files/profiles/release/config.yaml
@@ -5,11 +5,11 @@ catkin_make_args: [ ]
 cmake_args:
   - -DCMAKE_BUILD_TYPE=Release
   - -DCMAKE_C_COMPILER=clang-16
-  - -DCMAKE_CXX_FLAGS=-march=native -pipe
+  - -DCMAKE_CXX_FLAGS=-march=native -pipe -fgnuc-version=13.1.0
   - -DCMAKE_CXX_COMPILER=clang++-16
   - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
   - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache
-  - -DCMAKE_CUDA_HOST_COMPILER=clang++-16
+  - -DCMAKE_CUDA_HOST_COMPILER=g++-9
   - -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON
   - -Wno-dev
 devel_layout: linked

diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml
@@ -97,6 +97,7 @@
       - libgstreamer1.0-dev
       - libgstreamer-plugins-base1.0-dev
       - vainfo
+      - x264
 
 - name: Install Local APT Packages
   become: True

diff --git a/config/esw.yaml b/config/esw.yaml
@@ -43,7 +43,7 @@ can:
       id: 0x15
     - name: "joint_a"
       bus: 1
-      id: 0x20
+      id: 0x20  
     - name: "joint_b"
       bus: 1
       id: 0x21
@@ -64,10 +64,10 @@ can:
       id: 0x26
     - name: "mast_gimbal_y"
       bus: 3
-      id: 0x28
+      id: 0x27
     - name: "mast_gimbal_z"
       bus: 3
-      id: 0x27
+      id: 0x28
     - name: "sa_x"
       bus: 1
       id: 0x29
@@ -122,10 +122,10 @@ brushless_motors:
       max_velocity: 20.0
       max_torque: 5.0
     joint_a:
-      # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. 
+      # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s.
       # gear ratio is currently at 0.005080 revolutions = 1 meter
-      min_velocity: -0.10  # this means -0.10 meters per second.
-      max_velocity: 0.10
+      min_velocity: -0.05  # this means -0.10 meters per second.
+      max_velocity: 0.05
       limit_0_present: true
       limit_1_present: true
       limit_0_enabled: true
@@ -144,11 +144,11 @@ brushless_motors:
       max_backward_pos: 0.0
       max_torque: 20.0
     joint_c:
-      min_velocity: -0.08 # in terms of output
-      max_velocity: 0.08 # max output shaft speed: 5 rpm (for now)
+      min_velocity: -0.03 # in terms of output
+      max_velocity: 0.03 # max output shaft speed: 5 rpm (for now)
       min_position: -0.125
       max_position: 0.30 # 220 degrees of motion is the entire range
-      max_torque: 20.0
+      max_torque: 200.0
     joint_de_0:
       min_velocity: -5.0
       max_velocity: 5.0
@@ -595,4 +595,4 @@ auton_led_driver:
   baud: 115200
 
 science:
-  shutoff_temp: 50.0f
+  shutoff_temp: 50.0f
diff --git a/config/moteus/joint_c.cfg b/config/moteus/joint_c.cfg
@@ -1363,9 +1363,9 @@ servo.adc_cur_cycles 2
 servo.adc_aux_cycles 47
 servo.pid_dq.kp 0.060606
 servo.pid_dq.ki 102.321388
-servo.pid_position.kp 14.000000
+servo.pid_position.kp 800.000000
 servo.pid_position.ki 0.000000
-servo.pid_position.kd 1.450000
+servo.pid_position.kd 50.000000
 servo.pid_position.iratelimit -1.000000
 servo.pid_position.ilimit 0.000000
 servo.pid_position.max_desired_rate 0.000000

diff --git a/config/teleop.yaml b/config/teleop.yaml
@@ -6,34 +6,24 @@ teleop:
     joint_a:
       multiplier: -1
       slow_mode_multiplier: 0.5
-      invert: True
-      xbox_index: 0
     joint_b:
       multiplier: 1
       slow_mode_multiplier: 1
-      invert: False
-      xbox_index: 1
     joint_c:
-      multiplier: -1
+      multiplier: 1
       slow_mode_multiplier: 0.6
-      invert: False
-      xbox_index: 3
     joint_de_pitch:
-      multiplier: 0.5
+      multiplier: -1
       slow_mode_multiplier: 1
-      invert: True
     joint_de_roll:
-      multiplier: -0.5
+      multiplier: 1
       slow_mode_multiplier: 1
-      invert: True
     allen_key:
       multiplier: 1
       slow_mode_multiplier: 1
-      invert: False
     gripper:
       multiplier: 1
       slow_mode_multiplier: 1
-      invert: True
 
   sa_controls:
     sa_x:
@@ -67,21 +57,32 @@ teleop:
       enabled: true
       multiplier: 1
     twist:
-      multiplier: 0.7
+      multiplier: 0.6
+    tilt:
+      multiplier: 1
 
   xbox_mappings:
-    left_js_x: 0
-    left_js_y: 1
-    right_js_x: 2
-    right_js_y: 3
-    left_trigger: 6
-    right_trigger: 7
+    left_x: 0
+    left_y: 1
+    right_x: 2
+    right_y: 3
     a: 0
     b: 1
     x: 2
     y: 3
     left_bumper: 4
     right_bumper: 5
+    left_trigger: 6
+    right_trigger: 7
+    back: 8
+    forward: 9
+    left_stick_click: 10
+    right_stick_click: 11
+    dpad_up: 12
+    dpad_down: 13
+    dpad_left: 14
+    dpad_right: 15
+    home: 16
 
   joystick_mappings:
     left_right: 0
@@ -119,6 +120,6 @@ teleop:
     bar: ["/tf_static", "/rosout", "/tf"]
 
   ik_multipliers:
-    x: 1
-    y: 1
-    z: 1
+    x: 0.1
+    y: 0.1
+    z: 0.1
diff --git a/launch/basestation.launch b/launch/basestation.launch
@@ -4,14 +4,17 @@
 <launch>
   <arg name="run_rviz" default="false" />
   <arg name="run_network_monitor" default="false" />
+  <arg name="run_backend" default="true" />
+  <arg name="run_frontend" default="true" />
+  <arg name="run_browser" default="true" />
 
   <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
   <rosparam command="load" file="$(find mrover)/config/teleop.yaml" />
   <rosparam command="load" file="$(find mrover)/config/localization.yaml" />
 
-  <node name="gui_frontend" pkg="mrover" type="gui_frontend.sh" cwd="node" required="true" />
-  <node name="gui_backend" pkg="mrover" type="gui_backend.sh" cwd="node" required="true" />
-  <node name="gui_chromium" pkg="mrover" type="gui_chromium.sh" />
+  <node if="$(arg run_frontend)" name="gui_frontend" pkg="mrover" type="gui_frontend.sh" cwd="node" required="true" />
+  <node if="$(arg run_backend)" name="gui_backend" pkg="mrover" type="gui_backend.sh" cwd="node" required="true" />
+  <node if="$(arg run_browser)" name="gui_chromium" pkg="mrover" type="gui_chromium.sh" />
 
   <group if="$(arg run_rviz)">
     <arg name="rvizconfig" default="$(find mrover)/config/rviz/basestation.rviz" />

diff --git a/launch/esw_base.launch b/launch/esw_base.launch
@@ -17,19 +17,11 @@
             output="screen">
             <param name="interface" value="can2" />
       </node>
-      <!-- <node name="can_driver_3" pkg="mrover" type="can_driver"
+      <node name="can_driver_3" pkg="mrover" type="can_driver"
             respawn="true" respawn_delay="2"
             output="screen">
             <param name="interface" value="can3" />
-      </node> -->
-
-      <!-- <node pkg="mrover" type="gst_websocket_streamer" name="gst_websocket_streamer_0"
-            output="screen">
-            <param name="image_topic" value="/camera/left/image" />
-            <param name="port" value="8081" />
-            <param name="width" value="1280" />
-            <param name="height" value="720" />
-      </node> -->
+      </node>
 
       <node pkg="mrover" type="gst_websocket_streamer" name="gst_websocket_streamer_0"
             output="screen">
@@ -47,15 +39,23 @@
             <param name="height" value="720" />
       </node>
 
-      <!-- <node pkg="mrover" type="gst_websocket_streamer" name="gst_websocket_streamer_2"
+      <node pkg="mrover" type="gst_websocket_streamer" name="gst_websocket_streamer_2"
             output="screen">
             <param name="device" value="/dev/video4" />
             <param name="port" value="8083" />
-            <param name="width" value="640" />
-            <param name="height" value="480" />
+            <param name="width" value="1280" />
+            <param name="height" value="720" />
+      </node>
+
+      <node pkg="mrover" type="gst_websocket_streamer" name="gst_websocket_streamer_zed"
+            output="screen">
+            <param name="image_topic" value="/camera/left/image" />
+            <param name="port" value="8084" />
+            <param name="width" value="1280" />
+            <param name="height" value="720" />
       </node>
 
-      <node pkg="mrover" type="gst_websocket_streamer" name="gst_websocket_streamer_3"
+      <!-- <node pkg="mrover" type="gst_websocket_streamer" name="gst_websocket_streamer_3"
             output="screen">
             <param name="device" value="/dev/video6" />
             <param name="port" value="8084" />

diff --git a/launch/simulator.launch b/launch/simulator.launch
@@ -17,6 +17,11 @@
     <param name="headless" value="$(arg headless)"/>
   </node>
 
+  <node pkg="nodelet" type="nodelet" name="streamer_nodelet"
+        args="load mrover/GstWebsocketStreamerNodelet perception_nodelet_manager" output="screen">
+    <param name="image_topic" value="/camera/left/image"/>
+  </node>
+
   <node name="arm_controller" pkg="mrover" type="arm_controller" output="screen"/>
 
   <group if="$(arg run_rviz)">

diff --git a/scripts/debug_arm_ik.py b/scripts/debug_arm_ik.py
@@ -1,7 +1,8 @@
 #!/usr/bin/env python3
 
 import rospy
-from geometry_msgs.msg import Pose, Quaternion, Point, PointStamped
+from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, PointStamped
+from std_msgs.msg import Header
 from mrover.msg import IK
 
 if __name__ == "__main__":
@@ -13,18 +14,24 @@
 
     pub.publish(
         IK(
-            pose=Pose(
-                position=Point(x=0.5, y=0.5, z=0.5),
-                orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+            target=PoseStamped(
+                header=Header(stamp=rospy.Time.now(), frame_id="base_link"),
+                pose=Pose(
+                    position=Point(x=0.8, y=1.0, z=0.5),
+                    orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+                ),
             )
         )
     )
 
     def on_clicked_point(clicked_point: PointStamped):
         ik = IK(
-            pose=Pose(
-                position=clicked_point.point,
-                orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+            target=PoseStamped(
+                header=Header(stamp=rospy.Time.now(), frame_id="base_link"),
+                pose=Pose(
+                    position=clicked_point.point,
+                    orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+                ),
             )
         )
         pub.publish(ik)

diff --git a/setup.py b/setup.py
@@ -13,6 +13,7 @@
         "navigation.failure_identification",
         "esw",
         "teleoperation.teleoperation",
+        "teleoperation.arm_controller",
     ],
     package_dir={"": "src"},
 )

diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp
@@ -43,7 +43,8 @@ auto main(int argc, char** argv) -> int {
 }
 
 void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) {
-    // See 11.5.1 in "Controls Engineering in the FIRST Robotics Competition" for the math
+    // See 13.3.1.4 in "Modern Robotics" for the math
+    // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf
     auto forward = MetersPerSecond{msg->linear.x};
     auto turn = RadiansPerSecond{msg->angular.z};
     // TODO(quintin)    Don't ask me to explain perfectly why we need to cancel out a meters unit in the numerator