diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index 3d043e6..ac11a5b 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/src/ar3_config/launch/dev.launch.py @@ -6,6 +6,7 @@ from launch_ros.descriptions import ComposableNode from launch.actions import ExecuteProcess import yaml +# Helper functions to make passing parameters less disgusting def get_file(path): with open(path, 'r') as f: return f.read() @@ -16,16 +17,18 @@ def get_yaml(path): def generate_launch_description(): + # Get package locations launch_root = get_package_share_directory('ar3_config') description_root = get_package_share_directory('ar3_description') config_root = join(launch_root, 'config') + # Load files for the urdf(robot model), srdf (semantic model), and the kinematics robot_description = {'robot_description': get_file(join(description_root, 'ar3.urdf'))} robot_description_semantic = {'robot_description_semantic': get_file(join(description_root, 'ar3.srdf'))} robot_description_kinematics = get_yaml(join(config_root, 'kinematics.yaml')) return LaunchDescription([ - # RViz + # Start RViz (ui) Node( package='rviz2', executable='rviz2', @@ -42,6 +45,7 @@ def generate_launch_description(): arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link'] ), + # creates the cpp interface so you can use /move_pose Node( package='move_group_interface', executable='move_group_interface_node', @@ -53,13 +57,14 @@ def generate_launch_description(): get_yaml(join(config_root, "ar3_controllers.yaml"))], ), + # Our joystick node Node( package='joystickmove', executable='move', output='screen', ), - # Create joystick controller + # Create joystick controller and ComposableNodes for servo_server ComposableNodeContainer( name="joystick_container", namespace="/", @@ -88,7 +93,7 @@ def generate_launch_description(): output="screen", ), - # Controller + # Setup the controller manager Node( package='controller_manager', executable='ros2_control_node', @@ -106,7 +111,7 @@ def generate_launch_description(): shell=True, output='screen'), - # Run platform + # Run platform for interface between teensy Node( package='cobot_platform', executable='move', @@ -114,6 +119,7 @@ def generate_launch_description(): ), # Start the actual move_group node/action server + # TODO: AAAAAAAAAAAA Node( package="moveit_ros_move_group", executable="move_group", diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py index 964ff5f..8ccb1b1 100644 --- a/src/cobot_platform/cobot_platform/move.py +++ b/src/cobot_platform/cobot_platform/move.py @@ -12,17 +12,21 @@ from serial import Serial class CobotMove(Node): def __init__(self): super().__init__('cobot_move') + # Set up a logger for the node self._logger = self.get_logger() self._logger.info('COBOT init') + # Connect to the serial port on the teensy self._serial = Serial('/dev/ttyACM0', 115200) + # Set initial joint values to all 0s self._cur = [0.0] * 6 + # create a timer to update all the joint angles at once self._timer = self.create_timer(1 / 40, self._dosomething) + # Set joint angles independantly base_topic = 'move/' - self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data) self.create_subscription(Float32, base_topic+'j2', self._move_j2_cb, rclpy.qos.qos_profile_sensor_data) self.create_subscription(Float32, base_topic+'j3', self._move_j3_cb, rclpy.qos.qos_profile_sensor_data) @@ -32,6 +36,7 @@ class CobotMove(Node): qos = QoSProfile(depth=10) + # Publishing the values so other nodes can read them (so they don't need to keep track of current positions) publish_base_topic = 'update/' self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', qos) self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', qos) @@ -40,9 +45,11 @@ class CobotMove(Node): self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', qos) self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', qos) + # Set the initial position of the robot to 0 self._serial.write(b'STA0.0.1\n') print(self._serial.readline()) + # Callbacks for move/jX subscriptions def _move_j1_cb(self, msg): self._cur[0] = int(msg.data / pi / 2. * 8000 * 4) @@ -61,11 +68,14 @@ class CobotMove(Node): def _move_j6_cb(self, msg): self._cur[5] = int(msg.data / pi / 2. * 2000) + # Send all current positions to the teensy def _dosomething(self): self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8')) + # Get a response from line above of positions line = self._serial.readline().decode('UTF-8') + # Parse the line into each angle blocks = re.findall(r'[ABCDEF]-*\d+', line) a = float(blocks[0][1:]) / 8000. / 4. * pi * 2. b = float(blocks[1][1:]) / 10000 / 4. * pi * 2. @@ -74,6 +84,7 @@ class CobotMove(Node): e = float(blocks[4][1:]) / 2000. * pi * 2. f = float(blocks[5][1:]) / 2000. * pi * 2. + # Publish those to the update/jX subscribers self.s_j1.publish(Float32(data=a)) self.s_j2.publish(Float32(data=b)) self.s_j3.publish(Float32(data=c)) @@ -85,9 +96,11 @@ class CobotMove(Node): def main(args=None): rclpy.init(args=args) + # start the node cobot_move = CobotMove() rclpy.spin(cobot_move) + # kill the node cobot_move.destroy_node() rclpy.shutdown() diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py index f1b9215..5a38712 100644 --- a/src/joystickmove/joystickmove/move.py +++ b/src/joystickmove/joystickmove/move.py @@ -11,6 +11,8 @@ from moveit_msgs.msg import PlanningScene, PlanningSceneWorld from enum import Enum + +# Mappings for a DualShock4 (PS4) controller class Buttons(Enum): CROSS = 0 CIRCLE = 1 @@ -39,35 +41,48 @@ class Axes(Enum): class JoyMove(Node): def __init__(self): super().__init__('joystickmove') + + # Get the logger self._logger = self.get_logger() self._logger.info('joystick init') + # Set up topics for publishing JOY_TOPIC = '/joy' TWIST_TOPIC = '/servo_server/delta_twist_cmds' JOINT_TOPIC = '/servo_server/delta_joint_cmds' + + # Define some constants EE = 'joint_6' BASE = 'base_link' + # Create the needed subscribers and publishers for moveit_servo self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data) self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10); self.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10); self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10); + # Start a moveit_servo client self.servo_client = self.create_client(Trigger, '/servo_server/start_servo') # Check if the a service is available while not self.servo_client.wait_for_service(timeout_sec=1.0): self.get_logger().info('servo_server not available, waiting again...') self._logger.info('servo_server connected.') + # Create a request to tell the server the joystick exists req = Trigger.Request() self.servo_client.call_async(req) + # Callback for any button or stick that is pressed on the joystick def _joy_cb(self, msg): axes = msg.axes buttons = msg.buttons - if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]: + # If you press main buttons or dpad arrows + if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] \ + or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]: + + # combine default values and whatever you have pushed to create a JointJog object joint = JointJog() joint.joint_names = ['joint_1', 'joint_2', @@ -76,39 +91,51 @@ class JoyMove(Node): float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])] joint.duration = 0.1 + # Add a timestamp so we know when the action actually took place joint.header.stamp = self.get_clock().now().to_msg() - joint.header.frame_id = "link_3" + joint.header.frame_id = "link_6" + + # Publish the JointJog to servo_server self.joint_pub.publish(joint) return + # If you pressed a trigger or stick ts = TwistStamped() + # Define linear depth from right stick ts.twist.linear.z = axes[Axes.RIGHT_Y.value] ts.twist.linear.y = axes[Axes.RIGHT_X.value] + # Define linear X from trigger analog values lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1)); lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1)); ts.twist.linear.x = lin_x_right + lin_x_left + # Define angular up and down from left stick ts.twist.angular.y = axes[Axes.LEFT_Y.value] ts.twist.angular.x = axes[Axes.LEFT_X.value] + # Left and Right bumpers to control roll roll_positive = buttons[Buttons.R2.value] roll_negative = -1 * buttons[Buttons.L2.value] ts.twist.angular.z = float(roll_positive + roll_negative) + # Add a timestamp so we know when the action actually took place ts.header.stamp = self.get_clock().now().to_msg() ts.header.frame_id = "base_link" + # Publish the Twist to servo_server self.twist_pub.publish(ts) def main(args=None): rclpy.init(args=args) + # Start the node joy_move = JoyMove() rclpy.spin(joy_move) + # Kill the node joy_move.destroy_node() rclpy.shutdown() diff --git a/src/move_group_interface/src/move_group_interface_node.cpp b/src/move_group_interface/src/move_group_interface_node.cpp index b0179fc..d024a0a 100644 --- a/src/move_group_interface/src/move_group_interface_node.cpp +++ b/src/move_group_interface/src/move_group_interface_node.cpp @@ -15,13 +15,9 @@ const std::string PLANNING_GROUP = "ar3_arm"; class ScanNPlan : public rclcpp::Node { public: ScanNPlan() : Node("scan_n_plan") { - // create the node and start it - // nh_ = std::make_shared(reinterpret_cast(this)); - //nh_ = shared_from_this(); - RCLCPP_INFO(this->get_logger(), "SNP START"); - // create subscriber + // Create /move_pose subscriber pose_sub_ = this->create_subscription( "move_pose", rclcpp::QoS(1), @@ -34,11 +30,14 @@ private: void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) { (void) p; RCLCPP_INFO(this->get_logger(), "GOT MSG"); + // Get a shared pointer to the MoveItCpp library to use our planning scene and getting states auto moveit_cpp_ptr = std::make_shared(shared_from_this()); RCLCPP_INFO(this->get_logger(), "GOT MOVEIT"); //auto psm = std::make_shared("robot_description"); //psm->startSceneMonitor(); + + // ?????? moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); @@ -54,16 +53,17 @@ private: RCLCPP_INFO(this->get_logger(), "SETSTART TO CUR"); planning_components->setStartStateToCurrentState(); - // set the goal to the request pose + // Set the goal to the request pose RCLCPP_INFO(this->get_logger(), "SET GOAL"); planning_components->setGoal(*p, "base_link"); - // actually plan + // Actually plan RCLCPP_INFO(this->get_logger(), "DO PLAN"); auto plan_solution = planning_components->plan(); - // plan is valid + // Plan is valid if (plan_solution) { + // Follow through with the planned path planning_components->execute(); } }