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