Add arm automation init

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
@@ -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})

action/ArmAction.action

@@ -0,0 +1,3 @@
+string name
+---
+---

config/teleop.yaml

@@ -74,14 +74,23 @@ teleop:
     left_js_y: 1
     right_js_x: 2
     right_js_y: 3
-    left_trigger: 6
-    right_trigger: 7
     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

setup.py

@@ -13,6 +13,7 @@
         "navigation.failure_identification",
         "esw",
         "teleoperation.teleoperation",
+        "teleoperation.arm_controller",
     ],
     package_dir={"": "src"},
 )

src/teleoperation/arm_automation/arm_automation.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+
+import rospy
+
+import actionlib
+
+from mrover.msg import ArmActionAction, ArmActionGoal, ArmActionResult, Position
+
+from sensor_msgs.msg import JointState
+
+
+def arm_automation() -> None:
+    rospy.init_node("arm_automation")
+
+    server = None
+    joint_state = None
+
+    pos_pub = rospy.Publisher("arm_position_cmd", Position, queue_size=1)
+
+    def joint_state_callback(msg: JointState):
+        nonlocal joint_state
+        joint_state = msg
+
+    rospy.Subscriber("joint_states", JointState, joint_state_callback)
+
+    def execute_callback(goal: ArmActionGoal):
+        success = True
+
+        rate = rospy.Rate(20)
+        while not rospy.is_shutdown():
+            if server.is_preempt_requested():
+                server.set_preempted()
+                success = False
+                break
+
+            if goal.name == "de_home":
+                pos_pub.publish(Position(names=["joint_de_pitch", "joint_de_roll"], position=[0, 0]))
+                if joint_state is None:
+                    success = False
+                    break
+
+                if abs(joint_state.position[0]) < 0.1 and abs(joint_state.position[1]) < 0.1:
+                    break
+
+            rate.sleep()
+
+        if success:
+            server.set_succeeded(ArmActionResult())
+        else:
+            server.set_aborted(ArmActionResult())
+
+    server = actionlib.SimpleActionServer("arm_action", ArmActionAction, execute_cb=execute_callback, auto_start=False)
+    server.start()
+
+
+if __name__ == "__main__":
+    try:
+        arm_automation()
+    except rospy.ROSInterruptException:
+        pass

src/teleoperation/backend/consumers.py

@@ -13,8 +13,12 @@
 from backend.models import AutonWaypoint, BasicWaypoint
 from geometry_msgs.msg import Twist, Pose, Point, Quaternion, Vector3, PoseStamped
 
+import actionlib
+
 # from cv_bridge import CvBridge
 from mrover.msg import (
+    ArmActionAction,
+    ArmActionGoal,
     PDLB,
     ControllerState,
     GPSWaypoint,
@@ -381,17 +385,26 @@ def handle_controls_message(self, msg):
         elif msg["type"] == "cache_values":
             names = ["cache"]
 
-        if msg["arm_mode"] == "ik":
-            self.publish_ik(msg["axes"], msg["buttons"])
+        if msg["buttons"][self.xbox_mappings["home"]] > 0.5:
+            client = actionlib.SimpleActionClient("arm_action", ArmActionAction)
+            client.wait_for_server()
+
+            goal = ArmActionGoal(name="de_home")
+            client.send_goal(goal)
+
+            client.wait_for_result()
+        else:
+            if msg["arm_mode"] == "ik":
+                self.publish_ik(msg["axes"], msg["buttons"])
 
-        elif msg["arm_mode"] == "position":
-            self.publish_position(type=msg["type"], names=names, positions=msg["positions"])
+            elif msg["arm_mode"] == "position":
+                self.publish_position(type=msg["type"], names=names, positions=msg["positions"])
 
-        elif msg["arm_mode"] == "velocity":
-            self.publish_velocity(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"])
+            elif msg["arm_mode"] == "velocity":
+                self.publish_velocity(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"])
 
-        elif msg["arm_mode"] == "throttle":
-            self.publish_throttle(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"])
+            elif msg["arm_mode"] == "throttle":
+                self.publish_throttle(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"])
 
     def handle_joystick_message(self, msg):
         # Tiny deadzone so we can safely e-stop with dampen switch