Ros2

.gitignore

@@ -1 +1,8 @@
 CMakeLists.txt.user*
+/build/
+/log/
+/install/
+.txt
+*.txt
+settings.json
+*.pyc

CMakeLists.txt

@@ -1,33 +1,27 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(emc_system)
 
+set(CMAKE_CXX_STANDARD 14)
+
 add_compile_options(-Wall -Werror=all)
 add_compile_options(-Wextra -Werror=extra)
 
-find_package(catkin REQUIRED COMPONENTS
-    geometry_msgs
-    sensor_msgs
-    nav_msgs
-    roscpp
-    tf2
-    tf2_ros    
-)
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+
 
 # find_package(Boost REQUIRED COMPONENTS system program_options)
 # find_package(PCL REQUIRED)
 find_package(OpenCV REQUIRED)
 
-# ------------------------------------------------------------------------------------------------
-#                                          CATKIN EXPORT
-# ------------------------------------------------------------------------------------------------
-
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES bla
-#  CATKIN_DEPENDS other_catkin_pkg
-#  DEPENDS system_lib
-)
-
 # ------------------------------------------------------------------------------------------------
 #                                              BUILD
 # ------------------------------------------------------------------------------------------------
@@ -36,67 +30,86 @@ include_directories(
     include
     ${catkin_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
+    ${tf2_geometry_msgs_INCLUDE_DIRS}
 )
 
-add_library(emc_system
+add_library(emc-framework SHARED
     include/emc/engine.h
     include/emc/communication.h
     include/emc/data.h
     include/emc/odom.h
     include/emc/io.h
     include/emc/rate.h
     include/emc/bumper.h
+    include/emc/ros2subscriber.h
+    include/emc/ros2publisher.h
 
     src/engine.cpp
     src/communication.cpp
-    src/data.cpp
+    #src/data.cpp
     src/io.cpp
     src/rate.cpp
 )
-target_link_libraries(emc_system ${catkin_LIBRARIES})
+ament_target_dependencies(emc-framework geometry_msgs sensor_msgs visualization_msgs nav_msgs rclcpp tf2 tf2_ros tf2_geometry_msgs)
+
+set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}")
+set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
 
 # ------------------------------------------------------------------------------------------------
 #                                              TOOLS
 # ------------------------------------------------------------------------------------------------
 
 add_executable(emc_viz tools/visualize.cpp)
-target_link_libraries(emc_viz emc_system ${OpenCV_LIBRARIES})
+target_link_libraries(emc_viz emc-framework ${OpenCV_LIBRARIES})
 
 # ------------------------------------------------------------------------------------------------
 #                                               IO
 # ------------------------------------------------------------------------------------------------
 
 add_executable(emc_test_io examples/test_io.cpp)
-target_link_libraries(emc_test_io emc_system)
+target_link_libraries(emc_test_io emc-framework)
 
-add_executable(testspeech examples/testspeech.cpp)
-target_link_libraries(testspeech emc_system)
+add_executable(emc_test_speech examples/testspeech.cpp)
+target_link_libraries(emc_test_speech emc-framework)
 
 add_executable(test_send_path examples/test_sendpath.cpp)
-target_link_libraries(test_send_path emc_system)
+target_link_libraries(test_send_path emc-framework)
 
 add_executable(test_send_pose examples/test_sendpose.cpp)
-target_link_libraries(test_send_pose emc_system)
+target_link_libraries(test_send_pose emc-framework)
 
 # ------------------------------------------------------------------------------------------------
 #                                             EXAMPLES
 # ------------------------------------------------------------------------------------------------
 
 add_executable(emc_example1 examples/example01.cpp)
-target_link_libraries(emc_example1 emc_system)
+target_link_libraries(emc_example1 emc-framework)
 
-add_executable(emc_example2 examples/example02.cpp)
-target_link_libraries(emc_example2 emc_system)
+add_executable(emc_test_subscriber examples/test_subscriber.cpp)
+target_link_libraries(emc_test_subscriber emc-framework)
+
+add_executable(emc_test_publisher examples/test_publisher.cpp)
+target_link_libraries(emc_test_publisher emc-framework)
 
-add_executable(emc_example3 examples/example03.cpp)
-target_link_libraries(emc_example3 emc_system)
+add_executable(emc_example2 examples/example02.cpp)
+target_link_libraries(emc_example2 emc-framework)
 
 # ------------------------------------------------------------------------------------------------
 #                                             UNIT TESTS
 # ------------------------------------------------------------------------------------------------
 
-if(CATKIN_ENABLE_TESTING)
-    find_package(rostest REQUIRED)
-    add_rostest_gtest(tests_io test/io.test test/test_io.cpp)
-    target_link_libraries(tests_io emc_system ${catkin_LIBRARIES})
+if(BUILD_TESTING)
+   find_package(ament_cmake_gtest REQUIRED)
+   ament_add_gtest(tests_io test/test_io.cpp)
+   target_link_libraries(tests_io emc-framework)
+   ament_target_dependencies(tests_io geometry_msgs sensor_msgs nav_msgs rclcpp tf2 tf2_ros)
 endif()
+
+#############
+## Install ##
+#############
+install(TARGETS emc-framework emc_example1 emc_example2  emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher 
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

examples/example02.cpp

@@ -1,4 +1,5 @@
 #include <emc/engine.h>
+#include <emc/data.h>
 
 #include <iostream>
 

examples/test_io.cpp

@@ -5,12 +5,15 @@
 
 int main()
 {
+    std::cout << "initialising" << std::endl;
     // Create IO object, which will initialize the io layer
     emc::IO io;
 
+    std::cout << "creating rate object" << std::endl;
     // Create Rate object, which will help using keeping the loop at a fixed frequency
     emc::Rate r(10);
 
+    std::cout << "starting loop" << std::endl;
     // Loop while we are properly connected
     while(io.ok())
     {
@@ -20,10 +23,14 @@ int main()
         emc::OdometryData odom;
         if (io.readOdometryData(odom))
             std::cout << "Odometry: " << odom.x << ", " << odom.y << ", " << odom.a << std::endl;
+        else
+            std::cout << "No Odom received" << std::endl;
 
         emc::LaserData scan;
         if (io.readLaserData(scan))
             std::cout << "Laser: " << scan.ranges.size() << " beams" << std::endl;
+        else
+            std::cout << "No laser received" << std::endl;
 
         // Sleep remaining time
         r.sleep();

examples/test_publisher.cpp

@@ -0,0 +1,28 @@
+#include "emc/ros2publisher.h"
+
+#include <emc/rate.h>
+#include "rclcpp/rclcpp.hpp"
+
+int main()
+{
+    rclcpp::init(0,nullptr);
+
+    // Create publisher
+    emc::Ros2Publisher pub;
+
+    // Create Rate object, which will help using keeping the loop at a fixed frequency
+    emc::Rate r(10);
+
+    // Loop while we are properly connected
+    while(rclcpp::ok())
+    {
+        std::cout << "sending msgs" << std::endl;
+        pub.sendBaseVelocity(1, 0, 0.1);
+        pub.speak("hello world");
+
+        // Sleep remaining time
+        r.sleep();
+    }
+
+    return 0;
+}

examples/test_subscriber.cpp

@@ -0,0 +1,36 @@
+#include "emc/ros2subscriber.h"
+
+#include <emc/rate.h>
+#include "rclcpp/rclcpp.hpp"
+
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+int main()
+{
+    rclcpp::init(0,nullptr);
+
+    // Create subscriber
+    std::shared_ptr<emc::Ros2Subscriber<sensor_msgs::msg::LaserScan>> laser_node = std::make_shared<emc::Ros2Subscriber<sensor_msgs::msg::LaserScan>>("laser_scan", "emc_laser");
+    rclcpp::executors::SingleThreadedExecutor executor;
+
+    executor.add_node(laser_node);
+
+    // Create Rate object, which will help using keeping the loop at a fixed frequency
+    emc::Rate r(10);
+
+    // Loop while we are properly connected
+    while(rclcpp::ok())
+    {
+        std::cout << "spin" <<std::endl;
+        executor.spin_once(std::chrono::nanoseconds(0)); // wait 0 nanoseconds for new messages. just empty the buffer.
+        sensor_msgs::msg::LaserScan msg;
+        bool newdata = laser_node->readMsg(msg);
+        std::cout << "newdata: " << newdata << std::endl;
+        std::cout << "Laser: " << msg.range_min << " range_min" << std::endl;
+
+        // Sleep remaining time
+        r.sleep();
+    }
+
+    return 0;
+}