Implement initial axidraw_controller

src/axidraw_controller/CMakeLists.txt

@@ -10,6 +10,29 @@ find_package(ament_cmake REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
+#
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(robot_interfaces REQUIRED)
+find_package(robot_controller REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+find_package(tf2_ros REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+add_executable(axidraw_controller src/cpp/axidraw_controller.cpp)
+ament_target_dependencies(axidraw_controller
+    "rclcpp"
+    "geometry_msgs"
+    "robot_controller"
+    "robot_interfaces"
+)
+
+install(PROGRAMS
+    src/py/axidraw_serial.py
+    DESTINATION lib/${PROJECT_NAME}
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -22,4 +45,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+# Install directories
+#install(DIRECTORY launch rviz urdf worlds DESTINATION share/${PROJECT_NAME})
+
 ament_package()

src/axidraw_controller/README.md

@@ -0,0 +1,10 @@
+# Axidraw controller
+
+## High-level python api
+
+https://axidraw.com/doc/py_api/
+
+## Low-level serial API
+
+Can be used to send commands directly to the Axidraw controller board over serial.
+http://evil-mad.github.io/EggBot/ebb.html

src/axidraw_controller/package.xml

@@ -12,6 +12,12 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>rclcpp</depend>
+  <!-- <depend>robot_interfaces</depend> -->
+  <!-- <depend>robot_controller</depend> -->
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -0,0 +1,175 @@
+#include <cstdio>
+#include <functional>
+#include <memory>
+#include <thread>
+#include <vector>
+#include <chrono>
+
+#include "robot_controller/robot_controller.hpp"
+#include "robot_interfaces/action/execute_motion.hpp"
+#include "robot_interfaces/msg/points.hpp"
+#include "robot_interfaces/srv/status.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "std_msgs/msg/empty.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+using namespace std::chrono_literals;
+// 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;
+  rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr move_pub;
+  rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr penup_pub;
+  rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr pendown_pub;
+  rclcpp::Publisher<robot_interfaces::msg::Points>::SharedPtr path_pub;
+
+  public:
+  AxidrawController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options)
+  {
+    status_client = this->create_client<robot_interfaces::srv::Status>("axidraw_status");
+
+    while (!status_client->wait_for_service(1s))
+    {
+      if (!rclcpp::ok())
+      {
+        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for axidraw_status");
+        //return 0;
+      }
+      RCLCPP_ERROR(this->get_logger(), "Failed to connect to axidraw_status");
+    }
+
+    // Create publishers for axidraw_serial.py topics
+    move_pub = this->create_publisher<geometry_msgs::msg::Point>("axidraw_move", 10);
+    penup_pub = this->create_publisher<std_msgs::msg::Empty>("axidraw_penup", 10);
+    pendown_pub = this->create_publisher<std_msgs::msg::Empty>("axidraw_pendown", 10);
+    path_pub = this->create_publisher<robot_interfaces::msg::Points>("axidraw_path", 10);
+  }
+
+  bool is_ready()
+  {
+    auto request = std::make_shared<robot_interfaces::srv::Status::Request>();
+    request->resource = "motion";
+    auto result = status_client->async_send_request(request);
+
+    result.wait();
+    return result.get()->status == "waiting";
+  }
+
+  // Set limits for A4 paper: 297x210mm
+  float xlim = 297;
+  float ylim = 210;
+
+  float translate(float val, float lmin, float lmax, float rmin, float rmax)
+  {
+    float lspan = lmax - lmin;
+    float rspan = rmax - rmin;
+    val = (val - lmin) / lspan;
+    return rmin + (val * rspan);
+  }
+
+  // Translate a pose to axidraw coordinates (mm)
+  geometry_msgs::msg::Point translate_pose(geometry_msgs::msg::PoseStamped ps)
+  {
+    auto pose = ps.pose;
+    auto point = geometry_msgs::msg::Point();
+
+    point.x = translate(pose.position.x, 0, 1, 0, xlim);
+    point.y = translate(pose.position.y, 0, 1, 0, ylim);
+
+    // Axidraw z-axis has 2 states, pen up is 1, pen down is 0.
+    point.z = pose.position.z > 0 ? 1 : 0;
+
+    return point;
+  }
+
+  std::vector<geometry_msgs::msg::Point> translate_poses(std::vector<geometry_msgs::msg::PoseStamped> ps)
+  {
+    std::vector<geometry_msgs::msg::Point> points;
+    for (auto p : ps)
+      points.push_back(translate_pose(p));
+    return points;
+  }
+
+
+  /// 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");
+    rclcpp::Rate loop_rate(20);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
+    auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
+
+    //for (auto p : goal->motion.path)
+    //{
+    //  auto pp = p.pose;
+    //  RCLCPP_INFO(this->get_logger(), "Position x:%f y:%f z:%f",pp.position.x,pp.position.y,pp.position.z);
+    //  RCLCPP_INFO(this->get_logger(), "Orientation w:%f x:%f y:%f z:%f", pp.orientation.w, pp.orientation.x,pp.orientation.y,pp.orientation.z);
+    //  //  W:%f X:%f Y:%f Z:%f
+    //}
+
+    auto points = translate_poses(goal->motion.path);
+
+    for (long unsigned int i = 0; (i < points.size()) && rclcpp::ok(); ++i)
+    {
+      // Check if there is a cancel request
+      if (goal_handle->is_canceling())
+      {
+        result->result = feedback->status;
+        goal_handle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "Goal canceled");
+        return;
+      }
+
+      auto p = points[i];
+
+      // Ensure axidraw is not busy
+      while (!is_ready())
+        loop_rate.sleep();
+
+      if (p.z > 0)
+        this->penup_pub->publish(std_msgs::msg::Empty());
+      else
+        this->pendown_pub->publish(std_msgs::msg::Empty());
+
+      // Wait for pen movement to complete
+      while (!is_ready())
+        loop_rate.sleep();
+
+      this->move_pub->publish(p);
+
+      // Update status
+      feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " complete";
+      // Publish feedback
+      goal_handle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), feedback->status.c_str());
+
+    }
+
+    // Check if goal is done
+    if (rclcpp::ok())
+    {
+      result->result = "success";
+      goal_handle->succeed(result);
+      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+    }
+  }
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting axidraw_controller");
+  auto robot = std::make_shared<AxidrawController>();
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(robot);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

src/axidraw_controller/src/py/axidraw_serial.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+from pyaxidraw import axidraw
+
+from robot_interfaces.srv import Status, Points
+from geometry_msgs.msg import Point
+from std_msgs.msg import Empty
+
+import rclpy
+from rclpy.node import Node
+
+
+#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):
+    status = {
+        "serial": "not ready",
+        "motion": "waiting",
+    }
+
+    def init_serial(self, port):
+        self.ad = axidraw.AxiDraw()          # Initialize class
+        self.ad.interactive()                # Enter interactive context
+        self.ad.options.port = port
+        if not self.ad.connect():            # Open serial port to AxiDraw;
+            return False
+        self.ad.options.units = 2            # set working units to mm.
+        self.ad.options.model = 2            # set model to AxiDraw V3/A3
+        self.ad.update()                     # Process changes to options
+        self.status["serial"] = "ready"
+        self.status["motion"] = "ready"
+        return True
+
+    def __init__(self, port="/dev/ttyACM0"):
+        super().__init__('axidraw_serial')
+        self.status_srv = self.create_service(Status, 'axidraw_status', self.get_status)
+
+        while not self.init_serial(port):
+            self.get_logger().error("Failed to connect to axidraw on port:{}".format(port))
+
+        self.move_sub = self.create_subscription(Point, 'axidraw_move', move_callback)
+        self.penup_sub = self.create_subscription(Empty, 'axidraw_penup', penup_callback)
+        self.pendown_sub = self.create_subscription(Empty, 'axidraw_pendown', pendown_callback)
+        self.path_sub = self.create_subscription(Points, 'axidraw_path', stroke_callback)
+
+    def get_status(self, request, response):
+        response.status = status.get(request.resource, "Resource '{}' not found.".format(request.resource))
+        return response
+
+    def set_busy(self):
+        self["motion"] = "busy"
+
+    def set_ready(self):
+        self["motion"] = "ready"
+
+    def move_callback(self, msg):
+        self.set_busy()
+
+        self.get_logger().info("Received move: {}".format(msg))
+
+        self.ad.goto(msg.x,msg.y)
+        self.set_ready()
+
+    def penup_callback(self, msg):
+        self.set_busy()
+
+        self.get_logger().info("Received penup: {}".format(msg))
+
+        self.ad.penup()
+        self.set_ready()
+
+    def pendown_callback(self, msg):
+        self.set_busy()
+
+        self.get_logger().info("Received pendown: {}".format(msg))
+
+        self.ad.pendown()
+        self.set_ready()
+
+    def path_callback(self, msg):
+        self.set_busy()
+
+        self.get_logger().info("Received path: {}".format(msg))
+
+        path = [ [p.x,p.y] for p in msg.points ]
+        self.ad.draw_path(path)
+        self.set_ready()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    axidraw_serial = AxidrawSerial()
+
+    rclpy.spin(axidraw_serial)
+
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/axidraw_controller/src/test.py

@@ -1,14 +0,0 @@
-#!/usr/bin/env python3
-from pyaxidraw import axidraw   # import module
-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 = 1            # set working units to cm.
-ad.options.model = 2            # set model to AxiDraw V3/A3
-                                # Absolute moves follow:
-ad.moveto(10, 10)               # Pen-up move to (10cm, 10cm)
-ad.lineto(20, 10)               # Pen-down move, to (20cm, 10)
-ad.moveto(0, 0)                 # Pen-up move, back to origin.
-ad.disconnect()                 # Close serial port to AxiDraw