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

26
src/lite6_controller/src/lite6_controller.cpp
View File

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

20
src/robot_controller/include/robot_controller/robot_controller.hpp
View File

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

11
src/robot_controller/src/cpp/dummy_controller.cpp
View File

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