Added comments

ar3_config launch file
cobot_platform move.py
joystick move.py
mgi cpp file

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",

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()
 

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()
 

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<Node>(reinterpret_cast<Node *>(this));
-	//nh_ = shared_from_this();
-
         RCLCPP_INFO(this->get_logger(), "SNP START");
 
-        // create subscriber
+        // Create /move_pose subscriber
         pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
             "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<moveit_cpp::MoveItCpp>(shared_from_this());
     	RCLCPP_INFO(this->get_logger(), "GOT MOVEIT");
 
         //auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("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();
         }
     }