diff --git a/CMakeLists.txt b/CMakeLists.txt index cddcff1..841ce7b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,8 +13,9 @@ find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(cv_bridge REQUIRED) find_package(tf2 REQUIRED) +find_package(clpe_ros_msgs REQUIRED) -catkin_package(DEPENDS roscpp image_transport) +catkin_package(DEPENDS roscpp image_transport clpe_ros_msgs) add_library(${PROJECT_NAME}_lib INTERFACE) target_include_directories(${PROJECT_NAME}_lib INTERFACE @@ -22,14 +23,18 @@ target_include_directories(${PROJECT_NAME}_lib INTERFACE ${geometry_msgs_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS} ${image_transport_INCLUDE_DIRS} - ${cv_bridge_INCLUDE_DIRS}) + ${cv_bridge_INCLUDE_DIRS} + ${clpe_ros_msgs_INCLUDE_DIRS} +) target_link_libraries(${PROJECT_NAME}_lib INTERFACE clpe ${roscpp_LIBRARIES} ${geometry_msgs_LIBRARIES} ${sensor_msgs_LIBRARIES} ${image_transport_LIBRARIES} - ${cv_bridge_LIBRARIES}) + ${cv_bridge_LIBRARIES} + ${clpe_ros_msgs_LIBRARIES} +) target_compile_features(${PROJECT_NAME}_lib INTERFACE cxx_std_14) add_executable(${PROJECT_NAME} diff --git a/package.xml b/package.xml index 21fc02b..46384f1 100644 --- a/package.xml +++ b/package.xml @@ -15,6 +15,7 @@ sensor_msgs cv_bridge tf2 + clpe_ros_msgs catkin diff --git a/src/ClpeNode.hpp b/src/ClpeNode.hpp index c948a2f..bd29c52 100644 --- a/src/ClpeNode.hpp +++ b/src/ClpeNode.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include #include @@ -9,8 +11,7 @@ #include #include #include - -#include +#include #include "errors.hpp" @@ -75,7 +76,7 @@ struct __attribute__((packed)) EepromData float p2; uint8_t reserved2[8]; uint16_t checksum; - uint8_t production_date[11]; + char production_date[11]; }; template @@ -137,11 +138,21 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_thisGetCameraInfo_(i, kCamInfos[i]); - if (error) { - ROS_FATAL("Failed to get camera info (%s)", error.message().c_str()); - exit(error.value()); + const auto error = this->GetCameraInfo_(i, kCamInfos[i]); + if (error) + { + ROS_FATAL("Failed to get camera info (%s)", error.message().c_str()); + exit(error.value()); + } + } + { + const auto error = this->GetClpeCameraInfo_(i, kClpeCamInfos[i]); + if (error) + { + ROS_FATAL("Failed to get camera info (%s)", error.message().c_str()); + exit(error.value()); + } } } ROS_INFO("Successfully discovered camera properties"); @@ -161,6 +172,8 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_thisparam(kCamInfoQueueSize[i], 10); kInfoPubs[i] = this->advertise("cam_" + std::to_string(i) + "/camera_info", info_queue_size, info_latch); + kClpeInfoPubs[i] = this->advertise("cam_" + std::to_string(i) + "/clpe_camera_info", + info_queue_size, info_latch); } // start publishing @@ -171,7 +184,9 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this(this->cam_enabled_[0]), static_cast(this->cam_enabled_[1]), @@ -205,6 +221,8 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this kImagePubs; static std::array kInfoPubs; static std::array kCamInfos; + static std::array kClpeInfoPubs; + static std::array kClpeCamInfos; std::unique_ptr transport_; std::string encoding_; @@ -273,6 +291,37 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_thisclpe_api.Clpe_GetEepromData(cam_id, reinterpret_cast(&eeprom_data)); + if (result != 0) { + return std::error_code(result, GetEepromDataError::get()); + } + + msg.calibration_model = static_cast(eeprom_data.calibration_model); + msg.fx = eeprom_data.fx; + msg.fy = eeprom_data.fy; + msg.cx = eeprom_data.cx; + msg.cy = eeprom_data.cy; + msg.k1 = eeprom_data.k1; + msg.k2 = eeprom_data.k2; + msg.k3 = eeprom_data.k3; + msg.k4 = eeprom_data.k4; + msg.rms = eeprom_data.rms; + msg.fov = eeprom_data.fov; + msg.p1 = eeprom_data.p1; + msg.p2 = eeprom_data.p2; + msg.production_date = std::string(eeprom_data.production_date); return kNoError; } @@ -329,11 +378,21 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this ClpeNode* ClpeNode::kNode_; + template std::unordered_map ClpeNode::kImagePubs; + template std::array ClpeNode::kInfoPubs; + template std::array ClpeNode::kCamInfos; +template +std::array +ClpeNode::kClpeInfoPubs; + +template +std::array ClpeNode::kClpeCamInfos; + } // namespace clpe