From b7b15eaba0026bea05388e75863f0eb45d4bfd0a Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Sun, 22 Jan 2023 16:56:50 +0200 Subject: [PATCH] Add comments and clean up --- .../src/cpp/axidraw_controller.cpp | 23 +++- .../src/py/axidraw_serial.py | 112 +++++++++++++++--- src/lite6_controller/src/lite6_controller.cpp | 26 +++- .../robot_controller/robot_controller.hpp | 20 +++- .../src/cpp/dummy_controller.cpp | 11 +- 5 files changed, 164 insertions(+), 28 deletions(-) diff --git a/src/axidraw_controller/src/cpp/axidraw_controller.cpp b/src/axidraw_controller/src/cpp/axidraw_controller.cpp index 69d3095..e434287 100644 --- a/src/axidraw_controller/src/cpp/axidraw_controller.cpp +++ b/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::SharedPtr status_client; @@ -49,7 +52,9 @@ class AxidrawController : public RobotController path_pub = this->create_publisher("axidraw_path", 10); } - // Return true if axidraw is ready + /** + * Return true if axidraw is ready + */ bool is_ready() { auto request = std::make_shared(); @@ -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> 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); diff --git a/src/axidraw_controller/src/py/axidraw_serial.py b/src/axidraw_controller/src/py/axidraw_serial.py index e81f9ec..7213d01 100644 --- a/src/axidraw_controller/src/py/axidraw_serial.py +++ b/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)) diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp index 2900e9c..9ec4440 100644 --- a/src/lite6_controller/src/lite6_controller.cpp +++ b/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 command_list_manager_; // command_list_manager_ = std::make_unique(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(std::move(this)), this->move_group.getRobotModel()); + /** + * Constructor + */ Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options), move_group(std::shared_ptr(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> goal_handle) { @@ -152,6 +171,9 @@ public: } }; +/** + * Starts lite6_controller + */ int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/src/robot_controller/include/robot_controller/robot_controller.hpp b/src/robot_controller/include/robot_controller/robot_controller.hpp index c89c870..3d02e4e 100644 --- a/src/robot_controller/include/robot_controller/robot_controller.hpp +++ b/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 goal_handle); - /// Callback that executes path on robot virtual void executePath(const std::shared_ptr goal_handle); }; using ExecuteMotion = robot_interfaces::action::ExecuteMotion; using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle; +/** + * + */ 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 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 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 goal_handle) { using namespace std::placeholders; @@ -70,7 +84,9 @@ void RobotController::motion_handle_accepted(const std::shared_ptr goal_handle) { const auto goal = goal_handle->get_goal(); diff --git a/src/robot_controller/src/cpp/dummy_controller.cpp b/src/robot_controller/src/cpp/dummy_controller.cpp index 5155817..cbc5cfb 100644 --- a/src/robot_controller/src/cpp/dummy_controller.cpp +++ b/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> 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);