nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add comments and clean up

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-01-22 16:56:50 +02:00
parent 35d20e38ea
commit b7b15eaba0
5 changed files with 164 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/axidraw_controller/src/cpp/axidraw_controller.cpp
View File

@@ -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);

112
src/axidraw_controller/src/py/axidraw_serial.py
View File

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