Add comments and clean up

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -17,7 +17,10 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 using namespace std::chrono_literals;
-// Controller for the axidraw robot. Calls axidraw python API over ROS2 topics
+
+/**
+ * Controller for the axidraw robot. Calls axidraw python API over ROS2 topics
+ */
 class AxidrawController : public RobotController
 {
   rclcpp::Client<robot_interfaces::srv::Status>::SharedPtr status_client;
@@ -49,7 +52,9 @@ class AxidrawController : public RobotController
     path_pub = this->create_publisher<robot_interfaces::msg::Points>("axidraw_path", 10);
   }
 
-  // Return true if axidraw is ready
+  /**
+   * Return true if axidraw is ready
+   */
   bool is_ready()
   {
     auto request = std::make_shared<robot_interfaces::srv::Status::Request>();
@@ -66,6 +71,9 @@ class AxidrawController : public RobotController
   float xlim = 297;
   float ylim = 210;
 
+  /**
+   * Function that translates an input value with a given range to a value within another range.
+   */
   float translate(float val, float lmin, float lmax, float rmin, float rmax)
   {
     float lspan = lmax - lmin;
@@ -74,7 +82,9 @@ class AxidrawController : public RobotController
     return rmin + (val * rspan);
   }
 
-  // Translate a pose to axidraw coordinates (mm)
+  /**
+   * Translate a pose to axidraw coordinates (mm)
+   */
   geometry_msgs::msg::Point translate_pose(geometry_msgs::msg::PoseStamped ps)
   {
     auto pose = ps.pose;
@@ -99,7 +109,9 @@ class AxidrawController : public RobotController
   }
 
 
-  /// Callback that executes path on robot
+  /**
+   * Callback that executes path on robot
+   */
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
     RCLCPP_INFO(this->get_logger(), "Executing goal");
@@ -172,6 +184,9 @@ class AxidrawController : public RobotController
   }
 };
 
+/**
+ *
+ */
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

src/axidraw_controller/src/py/axidraw_serial.py

@@ -10,32 +10,49 @@ import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile
 
-
-#TODO delete this
-class Test():
-    def __init__():
-        ad = axidraw.AxiDraw()          # Initialize class
-        ad.interactive()                # Enter interactive context
-        ad.options.port = "/dev/ttyACM0"
-        if not ad.connect():            # Open serial port to AxiDraw;
-            quit()                      #   Exit, if no connection.
-        ad.options.units = 2            # set working units to mm.
-        ad.options.model = 2            # set model to AxiDraw V3/A3
-        # Absolute moves follow:
-        ad.moveto(10, 10)               # Pen-up move to (10mm, 10mm)
-        ad.lineto(20, 10)               # Pen-down move, to (20mm, 10)
-        ad.moveto(0, 0)                 # Pen-up move, back to origin.
-        ad.disconnect()                 # Close serial port to AxiDraw
-
-
 #TODO handle serial disconnect
+
 class AxidrawSerial(Node):
+    """
+    Class for forwarding axidraw python API functions to ROS2 topics.
+
+    ...
+
+    Attributes
+    ----------
+    status : dict
+        contains the robot's status
+
+    Methods
+    -------
+    init_serial(port)
+
+    Services
+    -------
+    Status, 'axidraw_status'
+
+    Topics
+    -------
+    Point, 'axidraw_move'
+    Empty, 'axidraw_penup'
+    Empty, 'axidraw_pendown'
+    Points, 'axidraw_path'
+    """
+
     status = {
         "serial": "not ready",
         "motion": "waiting",
     }
 
     def init_serial(self, port):
+        '''
+        Initiates connection to axidraw over serial.
+
+            Parameters:
+                    port (string): The serial port or path to serial device (example: "/dev/ttyACM0")
+            Returns:
+                    False if connection failed and True if it succeeded.
+        '''
         self.ad = axidraw.AxiDraw()          # Initialize class
         self.ad.interactive()                # Enter interactive context
         self.ad.options.port = port
@@ -49,6 +66,15 @@ class AxidrawSerial(Node):
         return True
 
     def __init__(self):
+        """
+        Sets up a connection to the axidraw robot and starts the 'axidraw_***' services and topic subscriptions.
+        Retries connection to axidraw if it fails.
+        Fetches port from ROS2 parameter 'serial_port', defaulting to '/dev/ttyACM0'.
+
+        Parameters
+        ----------
+        """
+
         super().__init__('axidraw_serial')
 
         self.declare_parameter('serial_port', '/dev/ttyACM0')
@@ -65,28 +91,63 @@ class AxidrawSerial(Node):
         self.path_sub = self.create_subscription(Points, 'axidraw_path', self.stroke_callback, qos_profile=QoSProfile(depth=1))
 
     def get_status(self, request, response):
+        '''
+        Looks up the status of the requested resource, sets it as the response status and returns it.
+        Used directly by the "axidraw_status" service.
+
+            Parameters:
+                    request (robot_interfaces.srv.Status.request): The request
+                    response (robot_interfaces.srv.Status.response): The response
+            Returns:
+                    response (robot_interfaces.srv.Status.response): The response with the data added. If not found returns "Resource 'X' not found.".
+        '''
         response.status = self.status.get(request.resource, "Resource '{}' not found.".format(request.resource))
         return response
 
     def go_home(self):
+        '''
+        Moves the robot to (0,0).
+            Parameters:
+            Returns:
+        '''
         self.status["motion"] = "busy"
         if self.status["serial"] == "ready":
             self.ad.moveto(0,0)
         self.status["motion"] = "ready"
 
     def set_busy(self):
+        '''
+        Sets the robot motion to "busy"
+            Parameters:
+            Returns:
+        '''
         self.status["motion"] = "busy"
 
     def set_ready(self):
+        '''
+        Sets the robot motion to "ready"
+            Parameters:
+            Returns:
+        '''
         self.status["motion"] = "ready"
 
     def wait_ready(self):
+        '''
+        Sets the robot motion to "ready"
+            Parameters:
+            Returns:
+        '''
         rate = self.create_rate(2) #2Hz
         while self.status["motion"] != "ready":
             rate.sleep()
             pass
 
     def move_callback(self, msg):
+        '''
+        Callback for axidraw_move topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received move: {}".format(msg))
@@ -95,6 +156,11 @@ class AxidrawSerial(Node):
         self.set_ready()
 
     def penup_callback(self, msg):
+        '''
+        Callback for axidraw_penup topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received penup: {}".format(msg))
@@ -103,6 +169,11 @@ class AxidrawSerial(Node):
         self.set_ready()
 
     def pendown_callback(self, msg):
+        '''
+        Callback for axidraw_pendown topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received pendown: {}".format(msg))
@@ -111,6 +182,11 @@ class AxidrawSerial(Node):
         self.set_ready()
 
     def stroke_callback(self, msg):
+        '''
+        Callback for axidraw_stroke topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received path: {}".format(msg))

src/lite6_controller/src/lite6_controller.cpp

@@ -29,10 +29,17 @@ const std::string MOVE_GROUP = "lite6";
 // https://moveit.picknik.ai/humble/doc/examples/motion_planning_api/motion_planning_api_tutorial.html
 // https://github.com/ros-planning/moveit2/blob/main/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp
 //
+//
+
+/**
+ * Controller for xArm Lite6, implementing RobotController
+ */
 class Lite6Controller : public RobotController
 {
 public:
-  /// Move group interface for the robot
+  /**
+   * Move group interface for the robot
+   */
   moveit::planning_interface::MoveGroupInterface move_group;
 
   //bool moved = false;
@@ -40,9 +47,16 @@ public:
   // TODO get pilz working
   //std::unique_ptr<pilz_industrial_motion_planner::CommandListManager> command_list_manager_;
   // command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
+
+  /**
+   * CommandListManager, used to plan MotionSequenceRequest
+   */
   pilz_industrial_motion_planner::CommandListManager command_list_manager;
   //pilz_industrial_motion_planner::CommandListManager command_list_manager(*std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
 
+  /**
+   * Constructor
+   */
   Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
     : RobotController(options),
       move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP),
@@ -63,6 +77,9 @@ public:
 
   }
 
+  /**
+   * This function takes a pose and returns a MotionPlanRequest
+   */
   planning_interface::MotionPlanRequest createRequest(geometry_msgs::msg::PoseStamped pose)
   {
     planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
@@ -97,7 +114,9 @@ public:
   // https://answers.ros.org/question/330632/moveit-motion-planning-how-should-i-splice-several-planned-motion-trajectories/
   // https://groups.google.com/g/moveit-users/c/lZL2HTjLu-k
 
-  /// Callback that executes path on robot
+  /**
+   * Callback that executes path on robot
+   */
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
 
@@ -152,6 +171,9 @@ public:
   }
 };
 
+/**
+ * Starts lite6_controller
+ */
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

src/robot_controller/include/robot_controller/robot_controller.hpp

@@ -6,6 +6,9 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
 
+/**
+ * Class which creates the ExecuteMotion action which the drawing_controller uses to talk to the robots.
+ */
 class RobotController : public rclcpp::Node
 {
 public:
@@ -26,13 +29,15 @@ private:
 
   virtual void motion_handle_accepted(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle);
 
-  /// Callback that executes path on robot
   virtual void executePath(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle);
 };
 
 using ExecuteMotion = robot_interfaces::action::ExecuteMotion;
 using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle<ExecuteMotion>;
 
+/**
+ *
+ */
 RobotController::RobotController(const rclcpp::NodeOptions & options)
 : Node("robot_controller",options)
 {
@@ -46,6 +51,9 @@ RobotController::RobotController(const rclcpp::NodeOptions & options)
     std::bind(&RobotController::motion_handle_accepted, this, _1));
 }
 
+/**
+ *
+ */
 rclcpp_action::GoalResponse RobotController::motion_handle_goal(
   const rclcpp_action::GoalUUID & uuid,
   std::shared_ptr<const ExecuteMotion::Goal> goal)
@@ -55,6 +63,9 @@ rclcpp_action::GoalResponse RobotController::motion_handle_goal(
   return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
 
+/**
+ *
+ */
 rclcpp_action::CancelResponse RobotController::motion_handle_cancel(
   const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
 {
@@ -63,6 +74,9 @@ rclcpp_action::CancelResponse RobotController::motion_handle_cancel(
   return rclcpp_action::CancelResponse::ACCEPT;
 }
 
+/**
+ *
+ */
 void RobotController::motion_handle_accepted(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
 {
   using namespace std::placeholders;
@@ -70,7 +84,9 @@ void RobotController::motion_handle_accepted(const std::shared_ptr<GoalHandleExe
   std::thread{std::bind(&RobotController::executePath, this, _1), goal_handle}.detach();
 }
 
-/// Callback that executes path on robot
+/**
+ * Callback that executes path on robot
+ */
 void RobotController::executePath(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
 {
   const auto goal = goal_handle->get_goal();

src/robot_controller/src/cpp/dummy_controller.cpp

@@ -9,13 +9,17 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
 
-// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
+/**
+ * A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
+ */
 class DummyController : public RobotController
 {
   public:
   DummyController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options) {}
 
-  /// Callback that executes path on robot
+  /**
+   * Callback that executes path on robot
+   */
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
     RCLCPP_INFO(this->get_logger(), "Executing goal");
@@ -58,6 +62,9 @@ class DummyController : public RobotController
   }
 };
 
+/**
+ *
+ */
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);