Ros2 devel

CMakeLists.txt

@@ -1,102 +1,128 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(laser_assembler)
 
-##############################################################################
-# Find dependencies
-##############################################################################
-
-set(THIS_PACKAGE_ROS_DEPS 
-tf sensor_msgs message_filters roscpp laser_geometry filters)
-
-find_package(catkin REQUIRED COMPONENTS 
-  ${THIS_PACKAGE_ROS_DEPS} 
-  message_generation)
-find_package(Boost REQUIRED COMPONENTS system signals)
-include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
-
-##############################################################################
-# Build service definitions
-##############################################################################
-add_service_files(FILES
-AssembleScans.srv
-AssembleScans2.srv)
-
-generate_messages(DEPENDENCIES sensor_msgs std_msgs)
-
-##############################################################################
-# Define package
-##############################################################################
-
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} message_runtime
-  DEPENDS
-  )
-
-##############################################################################
-# Build
-##############################################################################
+if(NOT WIN32)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+endif()
 
-add_executable(laser_scan_assembler_srv src/laser_scan_assembler_srv.cpp)
-target_link_libraries(laser_scan_assembler_srv ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(laser_scan_assembler_srv ${PROJECT_NAME}_gencpp)
+set( filters_FOUND 1 )
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(laser_geometry REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(laser_assembler_srv_gen REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(filters REQUIRED)
+
+set(INCLUDE_DIRS include ${ament_cmake_INCLUDE_DIRS}
+    ${rosidl_default_generators_INCLUDE_DIRS} ${signals_INCLUDE_DIRS}
+    ${builtin_interfaces_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS}
+    ${sensor_msgs_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS}
+    ${laser_geometry_INCLUDE_DIRS}
+    ${tf2_ros_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS}
+    ${message_filters_INCLUDE_DIRS}
+    ${laser_assembler_srv_gen_INCLUDE_DIRS}
+    ${rostest_INCLUDE_DIRS} ${system_INCLUDE_DIRS}
+    ${geometry_msgs_INCLUDE_DIRS}
+    ${filters_INCLUDE_DIRS})
+
+include_directories(${INCLUDE_DIRS})
+
+set(LIBS ${rclcpp_LIBRARIES} ${ament_cmake_LIBRARIES}
+    ${rosidl_default_generators_LIBRARIES} ${signals_LIBRARIES} 
+    ${laser_assembler_srv_gen_LIBRARIES} ${std_msgs_LIBRARIES}
+    ${system_LIBRARIES} ${tf2_ros_LIBRARIES} ${tf2_LIBRARIES} 
+    ${laser_geometry_LIBRARIES}
+    ${geometry_msgs_LIBRARIES}
+    ${message_filters_LIBRARIES} ${sensor_msgs_LIBRARIES} 
+    ${filters_LIBRARIES})
+
+# add_executable(laser_scan_assembler_srv src/laser_scan_assembler_srv.cpp)
+# target_link_libraries(laser_scan_assembler_srv ${LIBS} ${Boost_LIBRARIES})
 
 add_executable(laser_scan_assembler src/laser_scan_assembler.cpp)
-target_link_libraries(laser_scan_assembler ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(laser_scan_assembler ${PROJECT_NAME}_gencpp)
-
-add_executable(merge_clouds src/merge_clouds.cpp)
-target_link_libraries(merge_clouds ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(merge_clouds ${PROJECT_NAME}_gencpp)
-
-add_executable(point_cloud_assembler_srv src/point_cloud_assembler_srv.cpp)
-target_link_libraries(point_cloud_assembler_srv ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(point_cloud_assembler_srv ${PROJECT_NAME}_gencpp)
-
-add_executable(point_cloud_assembler src/point_cloud_assembler.cpp)
-target_link_libraries(point_cloud_assembler ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(point_cloud_assembler ${PROJECT_NAME}_gencpp)
-
-add_executable(point_cloud2_assembler src/point_cloud2_assembler.cpp)
-target_link_libraries(point_cloud2_assembler ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(point_cloud2_assembler ${PROJECT_NAME}_gencpp)
-
-## unit testing
-
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest)
+target_link_libraries(laser_scan_assembler ${LIBS})
+install(
+  TARGETS laser_scan_assembler
+  DESTINATION lib/${PROJECT_NAME})
+
+#add_executable(merge_clouds src/merge_clouds.cpp)
+#target_link_libraries(merge_clouds ${LIBS})
+
+# add_executable(point_cloud_assembler_srv src/point_cloud_assembler_srv.cpp)
+# target_link_libraries(point_cloud_assembler_srv ${LIBS})
+
+#add_executable(point_cloud_assembler src/point_cloud_assembler.cpp)
+#target_link_libraries(point_cloud_assembler ${LIBS})
+#install(
+#  TARGETS point_cloud_assembler
+#  DESTINATION lib/${PROJECT_NAME})
+
+# add_executable(point_cloud2_assembler src/point_cloud2_assembler.cpp)
+# target_link_libraries(point_cloud2_assembler ${LIBS})
+# install(
+  #TARGETS point_cloud2_assembler
+  #DESTINATION lib/${PROJECT_NAME})
+
+include_directories(include)
+
+install(DIRECTORY "include/"
+  DESTINATION include)
+
+install(TARGETS laser_scan_assembler
+  #point_cloud_assembler
+  #point_cloud2_assembler
+  DESTINATION lib/${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include)
+
+ament_export_include_directories(include)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(signals)
+ament_export_dependencies(sensor_msgs)
+#ament_export_dependencies(laser_geometry)
+ament_export_dependencies(tf2_ros)
+ament_export_dependencies(tf2)
+ament_export_dependencies(message_filters)
+ament_export_include_directories(${INCLUDE_DIRS})
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
 
-  add_executable(periodic_snapshotter examples/periodic_snapshotter.cpp)
-  target_link_libraries(periodic_snapshotter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-  add_dependencies(periodic_snapshotter ${PROJECT_NAME}_gencpp)
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
 
   add_executable(dummy_scan_producer test/dummy_scan_producer.cpp)
-  target_link_libraries(dummy_scan_producer ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-  add_dependencies(dummy_scan_producer ${PROJECT_NAME}_gencpp)
-
-  add_executable(test_assembler test/test_assembler.cpp)
-  target_link_libraries(test_assembler ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest)
-  add_dependencies(test_assembler ${PROJECT_NAME}_gencpp)
+  target_link_libraries(dummy_scan_producer ${LIBS})
+  install(
+  TARGETS dummy_scan_producer
+  DESTINATION lib/${PROJECT_NAME})
+
+  ament_add_gtest(test_assembler test/test_assembler.cpp)
+  target_link_libraries(test_assembler ${LIBS})
+  install(
+  TARGETS test_assembler
+  DESTINATION lib/${PROJECT_NAME})
+
+  add_executable(periodic_snapshotter examples/periodic_snapshotter.cpp)
+  target_link_libraries(periodic_snapshotter ${LIBS})
+  install(
+  TARGETS periodic_snapshotter
+  DESTINATION lib/${PROJECT_NAME})
 
-  add_rostest(test/test_laser_assembler.launch)
 endif()
 
-##############################################################################
-# Install
-##############################################################################
-
-install(TARGETS 
-    laser_scan_assembler_srv
-    laser_scan_assembler
-    merge_clouds
-    point_cloud_assembler_srv
-    point_cloud_assembler
-    point_cloud2_assembler
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-# Install headers
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+ament_package()

examples/periodic_snapshotter.cpp

@@ -33,15 +33,25 @@
 *********************************************************************/
 
 #include <cstdio>
-#include <ros/ros.h>
-
+#include "rclcpp/rclcpp.hpp"
+#include <string>
+#include <chrono>
 // Services
-#include "laser_assembler/AssembleScans.h"
+#include "laser_assembler_srv_gen/srv/assemble_scans.hpp"
 
 // Messages
-#include "sensor_msgs/PointCloud.h"
-
-
+#include "sensor_msgs/msg/point_cloud.hpp"
+#include "rcutils/cmdline_parser.h"
+#include "rclcpp/clock.hpp"
+#include "std_msgs/msg/string.hpp"
+
+#define ROS_ERROR printf
+#define ROS_INFO printf
+#define ROS_WARN printf
+#define ROS_DEBUG printf
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
 /***
  * This a simple test app that requests a point cloud from the
  * point_cloud_assembler every 4 seconds, and then publishes the
@@ -50,59 +60,95 @@
 namespace laser_assembler
 {
 
-class PeriodicSnapshotter
+class PeriodicSnapshotter: public rclcpp::Node
 {
 
 public:
 
-  PeriodicSnapshotter()
+  explicit PeriodicSnapshotter(const std::string & service_name)
+  : Node(service_name)
   {
-    // Create a publisher for the clouds that we assemble
-    pub_ = n_.advertise<sensor_msgs::PointCloud> ("assembled_cloud", 1);
-
-    // Create the service client for calling the assembler
-    client_ = n_.serviceClient<AssembleScans>("assemble_scans");
+    std::cerr<< " Inside PeriodicSnapshotter constructor " << "\n";
 
-    // Start the timer that will trigger the processing loop (timerCallback)
-    timer_ = n_.createTimer(ros::Duration(5,0), &PeriodicSnapshotter::timerCallback, this);
+    client_ = this->create_client<laser_assembler_srv_gen::srv::AssembleScans>("build_cloud");
+
+    if (!client_->wait_for_service(20s)) {
+      printf("Service not available after waiting");
+      return;
+    }
+    printf(" Service assemble_scans started successfully");
+    //ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str());
+
+    // Create a function for when messages are to be sent.
+    auto timer_callback = [this]() -> void {
+        // msg_->name = "Hello World: " + std::to_string(count_++);
+        // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg_->status)
+        //RCLCPP_INFO(this->get_logger(), "Publishing: namespce %s and name is %s ",
+         // this->get_namespace(), this->get_name());
+
+        // Put the message into a queue to be processed by the middleware.
+        // This call is non-blocking.
+        this->timerCallback();
+      };
+
+    // Create a publisher with a custom Quality of Service profile.
+    rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
+    custom_qos_profile.depth = 7;
+    pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>(
+      "/assembled_cloud", custom_qos_profile);
+
+    // Use a timer to schedule periodic message publishing.
+    timer_ = this->create_wall_timer(std::chrono::milliseconds(25), timer_callback);
 
     // Need to track if we've called the timerCallback at least once
     first_time_ = true;
   }
 
-  void timerCallback(const ros::TimerEvent& e)
+  //void timerCallback(const builtin_interfaces::msg::Time& e)
+  void timerCallback()
   {
 
     // We don't want to build a cloud the first callback, since we we
     //   don't have a start and end time yet
     if (first_time_)
     {
+      last_time = rclcpp::Clock().now();
       first_time_ = false;
       return;
     }
 
     // Populate our service request based on our timer callback times
-    AssembleScans srv;
-    srv.request.begin = e.last_real;
-    srv.request.end   = e.current_real;
-
-    // Make the service call
-    if (client_.call(srv))
-    {
-      ROS_INFO("Published Cloud with %u points", (uint32_t)(srv.response.cloud.points.size())) ;
-      pub_.publish(srv.response.cloud);
-    }
-    else
-    {
-      ROS_ERROR("Error making service call\n") ;
-    }
-  }
+    auto request = std::make_shared<laser_assembler_srv_gen::srv::AssembleScans::Request>();
+
+    request->begin = last_time.sec;
+    last_time = rclcpp::Clock().now();
+    request->end = last_time.sec;
+
+
+    using ServiceResponseFuture =
+        rclcpp::Client<laser_assembler_srv_gen::srv::AssembleScans>::SharedFuture;
+
+      auto response_received_callback = [this](ServiceResponseFuture future) {
+        auto result = future.get();
+        printf("\n                                      Cloud points size = %ld\n", result.get()->cloud.points.size());
+        //RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", future.get()->sum)
+        if (result.get()->cloud.points.size() > 0 ) {
+
+          result.get()->cloud;
+          pub_->publish(result.get()->cloud);
+        }
+        else
+          ROS_INFO("Got an empty cloud. Going to try again on the next scan");
+      };
+
+    auto result = client_->async_send_request(request);
+}
 
 private:
-  ros::NodeHandle n_;
-  ros::Publisher pub_;
-  ros::ServiceClient client_;
-  ros::Timer timer_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr pub_;
+  rclcpp::Client<laser_assembler_srv_gen::srv::AssembleScans>::SharedPtr client_;
+  builtin_interfaces::msg::Time last_time;
+  rclcpp::TimerBase::SharedPtr timer_;
   bool first_time_;
 } ;
 
@@ -112,12 +158,13 @@ using namespace laser_assembler ;
 
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "periodic_snapshotter");
-  ros::NodeHandle n;
-  ROS_INFO("Waiting for [build_cloud] to be advertised");
-  ros::service::waitForService("build_cloud");
-  ROS_INFO("Found build_cloud! Starting the snapshotter");
-  PeriodicSnapshotter snapshotter;
-  ros::spin();
+  rclcpp::init(argc, argv);
+  auto service_name = std::string("periodic_snapshotter");
+  char * cli_option = rcutils_cli_get_option(argv, argv + argc, "-s");
+  if (nullptr != cli_option) {
+    service_name = std::string(cli_option);
+  }
+  auto node = std::make_shared<PeriodicSnapshotter>(service_name);
+  rclcpp::spin(node);
   return 0;
 }