Fix class structure and virtual function

src/drawing_controller/CMakeLists.txt

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

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, '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

src/drawing_controller/package.xml

@@ -7,12 +7,15 @@
   <maintainer email="root@todo.todo">root</maintainer>
   <license>TODO: License declaration</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>robot_interfaces</depend>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
 
   <export>
-    <build_type>ament_cmake</build_type>
+    <build_type>ament_python</build_type>
   </export>
 </package>

src/drawing_controller/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/drawing_controller
+[install]
+install_scripts=$base/lib/drawing_controller

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',
+        ],
+    },
+)

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'

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)

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'

src/robot_controller/src/cpp/robot_controller.cpp

@@ -5,32 +5,14 @@
 class RobotController : public rclcpp::Node
 {
 public:
-  /// Constructor
-  RobotController(std::string name)
-  : Node(name)
+  RobotController(std::string name) : Node(name)
   {
-  //this->service = this->create_service<robot_interfaces::srv::ExecuteMotion>(name+"_execute_path", &this->executePath);
-  this->service = this->create_service<robot_interfaces::srv::ExecuteMotion>(name+"_execute_path", std::bind(&RobotController::executePath, this, std::placeholders::_1, std::placeholders::_2));
-
-  // Subscribe to target pose
-  //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
-
-  //RCLCPP_INFO(this->get_logger(), "Initialization successful.");
-
-
-  //std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");
-
-  //rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service = node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);
-
-  //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");
-
-  //rclcpp::spin(node);
-  //rclcpp::shutdown();
+    this->service = this->create_service<robot_interfaces::srv::ExecuteMotion>("execute_path", std::bind(&RobotController::executePath, this, std::placeholders::_1, std::placeholders::_2));
   }
 
-  //int (RobotController::*executePath)(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response);
-void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response) const
-{
+  /// Callback that executes path on robot
+  virtual void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response)
+  {
     //request->motion->path PoseStamped[]
     //request->motion->acceleration float64
     //request->motion->velocity float64
@@ -38,23 +20,39 @@ void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Req
     //RCLCPP_INFO(this->get_logger(), "Acceleration: " + std::to_string(request.motion.acceleration));
     response->status = "executePath not implemented";
     RCLCPP_WARN(this->get_logger(), "executePath not implemented");
-}
+  }
 
 private:
-  /// Callback that executes path on robot
-
   rclcpp::Service<robot_interfaces::srv::ExecuteMotion>::SharedPtr service;
 };
 
+// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
+class DummyController : public RobotController
+{
+  public:
+
+    DummyController(std::string name) : RobotController(name) {}
+
+    virtual void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response)
+    {
+      //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)
 
 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<RobotController>("generic");
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting dummy_controller");
+  auto robot = std::make_shared<DummyController>("dummy");
 
   rclcpp::executors::SingleThreadedExecutor executor;
   executor.add_node(robot);