From ee4e82f3b6307d6e0079f0e420a2753c06e83c4b Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Mon, 12 Dec 2022 13:48:43 +0200 Subject: [PATCH] Restructure codebase --- .docker/run.bash | 8 +- README.md | 16 +++ src/axidraw_controller/CMakeLists.txt | 25 ++++ src/axidraw_controller/package.xml | 18 +++ src/axidraw_controller/{ => src}/test.py | 1 + src/draw_svg/src/py/draw_svg.py | 4 +- src/drawing_controller/CMakeLists.txt | 25 ++++ src/drawing_controller/package.xml | 18 +++ .../src/drawing_controller.py | 110 ++++++++++++++++++ src/lite6_controller/CMakeLists.txt | 25 ++++ src/lite6_controller/package.xml | 18 +++ src/robot_controller/CMakeLists.txt | 37 ++++++ src/robot_controller/package.xml | 18 +++ src/robot_controller/src/robotcontroller.cpp | 41 +++++++ 14 files changed, 360 insertions(+), 4 deletions(-) create mode 100644 src/axidraw_controller/CMakeLists.txt create mode 100644 src/axidraw_controller/package.xml rename src/axidraw_controller/{ => src}/test.py (95%) create mode 100644 src/drawing_controller/CMakeLists.txt create mode 100644 src/drawing_controller/package.xml create mode 100644 src/drawing_controller/src/drawing_controller.py create mode 100644 src/lite6_controller/CMakeLists.txt create mode 100644 src/lite6_controller/package.xml create mode 100644 src/robot_controller/CMakeLists.txt create mode 100644 src/robot_controller/package.xml create mode 100644 src/robot_controller/src/robotcontroller.cpp diff --git a/.docker/run.bash b/.docker/run.bash index 6cfab2a..ce22775 100755 --- a/.docker/run.bash +++ b/.docker/run.bash @@ -76,7 +76,9 @@ GUI_ENVS=( CUSTOM_VOLUMES+=("/etc/localtime:/etc/localtime:ro") # Pass USB devices to container -USB_VOLUME+=("/dev/bus/usb:/dev/bus/usb") +#USB_VOLUME+=("/dev/bus/usb:/dev/bus/usb") +DEVICE_VOLUME+=("/dev:/dev:rw") +#USB_SERIAL+=("/dev/ttyACM0:/dev/ttyACM0") ## Additional environment variables # Synchronize ROS_DOMAIN_ID with host @@ -114,7 +116,9 @@ DOCKER_RUN_CMD=( "${GPU_OPT}" "${GPU_ENVS[@]/#/"--env "}" "${CUSTOM_VOLUMES[@]/#/"--volume "}" - "${USB_VOLUME[@]/#/"--volume "}" + "${DEVICE_VOLUME[@]/#/"--volume "}" + #"${USB_VOLUME[@]/#/"--volume "}" + #"${USB_SERIAL[@]/#/"--volume "}" "${CUSTOM_ENVS[@]/#/"--env "}" "${TAG}" "${CMD}" diff --git a/README.md b/README.md index e50eced..31c9d84 100644 --- a/README.md +++ b/README.md @@ -12,3 +12,19 @@ ros2 launch draw_svg draw_svg.launch.py ## xArm lite6 - web interface: http://192.168.1.150:18333 +## ROS2 rpi4 +https://github.com/ros-realtime/ros-realtime-rpi4-image/releases + +After unpacking the tar file, flash it to sd card. +Log in with "ubuntu:ubuntu". + +``` sh +sudo -i +loadkeys fi +passwd ubuntu #change from default 'ubuntu' to '1234' +apt-mark hold $(uname -r) linux-firmware u-boot-rpi u-boot-tools #prevent kernel updates +apt-mark hold libraspberrypi-bin libraspberrypi-dev libraspberrypi-doc libraspberrypi0 +apt-mark hold raspberrypi-bootloader raspberrypi-kernel raspberrypi-kernel-headers + +apt update && apt upgrade +``` diff --git a/src/axidraw_controller/CMakeLists.txt b/src/axidraw_controller/CMakeLists.txt new file mode 100644 index 0000000..be27ef6 --- /dev/null +++ b/src/axidraw_controller/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.8) +project(axidraw_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/axidraw_controller/package.xml b/src/axidraw_controller/package.xml new file mode 100644 index 0000000..8bf8e76 --- /dev/null +++ b/src/axidraw_controller/package.xml @@ -0,0 +1,18 @@ + + + + axidraw_controller + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/axidraw_controller/test.py b/src/axidraw_controller/src/test.py similarity index 95% rename from src/axidraw_controller/test.py rename to src/axidraw_controller/src/test.py index 0db01e7..f2ccdfb 100644 --- a/src/axidraw_controller/test.py +++ b/src/axidraw_controller/src/test.py @@ -2,6 +2,7 @@ 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. diff --git a/src/draw_svg/src/py/draw_svg.py b/src/draw_svg/src/py/draw_svg.py index 18c1531..2366ceb 100755 --- a/src/draw_svg/src/py/draw_svg.py +++ b/src/draw_svg/src/py/draw_svg.py @@ -55,7 +55,7 @@ class PublishTarget(Node): def __init__(self): super().__init__('publisher') self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10) - timer_period = 7.0 # seconds + timer_period = 4.0 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) self.i = 0 @@ -66,7 +66,7 @@ class PublishTarget(Node): #print(p.orientation) xml = ET.parse('svg/test.svg') svg = xml.getroot() - self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.1, 0.5, -0.2, 0.2) + self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.2, 0.4, -0.1, 0.1) self.points = [] for child in svg: if (child.tag == 'line'): diff --git a/src/drawing_controller/CMakeLists.txt b/src/drawing_controller/CMakeLists.txt new file mode 100644 index 0000000..9837031 --- /dev/null +++ b/src/drawing_controller/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.8) +project(drawing_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/drawing_controller/package.xml b/src/drawing_controller/package.xml new file mode 100644 index 0000000..5731613 --- /dev/null +++ b/src/drawing_controller/package.xml @@ -0,0 +1,18 @@ + + + + drawing_controller + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/drawing_controller/src/drawing_controller.py b/src/drawing_controller/src/drawing_controller.py new file mode 100644 index 0000000..40000fb --- /dev/null +++ b/src/drawing_controller/src/drawing_controller.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 +"""Sends motions individually to robot_controller""" + +import rclpy +from geometry_msgs.msg import Pose, PoseStamped +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.qos import QoSProfile +from random import uniform as rand +import math +#from tf2_ros.transformations import quaternion_from_euler +import lxml.etree as ET + + +def quaternion_from_euler(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = [0,0,0,0] + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + + return q + +def translate(val, lmin, lmax, rmin, rmax): + lspan = lmax - lmin + rspan = rmax - rmin + val = float(val - lmin) / float(lspan) + return rmin + (val * rspan) + +def map_point_function(x_pixels, y_pixels, xlim_lower, xlim_upper, ylim_lower, ylim_upper): + def map_point(xpix,ypix): + x = translate(xpix, 0, x_pixels, xlim_lower, xlim_upper) + y = translate(ypix, 0, y_pixels, ylim_lower, ylim_upper) + return (x,y) + return map_point + + +class DrawingController(Node): + def __init__(self): + super().__init__('drawing_controller') + self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10) + timer_period = 7.0 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + # TODO get dimensions from svg + + #print(p) + #print(p.position) + #print(p.orientation) + xml = ET.parse('svg/test.svg') + svg = xml.getroot() + self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.1, 0.5, -0.2, 0.2) + self.points = [] + for child in svg: + if (child.tag == 'line'): + self.points.append((float(child.get('x1')), float(child.get('y1')))) + self.points.append((float(child.get('x2')), float(child.get('y2')))) + + + def timer_callback(self): + next_point = self.points[self.i] + point = self.map_point(float(next_point[0]),float(next_point[1])) + p = Pose() + p.position.x = point[0] + p.position.y = point[1] + p.position.z = 0.1 + q = quaternion_from_euler(0.0, math.pi, 0.0) + #p.orientation = q + p.orientation.x = q[0] + p.orientation.y = q[1] + p.orientation.z = q[2] + p.orientation.w = q[3] + ps = PoseStamped() + ps.pose = p + #print(ps) + self.publisher_.publish(ps) + self.get_logger().info('Publishing to /target_pose: "%s"' % p) + self.i = (self.i + 1) % len(self.points) + +def main(args=None): + rclpy.init(args=args) + + publisher = PublishTarget() + + rclpy.spin(publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/lite6_controller/CMakeLists.txt b/src/lite6_controller/CMakeLists.txt new file mode 100644 index 0000000..420b6b9 --- /dev/null +++ b/src/lite6_controller/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.8) +project(lite6_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/lite6_controller/package.xml b/src/lite6_controller/package.xml new file mode 100644 index 0000000..1db4938 --- /dev/null +++ b/src/lite6_controller/package.xml @@ -0,0 +1,18 @@ + + + + lite6_controller + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/robot_controller/CMakeLists.txt b/src/robot_controller/CMakeLists.txt new file mode 100644 index 0000000..cfbbf6e --- /dev/null +++ b/src/robot_controller/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(robot_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(robot_interfaces REQUIRED) + +#add_executable(robotcontroller src/robotcontroller.cpp) +#ament_target_dependencies(robotcontroller rclcpp robot_interfaces) + +install(TARGETS + talker + listener + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/src/robot_controller/package.xml b/src/robot_controller/package.xml new file mode 100644 index 0000000..c2d1ad9 --- /dev/null +++ b/src/robot_controller/package.xml @@ -0,0 +1,18 @@ + + + + robot_controller + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/robot_controller/src/robotcontroller.cpp b/src/robot_controller/src/robotcontroller.cpp new file mode 100644 index 0000000..2f49900 --- /dev/null +++ b/src/robot_controller/src/robotcontroller.cpp @@ -0,0 +1,41 @@ +#include +#include + +class RobotController : public rclcpp::Node +{ +public: + /// Constructor + RobotController(); + +private: + /// Callback that executes path on robot + virtual void add(const std::shared_ptr request, + std::shared_ptr response); +}; + +RobotController::RobotController(string name) : Node(name) +{ + // Subscribe to target pose + target_pose_sub_ = this->create_subscription("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "Initialization successful."); +} + +int main(int argc, char ** argv) +{ + //rclcpp::init(argc, argv); + + //auto target_follower = std::make_shared(); + + //rclcpp::executors::SingleThreadedExecutor executor; + //executor.add_node(target_follower); + //executor.spin(); + + //rclcpp::shutdown(); + //return EXIT_SUCCESS; + (void) argc; + (void) argv; + + printf("hello world\n"); + return 0; +}