diff --git a/ros2/naive/CMakeLists.txt b/ros2/naive/CMakeLists.txt new file mode 100644 index 0000000..f53d608 --- /dev/null +++ b/ros2/naive/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.16) +project(naive CXX) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +add_executable(naive src/node.cpp src/incrementer.cpp) +target_include_directories(naive PRIVATE include) +ament_target_dependencies(naive rclcpp std_msgs) + +install(TARGETS + naive + 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/ros2/naive/include/naive/incrementer.hpp b/ros2/naive/include/naive/incrementer.hpp new file mode 100644 index 0000000..a93f7c0 --- /dev/null +++ b/ros2/naive/include/naive/incrementer.hpp @@ -0,0 +1,55 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#pragma once + +#include +#include +#include +#include + +class Incrementer { +public: + /** + * @brief Incrementer constructor + * + * @param[in] node Shared pointer to a ROS node + * @param[in] in_topic ROS topic to the Incrementer subscribes to + * @param[in] out_topic ROS topic that the Incrementer publishes to + */ + Incrementer(std::shared_ptr node, std::string const& in_topic, std::string const& out_topic); +private: + std::shared_ptr node_; + std::shared_ptr> pub_; + std::shared_ptr> sub_; +}; diff --git a/ros2/naive/package.xml b/ros2/naive/package.xml new file mode 100644 index 0000000..b627d1e --- /dev/null +++ b/ros2/naive/package.xml @@ -0,0 +1,21 @@ + + + + naive + 0.0.0 + Reference pub/sub example + bilal + MIT + + ament_cmake + + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2/naive/src/incrementer.cpp b/ros2/naive/src/incrementer.cpp new file mode 100644 index 0000000..3388713 --- /dev/null +++ b/ros2/naive/src/incrementer.cpp @@ -0,0 +1,19 @@ +#include "naive/incrementer.hpp" + +#include +#include +#include + +namespace { + constexpr auto queue_size = 10; +} + +Incrementer::Incrementer(std::shared_ptr node, std::string const& in_topic, std::string const& out_topic) +: node_{std::move(node)}, + pub_{node_->create_publisher(out_topic,queue_size)}, + sub_{node_->create_subscription(in_topic, queue_size, + [this](std_msgs::msg::Int64::UniquePtr const msg) { + std_msgs::msg::Int64 incremented; + incremented.data = msg->data + 1; + pub_->publish(incremented); + })} {} diff --git a/ros2/naive/src/node.cpp b/ros2/naive/src/node.cpp new file mode 100644 index 0000000..266c1d1 --- /dev/null +++ b/ros2/naive/src/node.cpp @@ -0,0 +1,12 @@ +#include "naive/incrementer.hpp" +#include + +int main(int argc, char **argv) { + + rclcpp::init(argc,argv); + auto const node = std::make_shared("incrementer"); + Incrementer incrementer{node, "/in", "/out"}; + rclcpp::spin(node); + rclcpp::shutdown(); + +}