From 3b56bac43e9ecbdde3b06cdf6619287380eb914f Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Mon, 19 Dec 2022 21:00:51 +0200 Subject: [PATCH] Improve motion service --- src/drawing_controller/CMakeLists.txt | 25 ---------------- .../drawing_controller/__init__.py | 0 .../drawing_controller.py | 29 ++++++++++++++----- src/drawing_controller/package.xml | 11 ++++--- .../resource/drawing_controller | 0 src/drawing_controller/setup.cfg | 4 +++ src/drawing_controller/setup.py | 26 +++++++++++++++++ src/drawing_controller/test/test_copyright.py | 23 +++++++++++++++ src/drawing_controller/test/test_flake8.py | 25 ++++++++++++++++ src/drawing_controller/test/test_pep257.py | 23 +++++++++++++++ .../src/cpp/robot_controller.cpp | 25 ++++++++++++++-- 11 files changed, 153 insertions(+), 38 deletions(-) delete mode 100644 src/drawing_controller/CMakeLists.txt create mode 100644 src/drawing_controller/drawing_controller/__init__.py rename src/drawing_controller/{src => drawing_controller}/drawing_controller.py (75%) create mode 100644 src/drawing_controller/resource/drawing_controller create mode 100644 src/drawing_controller/setup.cfg create mode 100644 src/drawing_controller/setup.py create mode 100644 src/drawing_controller/test/test_copyright.py create mode 100644 src/drawing_controller/test/test_flake8.py create mode 100644 src/drawing_controller/test/test_pep257.py diff --git a/src/drawing_controller/CMakeLists.txt b/src/drawing_controller/CMakeLists.txt deleted file mode 100644 index 9837031..0000000 --- a/src/drawing_controller/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -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/drawing_controller/__init__.py b/src/drawing_controller/drawing_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/drawing_controller/src/drawing_controller.py b/src/drawing_controller/drawing_controller/drawing_controller.py similarity index 75% rename from src/drawing_controller/src/drawing_controller.py rename to src/drawing_controller/drawing_controller/drawing_controller.py index 40000fb..4500e98 100644 --- a/src/drawing_controller/src/drawing_controller.py +++ b/src/drawing_controller/drawing_controller/drawing_controller.py @@ -11,6 +11,8 @@ import math #from tf2_ros.transformations import quaternion_from_euler import lxml.etree as ET +from robot_interfaces.srv import ExecuteMotion +import sys def quaternion_from_euler(ai, aj, ak): ai /= 2.0 @@ -50,19 +52,24 @@ def map_point_function(x_pixels, y_pixels, xlim_lower, xlim_upper, ylim_lower, y class DrawingController(Node): - def __init__(self): + def __init__(self, svgpath): super().__init__('drawing_controller') - self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10) + #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 + self.cli = self.create_client(ExecuteMotion, 'dummy_execute_path') + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = ExecuteMotion.Request() + # TODO get dimensions from svg #print(p) #print(p.position) #print(p.orientation) - xml = ET.parse('svg/test.svg') + xml = ET.parse(svgpath) 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 = [] @@ -88,16 +95,24 @@ class DrawingController(Node): ps = PoseStamped() ps.pose = p #print(ps) - self.publisher_.publish(ps) - self.get_logger().info('Publishing to /target_pose: "%s"' % p) + #self.publisher_.publish(ps) + #self.get_logger().info('Publishing to /target_pose: "%s"' % p) self.i = (self.i + 1) % len(self.points) + #self.req.a = a + #self.req.b = b + self.future = self.cli.call_async(self.req) + #rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('Got result: "%s"' % self.future.result()) + #return self.future.result() + + def main(args=None): rclpy.init(args=args) - publisher = PublishTarget() + dc = DrawingController(sys.argv[1]) - rclpy.spin(publisher) + rclpy.spin(dc) # Destroy the node explicitly # (optional - otherwise it will be done automatically diff --git a/src/drawing_controller/package.xml b/src/drawing_controller/package.xml index 5731613..8ed1373 100644 --- a/src/drawing_controller/package.xml +++ b/src/drawing_controller/package.xml @@ -7,12 +7,15 @@ root TODO: License declaration - ament_cmake + rclpy + robot_interfaces - ament_lint_auto - ament_lint_common + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest - ament_cmake + ament_python diff --git a/src/drawing_controller/resource/drawing_controller b/src/drawing_controller/resource/drawing_controller new file mode 100644 index 0000000..e69de29 diff --git a/src/drawing_controller/setup.cfg b/src/drawing_controller/setup.cfg new file mode 100644 index 0000000..eb8ed1f --- /dev/null +++ b/src/drawing_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/drawing_controller +[install] +install_scripts=$base/lib/drawing_controller diff --git a/src/drawing_controller/setup.py b/src/drawing_controller/setup.py new file mode 100644 index 0000000..54b219a --- /dev/null +++ b/src/drawing_controller/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'drawing_controller' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='', + maintainer_email='todo@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'drawing_controller = drawing_controller.drawing_controller:main', + ], + }, +) diff --git a/src/drawing_controller/test/test_copyright.py b/src/drawing_controller/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/drawing_controller/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/drawing_controller/test/test_flake8.py b/src/drawing_controller/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/drawing_controller/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/drawing_controller/test/test_pep257.py b/src/drawing_controller/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/drawing_controller/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/robot_controller/src/cpp/robot_controller.cpp b/src/robot_controller/src/cpp/robot_controller.cpp index 3519e2d..8761602 100644 --- a/src/robot_controller/src/cpp/robot_controller.cpp +++ b/src/robot_controller/src/cpp/robot_controller.cpp @@ -46,6 +46,27 @@ private: rclcpp::Service::SharedPtr service; }; +// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation. +// CRTP pattern used here https://en.wikipedia.org/wiki/Curiously_recurring_template_pattern +//class DummyController : public RobotController +class DummyController : public RobotController +{ + public: + + DummyController(std::string name) : RobotController(name) {} + + void executePath(const std::shared_ptr request, std::shared_ptr response) const + { + //request->motion->path PoseStamped[] + //request->motion->acceleration float64 + //request->motion->velocity float64 + RCLCPP_INFO(this->get_logger(), "NEW MOTION: Acceleration: "); + //RCLCPP_INFO(this->get_logger(), "Acceleration: " + std::to_string(request.motion.acceleration)); + response->status = "executePath not implemented"; + RCLCPP_WARN(this->get_logger(), "AAAAAA executePath not implemented"); + } + +}; //RobotController::RobotController(name) : Node(name) @@ -53,8 +74,8 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting generic robot_controller"); - auto robot = std::make_shared("generic"); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting dummy_controller"); + auto robot = std::make_shared("dummy"); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(robot);