diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c68974f1..b363cfdf1 100644 --- 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 @@ -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 new file mode 100644 index 000000000..9cc4f5321 --- /dev/null +++ b/action/ArmAction.action @@ -0,0 +1,3 @@ +string name +--- +--- diff --git a/config/teleop.yaml b/config/teleop.yaml index 5aafb02ee..03676355d 100644 --- a/config/teleop.yaml +++ b/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 diff --git a/setup.py b/setup.py index 48ba751d1..e5807a321 100644 --- 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/teleoperation/arm_automation/__init__.py b/src/teleoperation/arm_automation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/teleoperation/arm_automation/arm_automation.py b/src/teleoperation/arm_automation/arm_automation.py new file mode 100755 index 000000000..0bd6afae6 --- /dev/null +++ b/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 diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index fef797f8c..fdc58227f 100644 --- a/src/teleoperation/backend/consumers.py +++ b/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