From b315c2dd28fb19b7587a125d60b2dc9ab3f31441 Mon Sep 17 00:00:00 2001 From: Sharron LIU Date: Tue, 22 May 2018 16:33:32 +0800 Subject: [PATCH] ament test: pass ament test gtest passed copyright passed cpplint passed cppcheck passed lint_cmake passed uncrustify passed Signed-off-by: Sharron LIU --- LICENSE | 2 +- README.md | 2 +- realsense_ros2_camera/CMakeLists.txt | 8 +- .../include/realsense_ros2_camera/constants.h | 40 +++---- .../src/realsense_camera_node.cpp | 107 +++++++++++------- realsense_ros2_camera/test/test_api.cpp | 14 ++- 6 files changed, 97 insertions(+), 76 deletions(-) diff --git a/LICENSE b/LICENSE index f8098cd..01c7ad7 100644 --- 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 index db7c37c..8a545f8 100644 --- 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 index 842dbe8..c699cb7 100644 --- 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 index 17afafd..aa4e92c 100644 --- 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 #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 index 09fc9d4..a6bce59 100644 --- 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 +// cpplint: c system headers #include -#include -#include #include #include #include @@ -34,6 +32,16 @@ #include #include #include +// cpplint: c++ system headers +#include +#include +#include +#include +#include +#include +#include +#include +// 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(96); *iter_g = static_cast(157); *iter_b = static_cast(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(reinterpret_cast(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 index 17e5a17..833a68b 100644 --- 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 +// cpplint: c system headers #include #include #include #include - #include #include #include #include #include - #include +// cpplint: c++ system headers +#include +#include +#include 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