From 5ce3d43841cdf2d4a347374d26a610da21ab384b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 10:15:52 +0000 Subject: [PATCH 01/32] initial commit --- cpp_pubsub/CMakeLists.txt | 38 +++++++++++++ cpp_pubsub/package.xml | 20 +++++++ cpp_pubsub/src/publisher_member_function.cpp | 56 +++++++++++++++++++ cpp_pubsub/src/subscriber_member_function.cpp | 45 +++++++++++++++ drone_sensors/CMakeLists.txt | 37 ++++++++++++ drone_sensors/package.xml | 21 +++++++ drone_sensors/src/height_sensor.cpp | 39 +++++++++++++ my_package/CMakeLists.txt | 35 ++++++++++++ my_package/package.xml | 18 ++++++ my_package/src/my_node.cpp | 10 ++++ my_package_py/my_package_py/__init__.py | 0 my_package_py/my_package_py/my_node_py.py | 6 ++ my_package_py/package.xml | 18 ++++++ my_package_py/resource/my_package_py | 0 my_package_py/setup.cfg | 4 ++ my_package_py/setup.py | 26 +++++++++ my_package_py/test/test_copyright.py | 25 +++++++++ my_package_py/test/test_flake8.py | 25 +++++++++ my_package_py/test/test_pep257.py | 23 ++++++++ py_pubsub/package.xml | 21 +++++++ py_pubsub/py_pubsub/__init__.py | 0 .../py_pubsub/publisher_member_function.py | 53 ++++++++++++++++++ .../py_pubsub/subscriber_member_function.py | 51 +++++++++++++++++ py_pubsub/resource/py_pubsub | 0 py_pubsub/setup.cfg | 4 ++ py_pubsub/setup.py | 27 +++++++++ py_pubsub/test/test_copyright.py | 25 +++++++++ py_pubsub/test/test_flake8.py | 25 +++++++++ py_pubsub/test/test_pep257.py | 23 ++++++++ 29 files changed, 675 insertions(+) create mode 100644 cpp_pubsub/CMakeLists.txt create mode 100644 cpp_pubsub/package.xml create mode 100644 cpp_pubsub/src/publisher_member_function.cpp create mode 100644 cpp_pubsub/src/subscriber_member_function.cpp create mode 100644 drone_sensors/CMakeLists.txt create mode 100644 drone_sensors/package.xml create mode 100644 drone_sensors/src/height_sensor.cpp create mode 100644 my_package/CMakeLists.txt create mode 100644 my_package/package.xml create mode 100644 my_package/src/my_node.cpp create mode 100644 my_package_py/my_package_py/__init__.py create mode 100644 my_package_py/my_package_py/my_node_py.py create mode 100644 my_package_py/package.xml create mode 100644 my_package_py/resource/my_package_py create mode 100644 my_package_py/setup.cfg create mode 100644 my_package_py/setup.py create mode 100644 my_package_py/test/test_copyright.py create mode 100644 my_package_py/test/test_flake8.py create mode 100644 my_package_py/test/test_pep257.py create mode 100644 py_pubsub/package.xml create mode 100644 py_pubsub/py_pubsub/__init__.py create mode 100644 py_pubsub/py_pubsub/publisher_member_function.py create mode 100644 py_pubsub/py_pubsub/subscriber_member_function.py create mode 100644 py_pubsub/resource/py_pubsub create mode 100644 py_pubsub/setup.cfg create mode 100644 py_pubsub/setup.py create mode 100644 py_pubsub/test/test_copyright.py create mode 100644 py_pubsub/test/test_flake8.py create mode 100644 py_pubsub/test/test_pep257.py diff --git a/cpp_pubsub/CMakeLists.txt b/cpp_pubsub/CMakeLists.txt new file mode 100644 index 00000000..f199cabc --- /dev/null +++ b/cpp_pubsub/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.8) +project(cpp_pubsub) + +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) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +add_executable(talker src/publisher_member_function.cpp) +ament_target_dependencies(talker rclcpp std_msgs) +add_executable(listener src/subscriber_member_function.cpp) +ament_target_dependencies(listener rclcpp std_msgs) + +install(TARGETS + talker + listener + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/cpp_pubsub/package.xml b/cpp_pubsub/package.xml new file mode 100644 index 00000000..8553fb33 --- /dev/null +++ b/cpp_pubsub/package.xml @@ -0,0 +1,20 @@ + + + + cpp_pubsub + 0.0.0 + Example of publisher/subscriber nodes in ROS 2 for the 5G drone + ic + Apache License 2.0 + + ament_cmake + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/cpp_pubsub/src/publisher_member_function.cpp b/cpp_pubsub/src/publisher_member_function.cpp new file mode 100644 index 00000000..c8d2ebfe --- /dev/null +++ b/cpp_pubsub/src/publisher_member_function.cpp @@ -0,0 +1,56 @@ +// Copyright 2016 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. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +/* This example creates a subclass of Node and uses std::bind() to register a + * member function as a callback from the timer. */ + +class MinimalPublisher : public rclcpp::Node +{ +public: + MinimalPublisher() + : Node("minimal_publisher"), count_(0) + { + publisher_ = this->create_publisher("topic", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&MinimalPublisher::timer_callback, this)); + } + +private: + void timer_callback() + { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + publisher_->publish(message); + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/cpp_pubsub/src/subscriber_member_function.cpp b/cpp_pubsub/src/subscriber_member_function.cpp new file mode 100644 index 00000000..1560403e --- /dev/null +++ b/cpp_pubsub/src/subscriber_member_function.cpp @@ -0,0 +1,45 @@ +// Copyright 2016 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. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +using std::placeholders::_1; + +class MinimalSubscriber : public rclcpp::Node +{ +public: + MinimalSubscriber() + : Node("minimal_subscriber") + { + subscription_ = this->create_subscription( + "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + } + +private: + void topic_callback(const std_msgs::msg::String::SharedPtr msg) const + { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + } + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/drone_sensors/CMakeLists.txt b/drone_sensors/CMakeLists.txt new file mode 100644 index 00000000..7368ee75 --- /dev/null +++ b/drone_sensors/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(drone_sensors) + +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) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +# add executable so that the node can be run with ros2 run +add_executable(height_sensor src/height_sensor.cpp) +ament_target_dependencies(height_sensor rclcpp std_msgs) + +# add install targets section so ros2 run can find the executable +install(TARGETS +height_sensor +DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/drone_sensors/package.xml b/drone_sensors/package.xml new file mode 100644 index 00000000..a2ba2a45 --- /dev/null +++ b/drone_sensors/package.xml @@ -0,0 +1,21 @@ + + + + drone_sensors + 0.0.0 + package for the environment sensors of the 5G drone (height meter, object detection) + Sem van der Hoeven + Apache License 2.0 + + ament_cmake + + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp new file mode 100644 index 00000000..909c558a --- /dev/null +++ b/drone_sensors/src/height_sensor.cpp @@ -0,0 +1,39 @@ +#include //time measurement + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +class HeightSensorPublisher : public rclcpp::Node +{ + public: + HeightSensorPublisher() : Node("height_sensor_publisher") + { + publisher_ = this->create_publisher("height_sensor", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + + } + private: + void timer_callback() + { + auto message = std_msgs::msg::String(); + message.data = "Hello heigth sensor!"; + RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); + publisher_->publish(message); + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc,argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} + + diff --git a/my_package/CMakeLists.txt b/my_package/CMakeLists.txt new file mode 100644 index 00000000..ac32ca48 --- /dev/null +++ b/my_package/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(my_package) + +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) + +add_executable(my_node src/my_node.cpp) +target_include_directories(my_node PUBLIC + $ + $) +target_compile_features(my_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS my_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/my_package/package.xml b/my_package/package.xml new file mode 100644 index 00000000..4bf230fe --- /dev/null +++ b/my_package/package.xml @@ -0,0 +1,18 @@ + + + + my_package + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/my_package/src/my_node.cpp b/my_package/src/my_node.cpp new file mode 100644 index 00000000..0248b108 --- /dev/null +++ b/my_package/src/my_node.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world my_package package\n"); + return 0; +} diff --git a/my_package_py/my_package_py/__init__.py b/my_package_py/my_package_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/my_package_py/my_package_py/my_node_py.py b/my_package_py/my_package_py/my_node_py.py new file mode 100644 index 00000000..ceb86f5c --- /dev/null +++ b/my_package_py/my_package_py/my_node_py.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from my_package_py.') + + +if __name__ == '__main__': + main() diff --git a/my_package_py/package.xml b/my_package_py/package.xml new file mode 100644 index 00000000..cb3801eb --- /dev/null +++ b/my_package_py/package.xml @@ -0,0 +1,18 @@ + + + + my_package_py + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/my_package_py/resource/my_package_py b/my_package_py/resource/my_package_py new file mode 100644 index 00000000..e69de29b diff --git a/my_package_py/setup.cfg b/my_package_py/setup.cfg new file mode 100644 index 00000000..8cc1be01 --- /dev/null +++ b/my_package_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/my_package_py +[install] +install_scripts=$base/lib/my_package_py diff --git a/my_package_py/setup.py b/my_package_py/setup.py new file mode 100644 index 00000000..564a8995 --- /dev/null +++ b/my_package_py/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'my_package_py' + +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='ubuntu', + maintainer_email='ubuntu@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'my_node_py = my_package_py.my_node_py:main' + ], + }, +) diff --git a/my_package_py/test/test_copyright.py b/my_package_py/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/my_package_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/my_package_py/test/test_flake8.py b/my_package_py/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/my_package_py/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/my_package_py/test/test_pep257.py b/my_package_py/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/my_package_py/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/py_pubsub/package.xml b/py_pubsub/package.xml new file mode 100644 index 00000000..b2dae22b --- /dev/null +++ b/py_pubsub/package.xml @@ -0,0 +1,21 @@ + + + + py_pubsub + 0.0.0 + Example of publisher/subscriber nodes in Python + ubuntu + Apache License 2.0 + + rclpy + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/py_pubsub/py_pubsub/__init__.py b/py_pubsub/py_pubsub/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/py_pubsub/py_pubsub/publisher_member_function.py b/py_pubsub/py_pubsub/publisher_member_function.py new file mode 100644 index 00000000..dfdb0320 --- /dev/null +++ b/py_pubsub/py_pubsub/publisher_member_function.py @@ -0,0 +1,53 @@ +# Copyright 2016 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. + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + + +class MinimalPublisher(Node): + + def __init__(self): + super().__init__('minimal_publisher') + self.publisher_ = self.create_publisher(String, 'topic', 10) + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = String() + msg.data = 'Hello World: %d' % self.i + self.publisher_.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.data) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/py_pubsub/py_pubsub/subscriber_member_function.py b/py_pubsub/py_pubsub/subscriber_member_function.py new file mode 100644 index 00000000..78c72668 --- /dev/null +++ b/py_pubsub/py_pubsub/subscriber_member_function.py @@ -0,0 +1,51 @@ +# Copyright 2016 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. + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + + +class MinimalSubscriber(Node): + + def __init__(self): + super().__init__('minimal_subscriber') + self.subscription = self.create_subscription( + String, + 'topic', + self.listener_callback, + 10) + self.subscription # prevent unused variable warning + + def listener_callback(self, msg): + self.get_logger().info('I heard: "%s"' % msg.data) + + +def main(args=None): + rclpy.init(args=args) + + minimal_subscriber = MinimalSubscriber() + + rclpy.spin(minimal_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/py_pubsub/resource/py_pubsub b/py_pubsub/resource/py_pubsub new file mode 100644 index 00000000..e69de29b diff --git a/py_pubsub/setup.cfg b/py_pubsub/setup.cfg new file mode 100644 index 00000000..3f886b98 --- /dev/null +++ b/py_pubsub/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/py_pubsub +[install] +install_scripts=$base/lib/py_pubsub diff --git a/py_pubsub/setup.py b/py_pubsub/setup.py new file mode 100644 index 00000000..ebc173ef --- /dev/null +++ b/py_pubsub/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = 'py_pubsub' + +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='ubuntu', + maintainer_email='ubuntu@todo.todo', + description='Example of publisher/subscriber nodes in Python', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'talker = py_pubsub.publisher_member_function:main', + 'listener = py_pubsub.subscriber_member_function:main', + ], + }, +) diff --git a/py_pubsub/test/test_copyright.py b/py_pubsub/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/py_pubsub/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/py_pubsub/test/test_flake8.py b/py_pubsub/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/py_pubsub/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/py_pubsub/test/test_pep257.py b/py_pubsub/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/py_pubsub/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' -- 2.47.2 From ee2d5c4d547b484ad096d016ffd25bf71492b7a5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 11:29:59 +0100 Subject: [PATCH 02/32] add gitignore --- .gitignore | 330 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 330 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..0c5c8f90 --- /dev/null +++ b/.gitignore @@ -0,0 +1,330 @@ +# Created by https://www.toptal.com/developers/gitignore/api/ros2,ros,cmake,c++,python,visualstudiocode,vim +# Edit at https://www.toptal.com/developers/gitignore?templates=ros2,ros,cmake,c++,python,visualstudiocode,vim + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### CMake ### +CMakeLists.txt.user +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +Makefile +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake +_deps + +### CMake Patch ### +# External projects +*-prefix/ + +### Python ### +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/#use-with-ide +.pdm.toml + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ + +### Python Patch ### +# Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration +poetry.toml + +# ruff +.ruff_cache/ + +### ROS ### +devel/ +logs/ +bin/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE + +### ROS2 ### +install/ +log/ + +# Ignore generated docs + +# eclipse stuff + +# qcreator stuff + + + +# Emacs + +# Colcon custom files +COLCON_IGNORE +AMENT_IGNORE + +### Vim ### +# Swap +[._]*.s[a-v][a-z] +!*.svg # comment out if you don't need vector files +[._]*.sw[a-p] +[._]s[a-rt-v][a-z] +[._]ss[a-gi-z] +[._]sw[a-p] + +# Session +Session.vim +Sessionx.vim + +# Temporary +.netrwhist +# Auto-generated tag files +tags +# Persistent undo +[._]*.un~ + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +!.vscode/*.code-snippets + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix + +### VisualStudioCode Patch ### +# Ignore all local history of files +.history +.ionide + +# End of https://www.toptal.com/developers/gitignore/api/ros2,ros,cmake,c++,python,visualstudiocode,vim -- 2.47.2 From eaeb2dba5ae8d837b1845eaea95ebc80ec207c64 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 12:47:04 +0100 Subject: [PATCH 03/32] add test print --- .gitignore | 2 ++ drone_sensors/src/height_sensor.cpp | 4 +++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 0c5c8f90..75923b1c 100644 --- a/.gitignore +++ b/.gitignore @@ -328,3 +328,5 @@ tags .ionide # End of https://www.toptal.com/developers/gitignore/api/ros2,ros,cmake,c++,python,visualstudiocode,vim + +.vscode/ diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 909c558a..a771d0ff 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -1,9 +1,10 @@ #include //time measurement +#include // reading from serial port #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -using namespace std::chrono_literals; +using namespace std::chrono_literals; // for time measurements class HeightSensorPublisher : public rclcpp::Node { @@ -13,6 +14,7 @@ class HeightSensorPublisher : public rclcpp::Node publisher_ = this->create_publisher("height_sensor", 10); timer_ = this->create_wall_timer( 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + RCLCPP_INFO(this->get_logger(), "Constructor of height sensor publisher"); } private: -- 2.47.2 From f0f63b1817b77e4e8626db9a523c7d48afbb1d3d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 12:58:55 +0100 Subject: [PATCH 04/32] serial port test if it can be opened --- drone_sensors/src/height_sensor.cpp | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index a771d0ff..9f1a1386 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -1,5 +1,5 @@ #include //time measurement -#include // reading from serial port +#include // reading from serial port #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -16,6 +16,8 @@ class HeightSensorPublisher : public rclcpp::Node 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "Constructor of height sensor publisher"); + setup_serial_port(); + } private: void timer_callback() @@ -25,8 +27,28 @@ class HeightSensorPublisher : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); publisher_->publish(message); } + + /** + * @brief Set the up serial port object to read from the sensor (/dev/ttyACM0) + * + */ + void setup_serial_port() + { + serial_port.open("/dev/ttyACM0", std::ios::in); + if (!serial_port.is_open()) + { + RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); + } + else + { + RCLCPP_INFO(this->get_logger(), "Serial port opened"); + } + + serial_port.close(); + } rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr publisher_; + std::fstream serial_port; // serial port for reading from device }; int main(int argc, char *argv[]) -- 2.47.2 From 015d7d7596610588101d0bb09c55bab085a61be4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 13:10:05 +0100 Subject: [PATCH 05/32] reading single byte --- drone_sensors/src/height_sensor.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 9f1a1386..2c3a7c63 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -20,10 +20,18 @@ class HeightSensorPublisher : public rclcpp::Node } private: + + + /** + * @brief Timer callback function to publish the height sensor data + * + */ void timer_callback() { + char* readdata = new char[1]; + serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - message.data = "Hello heigth sensor!"; + message.data = "Height: " + std::to_string((int)readdata[0]); RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); publisher_->publish(message); } @@ -38,6 +46,7 @@ class HeightSensorPublisher : public rclcpp::Node if (!serial_port.is_open()) { RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); + return; } else { -- 2.47.2 From 353bf749307259cf7acb3508a5298a9cd0cb861e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 12:23:20 +0000 Subject: [PATCH 06/32] change wait to 1 ms --- drone_sensors/src/height_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 2c3a7c63..8d5271d0 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -13,7 +13,7 @@ class HeightSensorPublisher : public rclcpp::Node { publisher_ = this->create_publisher("height_sensor", 10); timer_ = this->create_wall_timer( - 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + 1ms, std::bind(&HeightSensorPublisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "Constructor of height sensor publisher"); setup_serial_port(); -- 2.47.2 From ed71dfb0e943a8479d4ab4e6e003350148d7c48f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 13:28:41 +0100 Subject: [PATCH 07/32] test --- drone_sensors/src/height_sensor.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 2c3a7c63..39ba2ded 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -55,6 +55,11 @@ class HeightSensorPublisher : public rclcpp::Node serial_port.close(); } + + void test() + { + RCLCPP_INFO(this->get_logger(), "Je moeder is een plopkoek"); + } rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr publisher_; std::fstream serial_port; // serial port for reading from device @@ -63,7 +68,13 @@ class HeightSensorPublisher : public rclcpp::Node int main(int argc, char *argv[]) { rclcpp::init(argc,argv); - rclcpp::spin(std::make_shared()); + + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + node.get()->setup_serial_port(); + executor.add_node(node); + executor.spin(); + // rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -- 2.47.2 From b88d8beb39b145839b10cc1beb8fb4089e82288f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 13:37:31 +0100 Subject: [PATCH 08/32] add test calling method while node spinning --- drone_sensors/src/height_sensor.cpp | 107 ++++++++++++++-------------- 1 file changed, 54 insertions(+), 53 deletions(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 7400caca..f39d494f 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -1,4 +1,4 @@ -#include //time measurement +#include //time measurement #include // reading from serial port #include "rclcpp/rclcpp.hpp" @@ -8,76 +8,77 @@ using namespace std::chrono_literals; // for time measurements class HeightSensorPublisher : public rclcpp::Node { - public: - HeightSensorPublisher() : Node("height_sensor_publisher") - { - publisher_ = this->create_publisher("height_sensor", 10); - timer_ = this->create_wall_timer( - 1ms, std::bind(&HeightSensorPublisher::timer_callback, this)); - RCLCPP_INFO(this->get_logger(), "Constructor of height sensor publisher"); +public: + HeightSensorPublisher() : Node("height_sensor_publisher") + { + publisher_ = this->create_publisher("height_sensor", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + RCLCPP_INFO(this->get_logger(), "Constructor of height sensor publisher"); - setup_serial_port(); + setup_serial_port(); + } - } - private: + void test() + { + RCLCPP_INFO(this->get_logger(), "Je moeder is een plopkoek"); + } +private: + /** + * @brief Timer callback function to publish the height sensor data + * + */ + void timer_callback() + { + char *readdata = new char[1]; + serial_port.read(readdata, 1); + auto message = std_msgs::msg::String(); + message.data = "Height: " + std::to_string((int)readdata[0]); + RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); + publisher_->publish(message); + } - /** - * @brief Timer callback function to publish the height sensor data - * - */ - void timer_callback() + /** + * @brief Set the up serial port object to read from the sensor (/dev/ttyACM0) + * + */ + void setup_serial_port() + { + serial_port.open("/dev/ttyACM0", std::ios::in); + if (!serial_port.is_open()) { - char* readdata = new char[1]; - serial_port.read(readdata, 1); - auto message = std_msgs::msg::String(); - message.data = "Height: " + std::to_string((int)readdata[0]); - RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); - publisher_->publish(message); + RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); + return; + } + else + { + RCLCPP_INFO(this->get_logger(), "Serial port opened"); } - /** - * @brief Set the up serial port object to read from the sensor (/dev/ttyACM0) - * - */ - void setup_serial_port() - { - serial_port.open("/dev/ttyACM0", std::ios::in); - if (!serial_port.is_open()) - { - RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); - return; - } - else - { - RCLCPP_INFO(this->get_logger(), "Serial port opened"); - } + serial_port.close(); + } - serial_port.close(); - } - - void test() - { - RCLCPP_INFO(this->get_logger(), "Je moeder is een plopkoek"); - } - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - std::fstream serial_port; // serial port for reading from device + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + std::fstream serial_port; // serial port for reading from device }; int main(int argc, char *argv[]) { - rclcpp::init(argc,argv); + rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - node.get()->setup_serial_port(); + rclcpp::Node::SharedPtr node = std::make_shared(); executor.add_node(node); executor.spin(); // rclcpp::spin(std::make_shared()); + + while (true) + { + node->test(); + } rclcpp::shutdown(); return 0; } - - -- 2.47.2 From a6fd08738c56581571659ad5471e846546a07b9b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 12:55:22 +0000 Subject: [PATCH 09/32] change test method --- drone_sensors/src/height_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index f39d494f..c1b38c31 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -76,7 +76,7 @@ int main(int argc, char *argv[]) while (true) { - node->test(); + node.get()->test(); } rclcpp::shutdown(); -- 2.47.2 From 31503b8b702d097c4ab76b39fefb8893ea47be86 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 13:59:11 +0100 Subject: [PATCH 10/32] add check start of measurement test --- drone_sensors/src/height_sensor.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index c1b38c31..e5073731 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -13,8 +13,8 @@ public: { publisher_ = this->create_publisher("height_sensor", 10); timer_ = this->create_wall_timer( - 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); - RCLCPP_INFO(this->get_logger(), "Constructor of height sensor publisher"); + 20ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + RCLCPP_INFO(this->get_logger(), "Starting height sensor publisher"); setup_serial_port(); } @@ -34,7 +34,10 @@ private: char *readdata = new char[1]; serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - message.data = "Height: " + std::to_string((int)readdata[0]); + message.data = "Height: " + (char)readdata[0]; + if (readdata[0] == 'T') { + RCLCPP_INFO(this->get_logger(), "got start of measurement"); + } RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); publisher_->publish(message); } @@ -48,7 +51,7 @@ private: serial_port.open("/dev/ttyACM0", std::ios::in); if (!serial_port.is_open()) { - RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); + RCLCPP_ERROR(this->get_logger(), "ERROR Could not open serial port"); return; } else @@ -73,11 +76,6 @@ int main(int argc, char *argv[]) executor.add_node(node); executor.spin(); // rclcpp::spin(std::make_shared()); - - while (true) - { - node.get()->test(); - } rclcpp::shutdown(); return 0; -- 2.47.2 From 24fc0c2ca5c5ea007c10b2a5164894c666c929e5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:04:14 +0100 Subject: [PATCH 11/32] fix error --- drone_sensors/src/height_sensor.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index e5073731..a700de9a 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -13,7 +13,7 @@ public: { publisher_ = this->create_publisher("height_sensor", 10); timer_ = this->create_wall_timer( - 20ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "Starting height sensor publisher"); setup_serial_port(); @@ -34,9 +34,11 @@ private: char *readdata = new char[1]; serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - message.data = "Height: " + (char)readdata[0]; - if (readdata[0] == 'T') { - RCLCPP_INFO(this->get_logger(), "got start of measurement"); + message.data = "Height: " + std::to_string((char)readdata[0]); + + if (readdata[0] == 'T') + { + RCLCPP_INFO(this->get_logger(), "Height sensor start measurement"); } RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); publisher_->publish(message); @@ -51,7 +53,7 @@ private: serial_port.open("/dev/ttyACM0", std::ios::in); if (!serial_port.is_open()) { - RCLCPP_ERROR(this->get_logger(), "ERROR Could not open serial port"); + RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); return; } else -- 2.47.2 From 1df349b47890446717aa04b9afbec7486bf2c0a8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:19:42 +0100 Subject: [PATCH 12/32] add reading bytes when getting a T --- drone_sensors/src/height_sensor.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index a700de9a..13343dc0 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -13,7 +13,7 @@ public: { publisher_ = this->create_publisher("height_sensor", 10); timer_ = this->create_wall_timer( - 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + 50ms, std::bind(&HeightSensorPublisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "Starting height sensor publisher"); setup_serial_port(); @@ -34,14 +34,17 @@ private: char *readdata = new char[1]; serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - message.data = "Height: " + std::to_string((char)readdata[0]); if (readdata[0] == 'T') { RCLCPP_INFO(this->get_logger(), "Height sensor start measurement"); + char *measurement = new char[2]; + serial_port.read(measurement, 2); + message.data = "Height: " + std::to_string(measurement[0]) + std::to_string(measurement[1]); + RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); + publisher_->publish(message); + } - RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); - publisher_->publish(message); } /** -- 2.47.2 From 4f530da21db43620e991c09cfceb3ed45078830d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:22:08 +0100 Subject: [PATCH 13/32] change T to number --- drone_sensors/src/height_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 13343dc0..73a61275 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -35,7 +35,7 @@ private: serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - if (readdata[0] == 'T') + if (readdata[0] == 0x54) // 0x54 = T (from the user manual) { RCLCPP_INFO(this->get_logger(), "Height sensor start measurement"); char *measurement = new char[2]; -- 2.47.2 From 9e5f2363f6869c562ebc3035c46a97a5ad94be5f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:26:09 +0100 Subject: [PATCH 14/32] extra print --- drone_sensors/src/height_sensor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 73a61275..56fed3d4 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -34,6 +34,7 @@ private: char *readdata = new char[1]; serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); + RCLCPP_INFO(this->get_logger(), "data: %s", readdata); if (readdata[0] == 0x54) // 0x54 = T (from the user manual) { @@ -61,7 +62,7 @@ private: } else { - RCLCPP_INFO(this->get_logger(), "Serial port opened"); + RCLCPP_INFO(this->get_logger(), "Serial port opened on /dev/ttyACM0"); } serial_port.close(); -- 2.47.2 From e5ddab280e9307e2e04cf313cf87a2c3958fb7ab Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:45:06 +0100 Subject: [PATCH 15/32] add printing of T value --- drone_sensors/src/height_sensor.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 56fed3d4..507f3508 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -19,11 +19,6 @@ public: setup_serial_port(); } - void test() - { - RCLCPP_INFO(this->get_logger(), "Je moeder is een plopkoek"); - } - private: /** * @brief Timer callback function to publish the height sensor data @@ -34,7 +29,7 @@ private: char *readdata = new char[1]; serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - RCLCPP_INFO(this->get_logger(), "data: %s", readdata); + RCLCPP_INFO(this->get_logger(), "data: %s", std::to_string(readdata[0])); if (readdata[0] == 0x54) // 0x54 = T (from the user manual) { -- 2.47.2 From 345e796392d3df35660ea6a5cef15fabf7094956 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:45:26 +0100 Subject: [PATCH 16/32] decrease timer delay --- drone_sensors/src/height_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 507f3508..16f5757e 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -13,7 +13,7 @@ public: { publisher_ = this->create_publisher("height_sensor", 10); timer_ = this->create_wall_timer( - 50ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + 5ms, std::bind(&HeightSensorPublisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "Starting height sensor publisher"); setup_serial_port(); -- 2.47.2 From 126ff49a35aac565a8f84e4fa4d633c589b7290c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:47:11 +0100 Subject: [PATCH 17/32] change string to char* --- drone_sensors/src/height_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 16f5757e..2fff20d1 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -29,7 +29,7 @@ private: char *readdata = new char[1]; serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - RCLCPP_INFO(this->get_logger(), "data: %s", std::to_string(readdata[0])); + RCLCPP_INFO(this->get_logger(), "data: %s", readdata); if (readdata[0] == 0x54) // 0x54 = T (from the user manual) { -- 2.47.2 From 0f0324ab9dfba813e1f40f100cab40a64376f773 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:52:43 +0100 Subject: [PATCH 18/32] add modes and remove close of port --- drone_sensors/src/height_sensor.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 2fff20d1..480845fe 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -4,8 +4,18 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#define TEXT_MODE 0x00, 0x11, 0x01, 0x45 +#define BINARY_MODE 0x00, 0x11, 0x02, 0x4C +#define SINGLE_PIXEL_MODE 0x00, 0x21, 0x01, 0xBC +#define TWO_BY_TWO_PIXEL_MODE 0x00, 0x21, 0x02, 0xB5 +#define TWO_PIXEL_MODE 0x00, 0x21, 0x03, 0xB2 +#define SHORT_RANGE_MODE 0x00, 0x61, 0x01, 0xE7 +#define LONG_RANGE_MODE 0x00, 0x61, 0x03, 0xE9 + using namespace std::chrono_literals; // for time measurements + + class HeightSensorPublisher : public rclcpp::Node { public: @@ -60,7 +70,7 @@ private: RCLCPP_INFO(this->get_logger(), "Serial port opened on /dev/ttyACM0"); } - serial_port.close(); + serial_port.write({BINARY_MODE}); } rclcpp::TimerBase::SharedPtr timer_; @@ -77,6 +87,7 @@ int main(int argc, char *argv[]) executor.add_node(node); executor.spin(); // rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; -- 2.47.2 From 4e8e1c81775f1f5ee238e4d4defc2e4b465d43a4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:54:55 +0100 Subject: [PATCH 19/32] change binary mode --- drone_sensors/src/height_sensor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index 480845fe..89f1237f 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -70,7 +70,8 @@ private: RCLCPP_INFO(this->get_logger(), "Serial port opened on /dev/ttyACM0"); } - serial_port.write({BINARY_MODE}); + char bmode[] = {BINARY_MODE} + serial_port.write(bmode, 4); } rclcpp::TimerBase::SharedPtr timer_; -- 2.47.2 From 6da45f1240f7e6a8e566a0a331fea419d74fd32b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 12:49:44 +0200 Subject: [PATCH 20/32] remove unnecessary folders --- cpp_pubsub/CMakeLists.txt | 38 -------- cpp_pubsub/package.xml | 20 ---- cpp_pubsub/src/publisher_member_function.cpp | 56 ----------- cpp_pubsub/src/subscriber_member_function.cpp | 45 --------- drone_sensors/CMakeLists.txt | 37 -------- drone_sensors/package.xml | 21 ---- drone_sensors/src/height_sensor.cpp | 95 ------------------- my_package/CMakeLists.txt | 35 ------- my_package/package.xml | 18 ---- my_package/src/my_node.cpp | 10 -- my_package_py/my_package_py/__init__.py | 0 my_package_py/my_package_py/my_node_py.py | 6 -- my_package_py/package.xml | 18 ---- my_package_py/resource/my_package_py | 0 my_package_py/setup.cfg | 4 - my_package_py/setup.py | 26 ----- my_package_py/test/test_copyright.py | 25 ----- my_package_py/test/test_flake8.py | 25 ----- my_package_py/test/test_pep257.py | 23 ----- py_pubsub/package.xml | 21 ---- py_pubsub/py_pubsub/__init__.py | 0 .../py_pubsub/publisher_member_function.py | 53 ----------- .../py_pubsub/subscriber_member_function.py | 51 ---------- py_pubsub/resource/py_pubsub | 0 py_pubsub/setup.cfg | 4 - py_pubsub/setup.py | 27 ------ py_pubsub/test/test_copyright.py | 25 ----- py_pubsub/test/test_flake8.py | 25 ----- py_pubsub/test/test_pep257.py | 23 ----- 29 files changed, 731 deletions(-) delete mode 100644 cpp_pubsub/CMakeLists.txt delete mode 100644 cpp_pubsub/package.xml delete mode 100644 cpp_pubsub/src/publisher_member_function.cpp delete mode 100644 cpp_pubsub/src/subscriber_member_function.cpp delete mode 100644 drone_sensors/CMakeLists.txt delete mode 100644 drone_sensors/package.xml delete mode 100644 drone_sensors/src/height_sensor.cpp delete mode 100644 my_package/CMakeLists.txt delete mode 100644 my_package/package.xml delete mode 100644 my_package/src/my_node.cpp delete mode 100644 my_package_py/my_package_py/__init__.py delete mode 100644 my_package_py/my_package_py/my_node_py.py delete mode 100644 my_package_py/package.xml delete mode 100644 my_package_py/resource/my_package_py delete mode 100644 my_package_py/setup.cfg delete mode 100644 my_package_py/setup.py delete mode 100644 my_package_py/test/test_copyright.py delete mode 100644 my_package_py/test/test_flake8.py delete mode 100644 my_package_py/test/test_pep257.py delete mode 100644 py_pubsub/package.xml delete mode 100644 py_pubsub/py_pubsub/__init__.py delete mode 100644 py_pubsub/py_pubsub/publisher_member_function.py delete mode 100644 py_pubsub/py_pubsub/subscriber_member_function.py delete mode 100644 py_pubsub/resource/py_pubsub delete mode 100644 py_pubsub/setup.cfg delete mode 100644 py_pubsub/setup.py delete mode 100644 py_pubsub/test/test_copyright.py delete mode 100644 py_pubsub/test/test_flake8.py delete mode 100644 py_pubsub/test/test_pep257.py diff --git a/cpp_pubsub/CMakeLists.txt b/cpp_pubsub/CMakeLists.txt deleted file mode 100644 index f199cabc..00000000 --- a/cpp_pubsub/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(cpp_pubsub) - -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) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) - -add_executable(talker src/publisher_member_function.cpp) -ament_target_dependencies(talker rclcpp std_msgs) -add_executable(listener src/subscriber_member_function.cpp) -ament_target_dependencies(listener rclcpp std_msgs) - -install(TARGETS - talker - listener - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/cpp_pubsub/package.xml b/cpp_pubsub/package.xml deleted file mode 100644 index 8553fb33..00000000 --- a/cpp_pubsub/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - cpp_pubsub - 0.0.0 - Example of publisher/subscriber nodes in ROS 2 for the 5G drone - ic - Apache License 2.0 - - ament_cmake - rclcpp - std_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/cpp_pubsub/src/publisher_member_function.cpp b/cpp_pubsub/src/publisher_member_function.cpp deleted file mode 100644 index c8d2ebfe..00000000 --- a/cpp_pubsub/src/publisher_member_function.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2016 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. - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; - -/* This example creates a subclass of Node and uses std::bind() to register a - * member function as a callback from the timer. */ - -class MinimalPublisher : public rclcpp::Node -{ -public: - MinimalPublisher() - : Node("minimal_publisher"), count_(0) - { - publisher_ = this->create_publisher("topic", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&MinimalPublisher::timer_callback, this)); - } - -private: - void timer_callback() - { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/cpp_pubsub/src/subscriber_member_function.cpp b/cpp_pubsub/src/subscriber_member_function.cpp deleted file mode 100644 index 1560403e..00000000 --- a/cpp_pubsub/src/subscriber_member_function.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2016 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. - -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" -using std::placeholders::_1; - -class MinimalSubscriber : public rclcpp::Node -{ -public: - MinimalSubscriber() - : Node("minimal_subscriber") - { - subscription_ = this->create_subscription( - "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); - } - -private: - void topic_callback(const std_msgs::msg::String::SharedPtr msg) const - { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); - } - rclcpp::Subscription::SharedPtr subscription_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/drone_sensors/CMakeLists.txt b/drone_sensors/CMakeLists.txt deleted file mode 100644 index 7368ee75..00000000 --- a/drone_sensors/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(drone_sensors) - -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) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) - -# add executable so that the node can be run with ros2 run -add_executable(height_sensor src/height_sensor.cpp) -ament_target_dependencies(height_sensor rclcpp std_msgs) - -# add install targets section so ros2 run can find the executable -install(TARGETS -height_sensor -DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/drone_sensors/package.xml b/drone_sensors/package.xml deleted file mode 100644 index a2ba2a45..00000000 --- a/drone_sensors/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - drone_sensors - 0.0.0 - package for the environment sensors of the 5G drone (height meter, object detection) - Sem van der Hoeven - Apache License 2.0 - - ament_cmake - - rclcpp - std_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp deleted file mode 100644 index 89f1237f..00000000 --- a/drone_sensors/src/height_sensor.cpp +++ /dev/null @@ -1,95 +0,0 @@ -#include //time measurement -#include // reading from serial port - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -#define TEXT_MODE 0x00, 0x11, 0x01, 0x45 -#define BINARY_MODE 0x00, 0x11, 0x02, 0x4C -#define SINGLE_PIXEL_MODE 0x00, 0x21, 0x01, 0xBC -#define TWO_BY_TWO_PIXEL_MODE 0x00, 0x21, 0x02, 0xB5 -#define TWO_PIXEL_MODE 0x00, 0x21, 0x03, 0xB2 -#define SHORT_RANGE_MODE 0x00, 0x61, 0x01, 0xE7 -#define LONG_RANGE_MODE 0x00, 0x61, 0x03, 0xE9 - -using namespace std::chrono_literals; // for time measurements - - - -class HeightSensorPublisher : public rclcpp::Node -{ -public: - HeightSensorPublisher() : Node("height_sensor_publisher") - { - publisher_ = this->create_publisher("height_sensor", 10); - timer_ = this->create_wall_timer( - 5ms, std::bind(&HeightSensorPublisher::timer_callback, this)); - RCLCPP_INFO(this->get_logger(), "Starting height sensor publisher"); - - setup_serial_port(); - } - -private: - /** - * @brief Timer callback function to publish the height sensor data - * - */ - void timer_callback() - { - char *readdata = new char[1]; - serial_port.read(readdata, 1); - auto message = std_msgs::msg::String(); - RCLCPP_INFO(this->get_logger(), "data: %s", readdata); - - if (readdata[0] == 0x54) // 0x54 = T (from the user manual) - { - RCLCPP_INFO(this->get_logger(), "Height sensor start measurement"); - char *measurement = new char[2]; - serial_port.read(measurement, 2); - message.data = "Height: " + std::to_string(measurement[0]) + std::to_string(measurement[1]); - RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); - publisher_->publish(message); - - } - } - - /** - * @brief Set the up serial port object to read from the sensor (/dev/ttyACM0) - * - */ - void setup_serial_port() - { - serial_port.open("/dev/ttyACM0", std::ios::in); - if (!serial_port.is_open()) - { - RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); - return; - } - else - { - RCLCPP_INFO(this->get_logger(), "Serial port opened on /dev/ttyACM0"); - } - - char bmode[] = {BINARY_MODE} - serial_port.write(bmode, 4); - } - - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - std::fstream serial_port; // serial port for reading from device -}; - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::Node::SharedPtr node = std::make_shared(); - executor.add_node(node); - executor.spin(); - // rclcpp::spin(std::make_shared()); - - rclcpp::shutdown(); - - return 0; -} diff --git a/my_package/CMakeLists.txt b/my_package/CMakeLists.txt deleted file mode 100644 index ac32ca48..00000000 --- a/my_package/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(my_package) - -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) - -add_executable(my_node src/my_node.cpp) -target_include_directories(my_node PUBLIC - $ - $) -target_compile_features(my_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - -install(TARGETS my_node - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/my_package/package.xml b/my_package/package.xml deleted file mode 100644 index 4bf230fe..00000000 --- a/my_package/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - my_package - 0.0.0 - TODO: Package description - ubuntu - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/my_package/src/my_node.cpp b/my_package/src/my_node.cpp deleted file mode 100644 index 0248b108..00000000 --- a/my_package/src/my_node.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include - -int main(int argc, char ** argv) -{ - (void) argc; - (void) argv; - - printf("hello world my_package package\n"); - return 0; -} diff --git a/my_package_py/my_package_py/__init__.py b/my_package_py/my_package_py/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/my_package_py/my_package_py/my_node_py.py b/my_package_py/my_package_py/my_node_py.py deleted file mode 100644 index ceb86f5c..00000000 --- a/my_package_py/my_package_py/my_node_py.py +++ /dev/null @@ -1,6 +0,0 @@ -def main(): - print('Hi from my_package_py.') - - -if __name__ == '__main__': - main() diff --git a/my_package_py/package.xml b/my_package_py/package.xml deleted file mode 100644 index cb3801eb..00000000 --- a/my_package_py/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - my_package_py - 0.0.0 - TODO: Package description - ubuntu - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/my_package_py/resource/my_package_py b/my_package_py/resource/my_package_py deleted file mode 100644 index e69de29b..00000000 diff --git a/my_package_py/setup.cfg b/my_package_py/setup.cfg deleted file mode 100644 index 8cc1be01..00000000 --- a/my_package_py/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/my_package_py -[install] -install_scripts=$base/lib/my_package_py diff --git a/my_package_py/setup.py b/my_package_py/setup.py deleted file mode 100644 index 564a8995..00000000 --- a/my_package_py/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import setup - -package_name = 'my_package_py' - -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='ubuntu', - maintainer_email='ubuntu@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'my_node_py = my_package_py.my_node_py:main' - ], - }, -) diff --git a/my_package_py/test/test_copyright.py b/my_package_py/test/test_copyright.py deleted file mode 100644 index 97a39196..00000000 --- a/my_package_py/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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 - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/my_package_py/test/test_flake8.py b/my_package_py/test/test_flake8.py deleted file mode 100644 index 27ee1078..00000000 --- a/my_package_py/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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/my_package_py/test/test_pep257.py b/my_package_py/test/test_pep257.py deleted file mode 100644 index b234a384..00000000 --- a/my_package_py/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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/py_pubsub/package.xml b/py_pubsub/package.xml deleted file mode 100644 index b2dae22b..00000000 --- a/py_pubsub/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - py_pubsub - 0.0.0 - Example of publisher/subscriber nodes in Python - ubuntu - Apache License 2.0 - - rclpy - std_msgs - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/py_pubsub/py_pubsub/__init__.py b/py_pubsub/py_pubsub/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/py_pubsub/py_pubsub/publisher_member_function.py b/py_pubsub/py_pubsub/publisher_member_function.py deleted file mode 100644 index dfdb0320..00000000 --- a/py_pubsub/py_pubsub/publisher_member_function.py +++ /dev/null @@ -1,53 +0,0 @@ -# Copyright 2016 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. - -import rclpy -from rclpy.node import Node - -from std_msgs.msg import String - - -class MinimalPublisher(Node): - - def __init__(self): - super().__init__('minimal_publisher') - self.publisher_ = self.create_publisher(String, 'topic', 10) - timer_period = 0.5 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - self.i = 0 - - def timer_callback(self): - msg = String() - msg.data = 'Hello World: %d' % self.i - self.publisher_.publish(msg) - self.get_logger().info('Publishing: "%s"' % msg.data) - self.i += 1 - - -def main(args=None): - rclpy.init(args=args) - - minimal_publisher = MinimalPublisher() - - rclpy.spin(minimal_publisher) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - minimal_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/py_pubsub/py_pubsub/subscriber_member_function.py b/py_pubsub/py_pubsub/subscriber_member_function.py deleted file mode 100644 index 78c72668..00000000 --- a/py_pubsub/py_pubsub/subscriber_member_function.py +++ /dev/null @@ -1,51 +0,0 @@ -# Copyright 2016 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. - -import rclpy -from rclpy.node import Node - -from std_msgs.msg import String - - -class MinimalSubscriber(Node): - - def __init__(self): - super().__init__('minimal_subscriber') - self.subscription = self.create_subscription( - String, - 'topic', - self.listener_callback, - 10) - self.subscription # prevent unused variable warning - - def listener_callback(self, msg): - self.get_logger().info('I heard: "%s"' % msg.data) - - -def main(args=None): - rclpy.init(args=args) - - minimal_subscriber = MinimalSubscriber() - - rclpy.spin(minimal_subscriber) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - minimal_subscriber.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/py_pubsub/resource/py_pubsub b/py_pubsub/resource/py_pubsub deleted file mode 100644 index e69de29b..00000000 diff --git a/py_pubsub/setup.cfg b/py_pubsub/setup.cfg deleted file mode 100644 index 3f886b98..00000000 --- a/py_pubsub/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/py_pubsub -[install] -install_scripts=$base/lib/py_pubsub diff --git a/py_pubsub/setup.py b/py_pubsub/setup.py deleted file mode 100644 index ebc173ef..00000000 --- a/py_pubsub/setup.py +++ /dev/null @@ -1,27 +0,0 @@ -from setuptools import setup - -package_name = 'py_pubsub' - -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='ubuntu', - maintainer_email='ubuntu@todo.todo', - description='Example of publisher/subscriber nodes in Python', - license='Apache License 2.0', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'talker = py_pubsub.publisher_member_function:main', - 'listener = py_pubsub.subscriber_member_function:main', - ], - }, -) diff --git a/py_pubsub/test/test_copyright.py b/py_pubsub/test/test_copyright.py deleted file mode 100644 index 97a39196..00000000 --- a/py_pubsub/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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 - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/py_pubsub/test/test_flake8.py b/py_pubsub/test/test_flake8.py deleted file mode 100644 index 27ee1078..00000000 --- a/py_pubsub/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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/py_pubsub/test/test_pep257.py b/py_pubsub/test/test_pep257.py deleted file mode 100644 index b234a384..00000000 --- a/py_pubsub/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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' -- 2.47.2 From 85390c0d4d4ee42d1e15c837c968e399ea43c5da Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 12:47:15 +0000 Subject: [PATCH 21/32] add height sensor files --- src/height/CMakeLists.txt | 45 ++++++++++++++++++++++++++++++++ src/height/package.xml | 20 ++++++++++++++ src/height/src/height_reader.cpp | 10 +++++++ 3 files changed, 75 insertions(+) create mode 100644 src/height/CMakeLists.txt create mode 100644 src/height/package.xml create mode 100644 src/height/src/height_reader.cpp diff --git a/src/height/CMakeLists.txt b/src/height/CMakeLists.txt new file mode 100644 index 00000000..96011584 --- /dev/null +++ b/src/height/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(height) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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) +find_package(rclcpp REQUIRED) + +add_executable(height_reader src/height_reader.cpp) +target_include_directories(height_reader PUBLIC + $ + $) +ament_target_dependencies( + height_reader + "rclcpp" +) + +install(TARGETS height_reader + DESTINATION lib/${PROJECT_NAME}) + +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/height/package.xml b/src/height/package.xml new file mode 100644 index 00000000..78221ce1 --- /dev/null +++ b/src/height/package.xml @@ -0,0 +1,20 @@ + + + + height + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp new file mode 100644 index 00000000..6b157114 --- /dev/null +++ b/src/height/src/height_reader.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world height package\n"); + return 0; +} -- 2.47.2 From 8cbf27cd630d81f994119e633d132e0bd51ae532 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 14:52:31 +0200 Subject: [PATCH 22/32] add dependencies for terabee api --- src/height/CMakeLists.txt | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/height/CMakeLists.txt b/src/height/CMakeLists.txt index 96011584..8cd4b7d9 100644 --- a/src/height/CMakeLists.txt +++ b/src/height/CMakeLists.txt @@ -18,14 +18,21 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(TerabeeApi REQUIRED) +find_package(height REQUIRED) add_executable(height_reader src/height_reader.cpp) target_include_directories(height_reader PUBLIC $ $) + + target_link_libraries(height_reader ${TerabeeApi_LIBRARIES}) + target_include_directories(height_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) + ament_target_dependencies( height_reader - "rclcpp" + rclcpp + height ) install(TARGETS height_reader -- 2.47.2 From 5f98cb2f10fcdc489b760617022af0a37a574dcb Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 15:12:34 +0200 Subject: [PATCH 23/32] add node with basic code to read sensor --- src/height/src/height_reader.cpp | 81 ++++++++++++++++++++++++++++++-- 1 file changed, 76 insertions(+), 5 deletions(-) diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index 6b157114..5e4ebfa3 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -1,10 +1,81 @@ -#include +#include +#include -int main(int argc, char ** argv) +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +using terabee::DistanceData; +using terabee::ITerarangerEvoMini; + +std::ostream &operator<<(std::ostream &os, const DistanceData &d) { - (void) argc; - (void) argv; + os << "["; + for (size_t i = 0; i < d.distance.size(); i++) + { + os << d.distance[i] << ", "; + } + os << "\b\b" + << " ]"; + return os; +} + +class HeightReader : public rclcpp::Node +{ +public: + HeightReader() : rclcpp::Node("height_reader") + { + rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + descriptor.description = "serial port for the USB port of the height sensor"; + + this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); + + factory = terabee::ITerarangerFactory::getFactory(); + evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); + + if (!evo_mini) + { + RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); + return; + } + + evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); + + if (!evo_mini->initialize()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); + + timer_ = this->create_wall_timer(500ms, std::bind(HeightReader::read_height, this)); + } + +private: + void read_height() + { + std::cout << "Distance = " << evo_mini->getDistance() << std::endl; + } + + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr factory; + std::unique_ptr evo_mini; +}; + +int main(int argc, char **argv) +{ + (void)argc; + (void)argv; + + rclcpp::init(argc, argv); + + rclcpp::shutdown(); - printf("hello world height package\n"); return 0; } -- 2.47.2 From b258131480ee9b019f366aba4c1d0577300c0791 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 13:18:42 +0000 Subject: [PATCH 24/32] Revert "add node with basic code to read sensor" This reverts commit 5f98cb2f10fcdc489b760617022af0a37a574dcb. --- src/height/src/height_reader.cpp | 81 ++------------------------------ 1 file changed, 5 insertions(+), 76 deletions(-) diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index 5e4ebfa3..6b157114 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -1,81 +1,10 @@ -#include -#include +#include -#include "rclcpp/rclcpp.hpp" - -#include -#include -#include - -using namespace std::chrono_literals; - -using terabee::DistanceData; -using terabee::ITerarangerEvoMini; - -std::ostream &operator<<(std::ostream &os, const DistanceData &d) +int main(int argc, char ** argv) { - os << "["; - for (size_t i = 0; i < d.distance.size(); i++) - { - os << d.distance[i] << ", "; - } - os << "\b\b" - << " ]"; - return os; -} - -class HeightReader : public rclcpp::Node -{ -public: - HeightReader() : rclcpp::Node("height_reader") - { - rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - descriptor.description = "serial port for the USB port of the height sensor"; - - this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); - - factory = terabee::ITerarangerFactory::getFactory(); - evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); - - if (!evo_mini) - { - RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); - return; - } - - evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); - - if (!evo_mini->initialize()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); - return; - } - - RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); - - timer_ = this->create_wall_timer(500ms, std::bind(HeightReader::read_height, this)); - } - -private: - void read_height() - { - std::cout << "Distance = " << evo_mini->getDistance() << std::endl; - } - - rclcpp::TimerBase::SharedPtr timer_; - - std::unique_ptr factory; - std::unique_ptr evo_mini; -}; - -int main(int argc, char **argv) -{ - (void)argc; - (void)argv; - - rclcpp::init(argc, argv); - - rclcpp::shutdown(); + (void) argc; + (void) argv; + printf("hello world height package\n"); return 0; } -- 2.47.2 From 1c1ea30dc079091c1472b1ee33e46b845fdf5f0b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 13:36:08 +0000 Subject: [PATCH 25/32] created node to read basics of height sensor --- src/beacon_positioning/CMakeLists.txt | 4 +- src/height/CMakeLists.txt | 3 +- src/height/package.xml | 4 +- src/height/src/height_reader.cpp | 79 +++++++++++++++++++++++++-- src/object_detection/CMakeLists.txt | 6 +- 5 files changed, 82 insertions(+), 14 deletions(-) diff --git a/src/beacon_positioning/CMakeLists.txt b/src/beacon_positioning/CMakeLists.txt index 8841d161..480f22e6 100644 --- a/src/beacon_positioning/CMakeLists.txt +++ b/src/beacon_positioning/CMakeLists.txt @@ -24,14 +24,14 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(positioning_systems_api REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(beacon_positioning REQUIRED) +#find_package(beacon_positioning REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/TrackerPosition.msg" # message for tracker position ) add_executable(tracker_position src/tracker_position.cpp) -ament_target_dependencies(tracker_position rclcpp std_msgs beacon_positioning) +ament_target_dependencies(tracker_position rclcpp std_msgs) target_link_libraries(tracker_position positioning_systems_api::serial_communication diff --git a/src/height/CMakeLists.txt b/src/height/CMakeLists.txt index 8cd4b7d9..d059c6d6 100644 --- a/src/height/CMakeLists.txt +++ b/src/height/CMakeLists.txt @@ -19,7 +19,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) -find_package(height REQUIRED) add_executable(height_reader src/height_reader.cpp) target_include_directories(height_reader PUBLIC @@ -32,7 +31,7 @@ target_include_directories(height_reader PUBLIC ament_target_dependencies( height_reader rclcpp - height + TerabeeApi ) install(TARGETS height_reader diff --git a/src/height/package.xml b/src/height/package.xml index 78221ce1..8a5fefe4 100644 --- a/src/height/package.xml +++ b/src/height/package.xml @@ -3,9 +3,9 @@ height 0.0.0 - TODO: Package description + package to read Terabee node ubuntu - TODO: License declaration + Apache License 2.0 ament_cmake diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index 6b157114..3ba06530 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -1,10 +1,79 @@ -#include +#include +#include -int main(int argc, char ** argv) +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +using terabee::DistanceData; +using terabee::ITerarangerEvoMini; + +std::ostream &operator<<(std::ostream &os, const DistanceData &d) { - (void) argc; - (void) argv; + os << "["; + for (size_t i = 0; i < d.distance.size(); i++) + { + os << d.distance[i] << ", "; + } + os << "\b\b" + << " ]"; + return os; +} + +class HeightReader : public rclcpp::Node +{ +public: + HeightReader() : rclcpp::Node("height_reader") + { + rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + descriptor.description = "serial port for the USB port of the height sensor"; + + this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); + + factory = terabee::ITerarangerFactory::getFactory(); + evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); + + if (!evo_mini) + { + RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); + return; + } + + evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); + + if (!evo_mini->initialize()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); + + timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); + } + +private: + void read_height() + { + std::cout << "Distance = " << evo_mini->getDistance() << std::endl; + } + + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr factory; + std::unique_ptr evo_mini; +}; + +int main(int argc, char **argv) +{ + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); - printf("hello world height package\n"); return 0; } diff --git a/src/object_detection/CMakeLists.txt b/src/object_detection/CMakeLists.txt index d5009b96..35d60383 100644 --- a/src/object_detection/CMakeLists.txt +++ b/src/object_detection/CMakeLists.txt @@ -23,7 +23,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(object_detection REQUIRED) +#find_package(object_detection REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/LidarReading.msg" @@ -31,13 +31,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) add_executable(lidar_reader src/lidar_reader.cpp) -ament_target_dependencies(lidar_reader rclcpp object_detection) +ament_target_dependencies(lidar_reader rclcpp) target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES}) target_include_directories(lidar_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) add_executable(multiflex_reader src/multiflex_reader.cpp) -ament_target_dependencies(multiflex_reader rclcpp object_detection) +ament_target_dependencies(multiflex_reader rclcpp) target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES}) target_include_directories(multiflex_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) -- 2.47.2 From df18c9bba649d3cceb5d50249403c08a1a8e7e17 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 15:38:16 +0200 Subject: [PATCH 26/32] add height message definition --- src/height/CMakeLists.txt | 4 ++++ src/height/msg/HeightData.msg | 2 ++ 2 files changed, 6 insertions(+) create mode 100644 src/height/msg/HeightData.msg diff --git a/src/height/CMakeLists.txt b/src/height/CMakeLists.txt index d059c6d6..74deb716 100644 --- a/src/height/CMakeLists.txt +++ b/src/height/CMakeLists.txt @@ -20,6 +20,10 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/HeightData.msg" + ) + add_executable(height_reader src/height_reader.cpp) target_include_directories(height_reader PUBLIC $ diff --git a/src/height/msg/HeightData.msg b/src/height/msg/HeightData.msg new file mode 100644 index 00000000..88d28cb5 --- /dev/null +++ b/src/height/msg/HeightData.msg @@ -0,0 +1,2 @@ +float32[4] heights +float32 min_height \ No newline at end of file -- 2.47.2 From ed542e2f21ec5476dff8345a39fc5d9d09d08d64 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 15:41:57 +0200 Subject: [PATCH 27/32] update package.xml of height package --- src/height/package.xml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/height/package.xml b/src/height/package.xml index 8a5fefe4..ceb7933a 100644 --- a/src/height/package.xml +++ b/src/height/package.xml @@ -8,12 +8,19 @@ Apache License 2.0 ament_cmake + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages rclcpp ament_lint_auto ament_lint_common + TerabeeApi + TerabeeApi + + ament_cmake -- 2.47.2 From 8fb86a5e2d35e191d5b965310217c20b68b0a62a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 15:48:50 +0200 Subject: [PATCH 28/32] add publishing of topic --- src/height/src/height_reader.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index 3ba06530..84662f9f 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -2,6 +2,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "height/msg/height_data.hpp" #include #include @@ -54,15 +55,30 @@ public: RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); + publisher_ = this->create_publisher("drone/height", 10); } private: void read_height() { - std::cout << "Distance = " << evo_mini->getDistance() << std::endl; + auto msg = height::msg::HeightData(); + + float min = 10000000; + terabee::DistanceData distance_data = evo_mini->getDistance(); + for (size_t i = 0; i < distance_data.size(); i++) + { + msg.heights[i] = distance_data.distance[i]; + if (distance_data.distance[i] < min) + { + min = distance_data.distance[i]; + } + } + msg.min_height = min; + publisher_->publish(msg); } rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; std::unique_ptr factory; std::unique_ptr evo_mini; -- 2.47.2 From 8aa5e6fe35a552019feff98008583311842b7721 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 17:19:22 +0200 Subject: [PATCH 29/32] add generators to cmake --- src/height/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/height/CMakeLists.txt b/src/height/CMakeLists.txt index 74deb716..25565ac2 100644 --- a/src/height/CMakeLists.txt +++ b/src/height/CMakeLists.txt @@ -19,6 +19,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) +find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/HeightData.msg" -- 2.47.2 From 09cda4e1d056bbf534eb729f5f7d30026db1d005 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 17:20:42 +0200 Subject: [PATCH 30/32] bruh --- src/height/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/height/CMakeLists.txt b/src/height/CMakeLists.txt index 25565ac2..32394387 100644 --- a/src/height/CMakeLists.txt +++ b/src/height/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(height REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/HeightData.msg" @@ -37,6 +38,7 @@ ament_target_dependencies( height_reader rclcpp TerabeeApi + height ) install(TARGETS height_reader -- 2.47.2 From 48b501689784a0edff40054585a54a1e0938f8d6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 15:41:11 +0000 Subject: [PATCH 31/32] why --- src/beacon_positioning/CMakeLists.txt | 6 ++++-- src/height/src/height_reader.cpp | 6 ++++-- src/object_detection/CMakeLists.txt | 8 ++++++-- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/beacon_positioning/CMakeLists.txt b/src/beacon_positioning/CMakeLists.txt index 480f22e6..8ce3803a 100644 --- a/src/beacon_positioning/CMakeLists.txt +++ b/src/beacon_positioning/CMakeLists.txt @@ -24,14 +24,16 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(positioning_systems_api REQUIRED) find_package(rosidl_default_generators REQUIRED) -#find_package(beacon_positioning REQUIRED) +find_package(beacon_positioning REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/TrackerPosition.msg" # message for tracker position ) add_executable(tracker_position src/tracker_position.cpp) -ament_target_dependencies(tracker_position rclcpp std_msgs) +ament_target_dependencies(tracker_position rclcpp std_msgs + beacon_positioning +) target_link_libraries(tracker_position positioning_systems_api::serial_communication diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index 84662f9f..d8f70b06 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -63,18 +63,20 @@ private: { auto msg = height::msg::HeightData(); - float min = 10000000; + float min = 255; terabee::DistanceData distance_data = evo_mini->getDistance(); for (size_t i = 0; i < distance_data.size(); i++) { msg.heights[i] = distance_data.distance[i]; - if (distance_data.distance[i] < min) + if (distance_data.distance[i] < min && distance_data.distance[i] >= 0) { min = distance_data.distance[i]; } } msg.min_height = min; publisher_->publish(msg); + + RCLCPP_INFO(this->get_logger(),"publishing message with min distance %f",msg.min_height); } rclcpp::TimerBase::SharedPtr timer_; diff --git a/src/object_detection/CMakeLists.txt b/src/object_detection/CMakeLists.txt index 35d60383..9c705f64 100644 --- a/src/object_detection/CMakeLists.txt +++ b/src/object_detection/CMakeLists.txt @@ -31,13 +31,17 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) add_executable(lidar_reader src/lidar_reader.cpp) -ament_target_dependencies(lidar_reader rclcpp) +ament_target_dependencies(lidar_reader rclcpp +# object_detection +) target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES}) target_include_directories(lidar_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) add_executable(multiflex_reader src/multiflex_reader.cpp) -ament_target_dependencies(multiflex_reader rclcpp) +ament_target_dependencies(multiflex_reader rclcpp +# object_detection +) target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES}) target_include_directories(multiflex_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) -- 2.47.2 From 9af542d4adfce0dd13e8d7167a3aa04ecd90cd88 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 09:11:44 +0000 Subject: [PATCH 32/32] everything built --- .../src/tracker_position.cpp | 4 +-- src/object_detection/CMakeLists.txt | 6 ++-- src/object_detection/src/lidar_reader.cpp | 28 +++++++++---------- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/beacon_positioning/src/tracker_position.cpp b/src/beacon_positioning/src/tracker_position.cpp index c7f35511..0b4d9f52 100644 --- a/src/beacon_positioning/src/tracker_position.cpp +++ b/src/beacon_positioning/src/tracker_position.cpp @@ -149,7 +149,7 @@ int main(int argc, char **argv) if (tracker_msg.is_valid_position) { RCLCPP_INFO(node->get_logger(), "x = %f, y = %f, z = %f", tracker_msg.tracker_position_xyz.at(0), tracker_msg.tracker_position_xyz.at(1), tracker_msg.tracker_position_xyz.at(2)); - for (int i = 0; i < tracker_msg.anchors_data.size(); i++) + for (size_t i = 0; i < tracker_msg.anchors_data.size(); i++) { RCLCPP_INFO(node->get_logger(), "anchor number= %d, distance = %d, x = %d, y = %d, z = %d", tracker_msg.anchors_data[i].number, tracker_msg.anchors_data[i].distance, tracker_msg.anchors_data[i].pos_x, tracker_msg.anchors_data[i].pos_y, tracker_msg.anchors_data[i].pos_z); } @@ -160,7 +160,7 @@ int main(int argc, char **argv) message.y_pos = tracker_msg.tracker_position_xyz.at(1); message.z_pos = tracker_msg.tracker_position_xyz.at(2); message.anchor_distances = {0, 0, 0, 0}; - for (int i = 0; i < tracker_msg.anchors_data.size(); i++) + for (size_t i = 0; i < tracker_msg.anchors_data.size(); i++) { message.anchor_distances[i] = tracker_msg.anchors_data[i].distance; } diff --git a/src/object_detection/CMakeLists.txt b/src/object_detection/CMakeLists.txt index 9c705f64..be10d823 100644 --- a/src/object_detection/CMakeLists.txt +++ b/src/object_detection/CMakeLists.txt @@ -23,7 +23,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) find_package(rosidl_default_generators REQUIRED) -#find_package(object_detection REQUIRED) +find_package(object_detection REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/LidarReading.msg" @@ -32,7 +32,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} add_executable(lidar_reader src/lidar_reader.cpp) ament_target_dependencies(lidar_reader rclcpp -# object_detection + object_detection ) target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES}) @@ -40,7 +40,7 @@ target_include_directories(lidar_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) add_executable(multiflex_reader src/multiflex_reader.cpp) ament_target_dependencies(multiflex_reader rclcpp -# object_detection + object_detection ) target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES}) diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp index 31647ba8..eaa98bd8 100644 --- a/src/object_detection/src/lidar_reader.cpp +++ b/src/object_detection/src/lidar_reader.cpp @@ -57,7 +57,7 @@ public: this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor); - publisher_ = this->create_publisher("drone/object_detection", 10); + // publisher_ = this->create_publisher("drone/object_detection", 10); timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc); @@ -87,24 +87,24 @@ private: // std::cout << "Distance = " << tower->getDistance() << std::endl; // std::cout << "IMU = " << tower->getImuData() << std::endl; - auto msg = object_detection::msg::LidarReading(); + // auto msg = object_detection::msg::LidarReading(); - msg.sensor_1 = tower->getDistance().distance.at(0); - msg.sensor_2 = tower->getDistance().distance.at(2); - msg.sensor_3 = tower->getDistance().distance.at(4); - msg.sensor_4 = tower->getDistance().distance.at(6); + // msg.sensor_1 = tower->getDistance().distance.at(0); + // msg.sensor_2 = tower->getDistance().distance.at(2); + // msg.sensor_3 = tower->getDistance().distance.at(4); + // msg.sensor_4 = tower->getDistance().distance.at(6); - ImuData imu_data = tower->getImuData(); + // ImuData imu_data = tower->getImuData(); - for (size_t i = 0; i < imu_data.data.size(); i++) - { - msg.imu_data.push_back(imu_data.data[i]); - } - publisher_->publish(msg); - RCLCPP_INFO(this->get_logger(), "Publishing message"); + // for (size_t i = 0; i < imu_data.data.size(); i++) + // { + // msg.imu_data.push_back(imu_data.data[i]); + // } + // // publisher_->publish(msg); + // RCLCPP_INFO(this->get_logger(), "Publishing message"); } - rclcpp::Publisher::SharedPtr publisher_; + // rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr timer_; // terabee tower evo variables -- 2.47.2