ament test: pass ament test

gtest passed copyright passed cpplint passed cppcheck passed lint_cmake passed uncrustify passed Signed-off-by: Sharron LIU <[email protected]>
intel · May 22, 2018 · b315c2d · b315c2d
1 parent 1158943
commit b315c2d
Show file tree

Hide file tree

Showing 6 changed files with 97 additions and 76 deletions.
diff --git a/LICENSE b/LICENSE
@@ -187,7 +187,7 @@
       same "printed page" as the copyright notice for easier
       identification within third-party archives.
 
-   Copyright 2017 Intel Corporation
+   Copyright 2018 Intel Corporation
 
    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.

diff --git a/README.md b/README.md
@@ -40,7 +40,7 @@ To start the camera node in ROS2, plug in the camera, then type the following co
 
 ```bash
 # To launch with "ros2 run"
-$ ros2 run realsense_ros2_camera realsenes_ros2_camera
+$ ros2 run realsense_ros2_camera realsense_ros2_camera
 # OR, to invoke the executable directly
 $ realsense_ros2_camera
 ```

diff --git a/realsense_ros2_camera/CMakeLists.txt b/realsense_ros2_camera/CMakeLists.txt
@@ -13,7 +13,7 @@ endif()
 # Compiler Defense Flags
 if(UNIX OR APPLE)
   # Linker flags.
-  if( ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" OR ${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel")
+  if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" OR ${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel")
     # GCC specific flags. ICC is compatible with them.
     set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -z noexecstack -z relro -z now")
     set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -z noexecstack -z relro -z now")
@@ -24,7 +24,7 @@ if(UNIX OR APPLE)
   endif()
 
   # Compiler flags.
-  if( ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU")
+  if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU")
     # GCC specific flags.
     if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.9)
       set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector-strong")
@@ -34,7 +34,7 @@ if(UNIX OR APPLE)
   elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
     # Clang is compatbile with some of the flags.
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector")
-  elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel" )
+  elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel")
     # Same as above, with exception that ICC compilation crashes with -fPIE option, even
     # though it uses -pie linker option that require -fPIE during compilation. Checksec
     # shows that it generates correct PIE anyway if only -pie is provided.
@@ -54,7 +54,7 @@ if(NOT realsense2_FOUND)
   message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n")
 endif()
 
-if (CMAKE_BUILD_TYPE EQUAL "RELEASE")
+if(CMAKE_BUILD_TYPE EQUAL "RELEASE")
   message(STATUS "Create Release Build.")
   set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}")
 else()

diff --git a/realsense_ros2_camera/include/realsense_ros2_camera/constants.h b/realsense_ros2_camera/include/realsense_ros2_camera/constants.h
@@ -1,4 +1,4 @@
-// Copyright (c) 2017 Intel Corporation. All Rights Reserved
+// Copyright (c) 2018 Intel Corporation. All Rights Reserved
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,8 +15,8 @@
 #include <string>
 
 #pragma once
-#ifndef REALSENSE_CAMERA_CONSTANTS_H
-#define REALSENSE_CAMERA_CONSTANTS_H
+#ifndef REALSENSE_ROS2_CAMERA__CONSTANTS_H_
+#define REALSENSE_ROS2_CAMERA__CONSTANTS_H_
 
 #define REALSENSE_ROS_MAJOR_VERSION    2
 #define REALSENSE_ROS_MINOR_VERSION    0
@@ -25,7 +25,7 @@
 #define STRINGIFY(arg) #arg
 #define VAR_ARG_STRING(arg) STRINGIFY(arg)
 /* Return version in "X.Y.Z" format */
-#define REALSENSE_ROS_VERSION_STR (VAR_ARG_STRING(REALSENSE_ROS_MAJOR_VERSION.\
+#define REALSENSE_ROS_VERSION_STR (VAR_ARG_STRING(REALSENSE_ROS_MAJOR_VERSION. \
   REALSENSE_ROS_MINOR_VERSION.REALSENSE_ROS_PATCH_VERSION))
 
 namespace realsense_ros2_camera
@@ -66,20 +66,20 @@ const bool ENABLE_FISHEYE = true;
 const bool ENABLE_IMU = true;
 
 
-const std::string DEFAULT_BASE_FRAME_ID = "camera_link";
-const std::string DEFAULT_DEPTH_FRAME_ID = "camera_depth_frame";
-const std::string DEFAULT_INFRA1_FRAME_ID = "camera_infra1_frame";
-const std::string DEFAULT_INFRA2_FRAME_ID = "camera_infra2_frame";
-const std::string DEFAULT_COLOR_FRAME_ID = "camera_color_frame";
-const std::string DEFAULT_FISHEYE_FRAME_ID = "camera_fisheye_frame";
-const std::string DEFAULT_IMU_FRAME_ID = "camera_imu_frame";
-const std::string DEFAULT_DEPTH_OPTICAL_FRAME_ID = "camera_depth_optical_frame";
-const std::string DEFAULT_INFRA1_OPTICAL_FRAME_ID = "camera_infra1_optical_frame";
-const std::string DEFAULT_INFRA2_OPTICAL_FRAME_ID = "camera_infra2_optical_frame";
-const std::string DEFAULT_COLOR_OPTICAL_FRAME_ID = "camera_color_optical_frame";
-const std::string DEFAULT_FISHEYE_OPTICAL_FRAME_ID = "camera_fisheye_optical_frame";
-const std::string DEFAULT_ACCEL_OPTICAL_FRAME_ID = "camera_accel_optical_frame";
-const std::string DEFAULT_GYRO_OPTICAL_FRAME_ID = "camera_gyro_optical_frame";
-const std::string DEFAULT_IMU_OPTICAL_FRAME_ID = "camera_imu_optical_frame";
+const char DEFAULT_BASE_FRAME_ID[] = "camera_link";
+const char DEFAULT_DEPTH_FRAME_ID[] = "camera_depth_frame";
+const char DEFAULT_INFRA1_FRAME_ID[] = "camera_infra1_frame";
+const char DEFAULT_INFRA2_FRAME_ID[] = "camera_infra2_frame";
+const char DEFAULT_COLOR_FRAME_ID[] = "camera_color_frame";
+const char DEFAULT_FISHEYE_FRAME_ID[] = "camera_fisheye_frame";
+const char DEFAULT_IMU_FRAME_ID[] = "camera_imu_frame";
+const char DEFAULT_DEPTH_OPTICAL_FRAME_ID[] = "camera_depth_optical_frame";
+const char DEFAULT_INFRA1_OPTICAL_FRAME_ID[] = "camera_infra1_optical_frame";
+const char DEFAULT_INFRA2_OPTICAL_FRAME_ID[] = "camera_infra2_optical_frame";
+const char DEFAULT_COLOR_OPTICAL_FRAME_ID[] = "camera_color_optical_frame";
+const char DEFAULT_FISHEYE_OPTICAL_FRAME_ID[] = "camera_fisheye_optical_frame";
+const char DEFAULT_ACCEL_OPTICAL_FRAME_ID[] = "camera_accel_optical_frame";
+const char DEFAULT_GYRO_OPTICAL_FRAME_ID[] = "camera_gyro_optical_frame";
+const char DEFAULT_IMU_OPTICAL_FRAME_ID[] = "camera_imu_optical_frame";
 }  // namespace realsense_ros2_camera
-#endif  // REALSENSE_CAMERA_CONSTANTS_H
+#endif  // REALSENSE_ROS2_CAMERA__CONSTANTS_H_
diff --git a/realsense_ros2_camera/src/realsense_camera_node.cpp b/realsense_ros2_camera/src/realsense_camera_node.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017 Intel Corporation. All Rights Reserved
+// Copyright (c) 2018 Intel Corporation. All Rights Reserved
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <csignal>
+// cpplint: c system headers
 #include <eigen3/Eigen/Geometry>
-#include <iostream>
-#include <map>
 #include <builtin_interfaces/msg/time.hpp>
 #include <console_bridge/console.h>
 #include <cv_bridge/cv_bridge.h>
@@ -34,6 +32,16 @@
 #include <librealsense2/rs.hpp>
 #include <librealsense2/rsutil.h>
 #include <librealsense2/hpp/rs_processing.hpp>
+// cpplint: c++ system headers
+#include <algorithm>
+#include <csignal>
+#include <iostream>
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+// cpplint: other headers
 #include "realsense_ros2_camera/constants.h"
 #include "realsense_camera_msgs/msg/imu_info.hpp"
 #include "realsense_camera_msgs/msg/extrinsics.hpp"
@@ -198,29 +206,37 @@ class RealSenseCameraNode : public rclcpp::Node
     this->get_parameter_or("enable_imu", _enable[GYRO], ENABLE_IMU);
     this->get_parameter_or("enable_imu", _enable[ACCEL], ENABLE_IMU);
 
-    this->get_parameter_or("base_frame_id", _base_frame_id, DEFAULT_BASE_FRAME_ID);
-    this->get_parameter_or("depth_frame_id", _frame_id[DEPTH], DEFAULT_DEPTH_FRAME_ID);
-    this->get_parameter_or("infra1_frame_id", _frame_id[INFRA1], DEFAULT_INFRA1_FRAME_ID);
-    this->get_parameter_or("infra2_frame_id", _frame_id[INFRA2], DEFAULT_INFRA2_FRAME_ID);
-    this->get_parameter_or("color_frame_id", _frame_id[COLOR], DEFAULT_COLOR_FRAME_ID);
-    this->get_parameter_or("fisheye_frame_id", _frame_id[FISHEYE], DEFAULT_FISHEYE_FRAME_ID);
-    this->get_parameter_or("imu_gyro_frame_id", _frame_id[GYRO], DEFAULT_IMU_FRAME_ID);
-    this->get_parameter_or("imu_accel_frame_id", _frame_id[ACCEL], DEFAULT_IMU_FRAME_ID);
+    this->get_parameter_or("base_frame_id", _base_frame_id,
+      std::string(DEFAULT_BASE_FRAME_ID));
+    this->get_parameter_or("depth_frame_id", _frame_id[DEPTH],
+      std::string(DEFAULT_DEPTH_FRAME_ID));
+    this->get_parameter_or("infra1_frame_id", _frame_id[INFRA1],
+      std::string(DEFAULT_INFRA1_FRAME_ID));
+    this->get_parameter_or("infra2_frame_id", _frame_id[INFRA2],
+      std::string(DEFAULT_INFRA2_FRAME_ID));
+    this->get_parameter_or("color_frame_id", _frame_id[COLOR],
+      std::string(DEFAULT_COLOR_FRAME_ID));
+    this->get_parameter_or("fisheye_frame_id", _frame_id[FISHEYE],
+      std::string(DEFAULT_FISHEYE_FRAME_ID));
+    this->get_parameter_or("imu_gyro_frame_id", _frame_id[GYRO],
+      std::string(DEFAULT_IMU_FRAME_ID));
+    this->get_parameter_or("imu_accel_frame_id", _frame_id[ACCEL],
+      std::string(DEFAULT_IMU_FRAME_ID));
 
     this->get_parameter_or("depth_optical_frame_id", _optical_frame_id[DEPTH],
-      DEFAULT_DEPTH_OPTICAL_FRAME_ID);
+      std::string(DEFAULT_DEPTH_OPTICAL_FRAME_ID));
     this->get_parameter_or("infra1_optical_frame_id", _optical_frame_id[INFRA1],
-      DEFAULT_INFRA1_OPTICAL_FRAME_ID);
+      std::string(DEFAULT_INFRA1_OPTICAL_FRAME_ID));
     this->get_parameter_or("infra2_optical_frame_id", _optical_frame_id[INFRA2],
-      DEFAULT_INFRA2_OPTICAL_FRAME_ID);
+      std::string(DEFAULT_INFRA2_OPTICAL_FRAME_ID));
     this->get_parameter_or("color_optical_frame_id", _optical_frame_id[COLOR],
-      DEFAULT_COLOR_OPTICAL_FRAME_ID);
+      std::string(DEFAULT_COLOR_OPTICAL_FRAME_ID));
     this->get_parameter_or("fisheye_optical_frame_id", _optical_frame_id[FISHEYE],
-      DEFAULT_FISHEYE_OPTICAL_FRAME_ID);
+      std::string(DEFAULT_FISHEYE_OPTICAL_FRAME_ID));
     this->get_parameter_or("gyro_optical_frame_id", _optical_frame_id[GYRO],
-      DEFAULT_GYRO_OPTICAL_FRAME_ID);
+      std::string(DEFAULT_GYRO_OPTICAL_FRAME_ID));
     this->get_parameter_or("accel_optical_frame_id", _optical_frame_id[ACCEL],
-      DEFAULT_ACCEL_OPTICAL_FRAME_ID);
+      std::string(DEFAULT_ACCEL_OPTICAL_FRAME_ID));
   }
 
   void setupDevice()
@@ -238,7 +254,7 @@ class RealSenseCameraNode : public rclcpp::Node
       }
 
       // Take the first device in the list.
-      // TODO: Add an ability to get the specific device to work with from outside
+      // Add an ability to get the specific device to work with from outside.
       _dev = list.front();
       _ctx->set_devices_changed_callback([this](rs2::event_information & info)
         {
@@ -296,7 +312,8 @@ class RealSenseCameraNode : public rclcpp::Node
       streams.insert(streams.end(), HID_STREAMS.begin(), HID_STREAMS.end());
       for (auto & elem : streams) {
         for (auto & stream_index : elem) {
-          if (true == _enable[stream_index] && _sensors.find(stream_index) == _sensors.end()) {             // check if device supports the enabled stream
+          if (true == _enable[stream_index] && _sensors.find(stream_index) == _sensors.end()) {
+            // check if device supports the enabled stream
             RCUTILS_LOG_INFO("%s sensor isn't supported by current device! -- Skipping...",
               rs2_stream_to_string(stream_index.first));
             _enable[stream_index] = false;
@@ -410,10 +427,10 @@ class RealSenseCameraNode : public rclcpp::Node
               }
             }
             if (_enabled_profiles.find(elem) == _enabled_profiles.end()) {
-              RCUTILS_LOG_WARN("Given stream configuration is not supported by the device! \
-                Stream: %s, Format: %d, Width: %d, Height: %d, FPS: %d", rs2_stream_to_string(
-                  elem.first),
-                _format[elem], _width[elem], _height[elem], _fps[elem]);
+              RCUTILS_LOG_WARN("Given stream configuration is not supported by the device!")
+              RCUTILS_LOG_WARN("Stream: %s, Format: %d, Width: %d, Height: %d, FPS: %d",
+                rs2_stream_to_string(elem.first), _format[elem], _width[elem], _height[elem],
+                _fps[elem]);
               _enable[elem] = false;
             }
           }
@@ -430,8 +447,8 @@ class RealSenseCameraNode : public rclcpp::Node
 
       auto frame_callback = [this](rs2::frame frame)
         {
-          // We compute a ROS timestamp which is based on an initial ROS time at point of first frame,
-          // and the incremental timestamp from the camera.
+          // We compute a ROS timestamp which is based on an initial ROS time at point of first
+          // frame, and the incremental timestamp from the camera.
           // In sync mode the timestamp is based on ROS time
           if (false == _intialize_time_base) {
             _intialize_time_base = true;
@@ -476,8 +493,7 @@ class RealSenseCameraNode : public rclcpp::Node
             publishFrame(frame, t);
           }
 
-          if (_pointcloud && is_depth_frame_arrived && is_color_frame_arrived/* &&
-                       (0 != _pointcloud_publisher.getNumSubscribers())*/) {
+          if (_pointcloud && is_depth_frame_arrived && is_color_frame_arrived) {
             RCUTILS_LOG_DEBUG("publishPCTopic(...)");
             publishPCTopic(t);
           }
@@ -510,7 +526,7 @@ class RealSenseCameraNode : public rclcpp::Node
             sens->start(frame_callback);
           }
         }
-      }          //end for
+      }          // end for
 
       if (_sync_frames) {
         _syncer.start(frame_callback);
@@ -559,8 +575,9 @@ class RealSenseCameraNode : public rclcpp::Node
             rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain()));
 
             auto stream_index = (stream == GYRO.first) ? GYRO : ACCEL;
-            if (1/*0 != _info_publisher[stream_index].getNumSubscribers() ||
-                            0 != _imu_publishers[stream_index].getNumSubscribers()*/) {
+            // if (0 != _info_publisher[stream_index].getNumSubscribers() ||
+            //    0 != _imu_publishers[stream_index].getNumSubscribers())
+            {
               uint64_t elapsed_camera_ns = (frame.get_timestamp() - _camera_time_base) * 1000000;
               rclcpp::Time t(_ros_time_base.nanoseconds() + elapsed_camera_ns, RCL_ROS_TIME);
 
@@ -664,13 +681,13 @@ class RealSenseCameraNode : public rclcpp::Node
       exit(1);
     }
 
-    // TODO: Why Depth to Color?
+    // Why Depth to Color?
     if (stream_index == DEPTH && _enable[DEPTH] && _enable[COLOR]) {
       _depth2color_extrinsics = depth_profile.get_extrinsics_to(_enabled_profiles[COLOR].front());
       // set depth to color translation values in Projection matrix (P)
-      _camera_info[stream_index].p.at(3) = _depth2color_extrinsics.translation[0];               // Tx
-      _camera_info[stream_index].p.at(7) = _depth2color_extrinsics.translation[1];               // Ty
-      _camera_info[stream_index].p.at(11) = _depth2color_extrinsics.translation[2];              // Tz
+      _camera_info[stream_index].p.at(3) = _depth2color_extrinsics.translation[0];             // Tx
+      _camera_info[stream_index].p.at(7) = _depth2color_extrinsics.translation[1];             // Ty
+      _camera_info[stream_index].p.at(11) = _depth2color_extrinsics.translation[2];            // Tz
     }
 
     _camera_info[stream_index].distortion_model = "plumb_bob";
@@ -855,7 +872,7 @@ class RealSenseCameraNode : public rclcpp::Node
       ir2_2_ir2o_msg.transform.rotation.w = ir2_2_ir2o.getW();
       _static_tf_broadcaster.sendTransform(ir2_2_ir2o_msg);
     }
-    // TODO: Publish Fisheye TF
+    // Publish Fisheye TF
   }
 
   void publishPCTopic(const rclcpp::Time & t)
@@ -913,8 +930,9 @@ class RealSenseCameraNode : public rclcpp::Node
         if (color_pixel[1] < 0.f || color_pixel[1] > color_intrinsics.height ||
           color_pixel[0] < 0.f || color_pixel[0] > color_intrinsics.width)
         {
-          // For out of bounds color data, default to a shade of blue in order to visually distinguish holes.
-          // This color value is same as the librealsense out of bounds color value.
+          // For out of bounds color data, default to a shade of blue in order to visually
+          // distinguish holes. This color value is same as the librealsense out of bounds color
+          // value.
           *iter_r = static_cast<uint8_t>(96);
           *iter_g = static_cast<uint8_t>(157);
           *iter_b = static_cast<uint8_t>(198);
@@ -1024,12 +1042,13 @@ class RealSenseCameraNode : public rclcpp::Node
     RCUTILS_LOG_DEBUG("publishFrame(...)");
     stream_index_pair stream{f.get_profile().stream_type(), f.get_profile().stream_index()};
     auto & image = _image[stream];
-    image.data = (uint8_t *)f.get_data();
+    image.data = const_cast<uchar *>(reinterpret_cast<const uchar *>(f.get_data()));
     ++(_seq[stream]);
     auto & info_publisher = _info_publisher[stream];
     auto & image_publisher = _image_publishers[stream];
-    if (1/*0 != info_publisher.getNumSubscribers() ||
-               0 != image_publisher.getNumSubscribers()*/) {
+    // if (0 != info_publisher.getNumSubscribers() ||
+    //     0 != image_publisher.getNumSubscribers())
+    {
       auto width = 0;
       auto height = 0;
       auto bpp = 1;
@@ -1120,9 +1139,9 @@ class RealSenseCameraNode : public rclcpp::Node
   bool _pointcloud;
   rs2::asynchronous_syncer _syncer;
   rs2_extrinsics _depth2color_extrinsics;
-};    //end class
+};  // end class
 
-}//end namespace
+}  // namespace realsense_ros2_camera
 
 int main(int argc, char * argv[])
 {

diff --git a/realsense_ros2_camera/test/test_api.cpp b/realsense_ros2_camera/test/test_api.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017 Intel Corporation. All Rights Reserved
+// Copyright (c) 2018 Intel Corporation. All Rights Reserved
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,19 +12,21 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <chrono>
+// cpplint: c system headers
 #include <gtest/gtest.h>
 #include <librealsense2/rs.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
-
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
-
 #include <realsense_ros2_camera/constants.h>
+// cpplint: c++ system headers
+#include <chrono>
+#include <map>
+#include <string>
 
 bool g_enable_color = true;
 bool g_color_recv = false;
@@ -48,7 +50,7 @@ float g_pc_depth_avg = 0.0f;
 bool g_tf_recv = false;
 bool g_tf_color = false;
 
-unsigned int g_fps = 0;
+int g_fps = 0;
 float g_latency = 0.0f;
 
 int encoding2Mat(const std::string & encoding)
@@ -236,7 +238,7 @@ TEST(TestAPI, testLatency) {
 }
 
 TEST(TestAPI, testFPS) {
-  EXPECT_TRUE(g_fps > 0);
+  EXPECT_GT(g_fps, 0);
 }
 
 int main(int argc, char * argv[]) try