diff --git a/.github/workflows/ros-build.yml b/.github/workflows/ros-build.yml index ade49f0a9..3a2c4df83 100644 --- a/.github/workflows/ros-build.yml +++ b/.github/workflows/ros-build.yml @@ -57,6 +57,11 @@ jobs: echo ${skip_packages} fi done + else + # if amd, then clone the mirte_gazebo repo, otherwise skip it, arm doesnt have gazebo compiled (yet) + if [[ "${{ matrix.arch }}" == "amd64" ]]; then + git clone https://github.com/mirte-robot/mirte-gazebo.git ./srcO/mirte_gazebo + fi fi # always skip generate_parameter_library example packages skip_packages+=" generate_parameter_library_example cmake_generate_parameter_module_example generate_parameter_library_example_external generate_parameter_module_example" diff --git a/mirte_telemetrix_cpp/libs/tmx-cpp b/mirte_telemetrix_cpp/libs/tmx-cpp index ba5a4b0c0..dfeeb1bf8 160000 --- a/mirte_telemetrix_cpp/libs/tmx-cpp +++ b/mirte_telemetrix_cpp/libs/tmx-cpp @@ -1 +1 @@ -Subproject commit ba5a4b0c0507977b3c59f98be8918aceceea7ad3 +Subproject commit dfeeb1bf87f8fdad4af704477bb50d5b6b6b9910 diff --git a/mirte_teleop/CMakeLists.txt b/mirte_teleop/CMakeLists.txt index ff17f6652..1ec0c2c75 100644 --- a/mirte_teleop/CMakeLists.txt +++ b/mirte_teleop/CMakeLists.txt @@ -11,7 +11,9 @@ find_package(ament_cmake REQUIRED) # manually. find_package( REQUIRED) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) - +install(PROGRAMS + scripts/mirte_master_arm.py + DESTINATION lib/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights comment the diff --git a/mirte_teleop/README.md b/mirte_teleop/README.md new file mode 100644 index 000000000..6d5e5a2d3 --- /dev/null +++ b/mirte_teleop/README.md @@ -0,0 +1,80 @@ +# Mirte teleop + +Sample teleoperation launch and scripts for MIRTE pioneer and master. + + +## Teleop Key: +This is an ugly hack to get input from the user, the launch file doesn't actually launch. Don't include this one in other launch files! + + +## Teleop joy mm arm: +Default settings are for an PS4 controller directly connected to the robot. +Change includes to use xbox controller (probably) +- drive: + - left stick for forward and rotate + - arrow buttons for move left right + - need to hold L1 for it to move. +- Arm: + - right stick to move the arm. + - Rotation is left-right + - Up down is done with up down of the stick. If the shoulder_lift servo is at the lowest position, the elbow and wrist start to move. + - Open gripper with X, close with O. +- Shutdown robot with options button for 2 seconds. + +## Teleop joy ps4: +PS4 controller works differently internally (/dev/jsX) than before, so joy-linux is required to read those inputs. +Uses L1 (left button) as enable for cmd_vel. + + +## Mirte master arm script: +Controls the arm as described above (mm arm). + + + +# Connecting bluetooth controller directly to robot +Make sure that bluez, teleop-twist-joy, joy and joy-linux are installed on the robot: +```bash +sudo apt install bluez bluetooth ros-humble-teleop-twist-joy ros-humble-joy ros-humble-joy-linux +``` + +Connect bluetooth controller: +```bash +sudo bluetoothctl + scan on + # hold and press share and PS button till fast blink + # find MAC of controller: + devices + pair + connect + trust + exit +``` +The controller should now have a blue bar and there should be a `/dev/js0` file. + + +Sometimes you'll need to restart the bluetooth service after it to auto-connect and show up as `/dev/jsX` + +```bash +sudo systemctl restart bluetooth.service +``` + + + +## Autostart on boot +Add +```python + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("mirte_teleop"), + "launch", + "teleop_joy_mm_arm.launch.py", + ] + ) + ] + ) + ) +``` +to `src/mirte-ros-packages/mirte_bringup/launch/minimal_master.launch.py`, line 300. \ No newline at end of file diff --git a/mirte_teleop/launch/teleop_joy.launch.py b/mirte_teleop/launch/teleop_joy.launch.py index cd51d5eb8..7696459db 100644 --- a/mirte_teleop/launch/teleop_joy.launch.py +++ b/mirte_teleop/launch/teleop_joy.launch.py @@ -6,13 +6,19 @@ def generate_launch_description(): return LaunchDescription( [ + # Node( + # package="joy", + # executable="joy_node", + # name="joy_node", + # parameters=[ + # {"dev": "/dev/input/js0", "deadzone": 0.1, "autorepeat_rate": 20.0} + # ], + # ), Node( - package="joy", - executable="joy_node", - name="joy_node", - parameters=[ - {"dev": "/dev/input/js0", "deadzone": 0.1, "autorepeat_rate": 20.0} - ], + package="joy_linux", + executable="joy_linux_node", + name="joy_linux_node", + parameters=[{"deadzone": 0.1, "autorepeat_rate": 20.0}], ), Node( package="teleop_twist_joy", diff --git a/mirte_teleop/launch/teleop_joy_mm_arm.launch.py b/mirte_teleop/launch/teleop_joy_mm_arm.launch.py new file mode 100644 index 000000000..6f63d8e89 --- /dev/null +++ b/mirte_teleop/launch/teleop_joy_mm_arm.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +# works for ps4 controller +# if other controller, maybe change the included launch file to teleop_joy.launch.py +def generate_launch_description(): + return LaunchDescription( + [ + Node( + package="mirte_teleop", + executable="mirte_master_arm.py", + name="mirte_master_arm", + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("mirte_teleop"), + "launch", + "teleop_joy_ps4.launch.py", + ) + ) + ), + ] + ) diff --git a/mirte_teleop/launch/teleop_joy_ps4.launch.py b/mirte_teleop/launch/teleop_joy_ps4.launch.py new file mode 100644 index 000000000..4ac47ef51 --- /dev/null +++ b/mirte_teleop/launch/teleop_joy_ps4.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +# works for ps4 controller +def generate_launch_description(): + return LaunchDescription( + [ + # Node( + # package="joy", + # executable="joy_node", + # name="joy_node", + # parameters=[ + # {"dev": "/dev/input/js0", "deadzone": 0.1, "autorepeat_rate": 20.0} + # ], + # ), + Node( + package="joy_linux", + executable="joy_linux_node", + name="joy_linux_node", + parameters=[{"deadzone": 0.1, "autorepeat_rate": 20.0}], + ), + Node( + package="teleop_twist_joy", + executable="teleop_node", + name="teleop_joy_node", + parameters=[ + { + "axis_linear.x": 1, + "axis_angular.yaw": 0, + "axis_linear.y": 6, + "scale_linear.x": 1.0, + "scale_linear.y": 1.0, + "scale_angular.yaw": 4.0, + "enable_button": 4, + # 'scale_angular': 1.0 + } + ], + remappings=[("/cmd_vel", "/mirte_base_controller/cmd_vel")], + ), + ] + ) diff --git a/mirte_teleop/package.xml b/mirte_teleop/package.xml index 20c89c023..c6849bde5 100644 --- a/mirte_teleop/package.xml +++ b/mirte_teleop/package.xml @@ -11,9 +11,12 @@ teleop_twist_keyboard diagnostic_updater - + rclpy + geometry_msgs + joy_linux ament_lint_auto ament_lint_common diff --git a/mirte_teleop/scripts/mirte_master_arm.py b/mirte_teleop/scripts/mirte_master_arm.py new file mode 100755 index 000000000..bdfd70bcc --- /dev/null +++ b/mirte_teleop/scripts/mirte_master_arm.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient + +from sensor_msgs.msg import Joy +from mirte_msgs.srv import SetServoAngle +from control_msgs.action import GripperCommand + + +class MirteMasterArm(Node): + def __init__(self): + super().__init__("mirte_master_arm") + + self.joy_sub = self.create_subscription(Joy, "/joy", self.joy_callback, 1) + + self.shoulder_client = self.create_client( + SetServoAngle, "/io/servo/hiwonder/shoulder_lift/set_angle" + ) + self.shoulder_pan_client = self.create_client( + SetServoAngle, "/io/servo/hiwonder/shoulder_pan/set_angle" + ) + self.elbow_client = self.create_client( + SetServoAngle, "/io/servo/hiwonder/elbow/set_angle" + ) + self.wrist_client = self.create_client( + SetServoAngle, "/io/servo/hiwonder/wrist/set_angle" + ) + self.gripper_action_client = ActionClient( + self, GripperCommand, "/mirte_master_gripper_controller/gripper_cmd" + ) + # Right stick: axis 3 = right horizontal, axis 4 = right vertical (may vary by controller) + self.right_axis_horizontal = 5 # rotation + self.right_axis_vertical = 2 # lift + + self.shoulder_angle = 0.0 + self.shoulder_pan_angle = 0.0 + + self.deadzone = 0.1 + self.step = 0.050 # rad per callback + + self.axes = [] + self.buttons = [] + self.timer = self.create_timer(0.1, self.joy_calc) # 10 Hz + self.shutdown_timer = None # Timer for shutdown button press duration + self.get_logger().info("MirteMasterArm node started, listening to /joy") + + def joy_callback(self, msg: Joy): + self.axes = msg.axes + self.buttons = msg.buttons + + def joy_calc(self): + self.calc_gripper() + self.check_shutdown() + axes = self.axes + # print(f"Received Joy message: {msg}") + if len(axes) <= max(self.right_axis_horizontal, self.right_axis_vertical): + self.get_logger().warn("Not enough axes in Joy message") + return + + horiz = axes[self.right_axis_horizontal] + vert = axes[self.right_axis_vertical] + + shoulder_changed = False + shoulder_pan_changed = False + + if abs(horiz) > self.deadzone: + self.shoulder_angle += horiz * -self.step + self.shoulder_angle = max(-6.0, min(1.77, self.shoulder_angle)) + shoulder_changed = True + + if abs(vert) > self.deadzone: + self.shoulder_pan_angle += vert * self.step + self.shoulder_pan_angle = max(-1.7, min(1.7, self.shoulder_pan_angle)) + shoulder_pan_changed = True + + if shoulder_changed: + self.calc_shoulder_angles(self.shoulder_angle) + self.send_servo_request( + self.shoulder_client, self.shoulder_angle, "shoulder" + ) + self.send_servo_request(self.elbow_client, self.elbow_angle, "elbow") + self.send_servo_request(self.wrist_client, self.wrist_angle, "wrist") + + if shoulder_pan_changed: + self.send_servo_request( + self.shoulder_pan_client, self.shoulder_pan_angle, "shoulder_pan" + ) + # print(f"Shoulder angle: {self.shoulder_angle:.2f}, Shoulder pan angle: {self.shoulder_pan_angle:.2f}") + + def send_gripper_request(self, angle): + goal_msg = GripperCommand.Goal() + goal_msg.command.position = angle + goal_msg.command.max_effort = 10.0 + + self.gripper_action_client.wait_for_server() + print(f"Sending gripper command: {angle}") + return self.gripper_action_client.send_goal_async(goal_msg) + + def calc_gripper(self): + if len(self.buttons) < 2: + self.get_logger().warn("Not enough buttons in Joy message") + return + # print(f"Gripper button state: {self.buttons}") + open_button = self.buttons[2] # Assuming button 2 is for opening the gripper + close_button = self.buttons[1] # Assuming button 1 is for closing the gripper + + if open_button and not close_button: + future = self.send_gripper_request(-0.5) # Open gripper + future.add_done_callback(self.goal_response_callback) + elif close_button and not open_button: + future = self.send_gripper_request(0.6) # Close gripper + future.add_done_callback(self.goal_response_callback) + + # rclpy.spin_until_future_complete(self, future) + + def check_shutdown(self): + # if options button is pressed for 2 seconds, shutdown the robot + print(f"Buttons: {self.buttons}") + if len(self.buttons) < 10: + self.get_logger().warn("Not enough buttons in Joy message") + return + + if self.buttons[9] == 1: # Assuming button 9 is the options button + if self.shutdown_timer is None: + self.shutdown_timer = self.create_timer(2.0, self.shutdown_robot) + else: + if self.shutdown_timer is not None: + self.shutdown_timer.cancel() + self.shutdown_timer = None # Reset the timer if the button is released + self.shutdown_timer = None # Reset the timer if the button is released + + def shutdown_robot(self): + # check if node is running on robot, check if /home/mirte exists, if so, shutdown the robot + import os + + if os.path.exists("/home/mirte"): + self.get_logger().info("Shutting down the robot...") + os.system("sudo shutdown now") + else: + self.get_logger().info("Not running on robot, shutting down ROS...") + self.destroy_node() + raise SystemExit("Shutting down ROS...") + rclpy.shutdown() + + def send_servo_request(self, client, angle, name): + if not client.service_is_ready(): + self.get_logger().warn(f"{name} service not available") + return + + req = SetServoAngle.Request() + req.angle = angle + req.degrees = False + future = client.call_async(req) + future.add_done_callback(lambda f: self.service_response_callback(f, name)) + + def calc_shoulder_angles(self, shoulder_angle): + # if shoulder angle is less than -1.66, start moving the elbow to go down. + if shoulder_angle < -1.66: + self.elbow_angle = ( + shoulder_angle + 1.66 + ) * 1.0 # simple linear mapping for demonstration + + else: + self.elbow_angle = 0.0 # (shoulder_angle + 1.66) * 0.5 # simple linear mapping for demonstration + + self.wrist_angle = ( + -self.elbow_angle + ) # simple inverse relationship for demonstration + + def service_response_callback(self, future, name): + try: + out = future.result() + except Exception as e: + self.get_logger().error(f"{name} service call failed: {e}") + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info("Goal rejected :(") + return + + self.get_logger().info("Goal accepted :)") + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info(f"Result: {result}") + + +def main(args=None): + rclpy.init(args=args) + node = MirteMasterArm() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/mirte_test/mirte_test/mirte_master_hw_check.py b/mirte_test/mirte_test/mirte_master_hw_check.py index 1335aca3e..3a5ac31df 100644 --- a/mirte_test/mirte_test/mirte_master_hw_check.py +++ b/mirte_test/mirte_test/mirte_master_hw_check.py @@ -36,7 +36,10 @@ def check_wheels(self): # check service existence motors = ["front_left", "front_right", "rear_left", "rear_right"] for motor in motors: - print("testing motor", motor) + print( + "testing motor %s, should move forward, if not, change configuration for motor and encoder!" + % motor + ) service = "/io/motor/%s/set_speed" % motor # check if service exists if service not in self.all_services: @@ -111,6 +114,14 @@ def update_encoder(msg, m): self.get_logger().error("Encoder %s is not updating" % encoder_topic) self.ok = False continue + # check for direction + if (start_enc - last_encoder) > 0: + self.get_logger().error( + "Encoder %s and motor %s are not moving in the same direction. Change configuration for encoder or motor. It should've moved forward just now." + % (encoder_topic, motor) + ) + self.ok = False + continue # check encoder existence and correct last_odom = None diff --git a/sources.repos b/sources.repos index d6d050911..ce0555ed4 100644 --- a/sources.repos +++ b/sources.repos @@ -22,7 +22,7 @@ repositories: orbbecsdk_ros2: type: git url: https://github.com/orbbec/OrbbecSDK_ROS2.git - version: main # v2-main doesn't support astra mini pro yet + version: v2-main # v2-main doesn't support astra mini pro yet # generate_parameter_library: # type: git # url: https://github.com/ArendJan/generate_parameter_library.git