Create launch file for axidraw

src/axidraw_controller/CMakeLists.txt

@@ -10,6 +10,34 @@ 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(TARGETS
+  axidraw_controller
+  DESTINATION lib/${PROJECT_NAME})
+
+install(PROGRAMS
+    src/py/axidraw_serial.py
+    DESTINATION lib/${PROJECT_NAME}
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -22,4 +50,8 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+# Install directories
+#install(DIRECTORY launch rviz urdf worlds DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch 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/launch/axidraw_controller.launch.py

@@ -0,0 +1,47 @@
+import os
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+from ament_index_python import get_package_share_directory
+from launch.launch_description_sources import load_python_launch_file_as_module
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler, EmitEvent
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+
+
+def launch_setup(context, *args, **kwargs):
+    serial_port = LaunchConfiguration('serial_port', default='/dev/ttyACM0')
+    log_level = LaunchConfiguration("log_level", default='warn')
+
+    nodes = [
+        Node(
+            package="axidraw_controller",
+            executable="axidraw_controller",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+            ],
+        ),
+        Node(
+            package="axidraw_controller",
+            executable="axidraw_serial.py",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+                {"serial_port": serial_port},
+            ],
+        ),
+    ]
+
+    return nodes
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

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,122 @@
+#!/usr/bin/env python3
+from pyaxidraw import axidraw
+
+from robot_interfaces.srv import Status
+from robot_interfaces.msg import Points
+from geometry_msgs.msg import Point
+from std_msgs.msg import Empty
+
+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):
+    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):
+        super().__init__('axidraw_serial')
+
+        self.declare_parameter('serial_port', '/dev/ttyACM0')
+        port = self.get_parameter('serial_port').get_parameter_value().string_value
+
+        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', self.move_callback, qos_profile=QoSProfile(depth=1))
+        self.penup_sub = self.create_subscription(Empty, 'axidraw_penup', self.penup_callback, qos_profile=QoSProfile(depth=1))
+        self.pendown_sub = self.create_subscription(Empty, 'axidraw_pendown', self.pendown_callback, qos_profile=QoSProfile(depth=1))
+        self.path_sub = self.create_subscription(Points, 'axidraw_path', self.stroke_callback, qos_profile=QoSProfile(depth=1))
+
+    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 stroke_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,13 +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/ttyAXI"
-if not ad.connect():            # Open serial port to AxiDraw;
-    quit()                      #   Exit, if no connection.
-ad.options.units = 1            # set working units to cm.
-                                # Absolute moves follow:
-ad.moveto(1, 1)                 # Pen-up move to (1 inch, 1 inch)
-ad.lineto(2, 1)                 # Pen-down move, to (2 inch, 1 inch)
-ad.moveto(0, 0)                 # Pen-up move, back to origin.
-ad.disconnect()                 # Close serial port to AxiDraw

src/drawing_controller/README.md

@@ -0,0 +1,10 @@
+# Drawing controller
+
+This package handles translating an SVG image into a series of motion messages (robot_interfaces/msg/Motion.msg) and sending them to a robot_controller.
+
+## Coordinates
+The origin (0,0,0) of the motion message coordinates is the top left corner of an A4 sheet with the pen in contact with the page, (1,1,0) is the bottom right corner.
+
+Raising the pen from the page is indicated with positive z-axis values. (0,0,1) raises the pen 210mm above the paper
+
+Negative z-values push the pen deeper into the page (if possible on the robot).

src/robot_controller/CMakeLists.txt

@@ -18,11 +18,11 @@ find_package(robot_interfaces REQUIRED)
 include_directories(include)
 
 add_library(robot_controller src/cpp/robot_controller.cpp)
-add_executable(dummy_controller src/cpp/dummy_controller.cpp)
 ament_target_dependencies(robot_controller
   "rclcpp"
   "rclcpp_action"
   "robot_interfaces")
+add_executable(dummy_controller src/cpp/dummy_controller.cpp)
 ament_target_dependencies(dummy_controller
   "rclcpp"
   "rclcpp_action"

src/robot_interfaces/CMakeLists.txt

@@ -23,14 +23,17 @@ if(BUILD_TESTING)
 endif()
 
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(rclcpp REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Motion.msg"
+  "msg/Points.msg"
   "srv/ExecuteMotion.srv"
+  "srv/Status.srv"
   "action/ExecuteMotion.action"
-  DEPENDENCIES geometry_msgs # Add packages that above messages depend on
+  DEPENDENCIES geometry_msgs std_msgs # Add packages that above messages depend on
 )
 
 ament_package()

src/robot_interfaces/msg/Motion.msg

@@ -1,5 +1,5 @@
 # A single motion, consisting of a list of timestamped points
 
 geometry_msgs/PoseStamped[] path
+float64[] velocity
 float64 acceleration
-float64 velocity

src/robot_interfaces/msg/Points.msg

@@ -0,0 +1,3 @@
+# A list of points
+
+geometry_msgs/Point[] points

src/robot_interfaces/package.xml

@@ -14,6 +14,7 @@
 
   <depend>geometry_msgs</depend>
   <depend>action_msgs</depend>
+  <depend>std_msgs</depend>
 
   <build_depend>rosidl_default_generators</build_depend>
 

src/robot_interfaces/srv/Status.srv

@@ -0,0 +1,6 @@
+# Status service
+# Provide a resource and recieve the status of that resource
+
+string resource
+---
+string status