Skip to content

Commit

Permalink
Merge pull request #31 from osrf/noetic-pub-clpe-info
Browse files Browse the repository at this point in the history 
Backport clpe camera info to noetic
  • Loading branch information
@Yadunund
Yadunund authored Apr 20, 2022
2 parents a666767 + cb5a5a6 commit 6aabfc7
Show file tree
Hide file tree
Showing 3 changed files with 76 additions and 11 deletions.
11 changes: 8 additions & 3 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,23 +13,28 @@ 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
${roscpp_INCLUDE_DIRS}
${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}
Expand Down
1 change: 1 addition & 0 deletions package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>tf2</depend>
<depend>clpe_ros_msgs</depend>

<buildtool_depend>catkin</buildtool_depend>
</package>
75 changes: 67 additions & 8 deletions src/ClpeNode.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#pragma once

#include <unordered_map>

#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/Transform.h>
#include <image_transport/image_transport.h>
Expand All @@ -9,8 +11,7 @@
#include <sensor_msgs/distortion_models.h>
#include <sensor_msgs/image_encodings.h>
#include <tf2/LinearMath/Quaternion.h>

#include <unordered_map>
#include <clpe_ros_msgs/ClpeCameraInfo.h>

#include "errors.hpp"

Expand Down Expand Up @@ -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 <typename ClpeClientApi>
Expand Down Expand Up @@ -137,11 +138,21 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this<Clp
ROS_INFO("Skipped cam_%i because it is not enabled", i);
continue;
}
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->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");
Expand All @@ -161,6 +172,8 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this<Clp
bool info_queue_size = this->param<int>(kCamInfoQueueSize[i], 10);
kInfoPubs[i] = this->advertise<sensor_msgs::CameraInfo>("cam_" + std::to_string(i) + "/camera_info",
info_queue_size, info_latch);
kClpeInfoPubs[i] = this->advertise<clpe_ros_msgs::ClpeCameraInfo>("cam_" + std::to_string(i) + "/clpe_camera_info",
info_queue_size, info_latch);
}

// start publishing
Expand All @@ -171,7 +184,9 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this<Clp
ROS_DEBUG("got new image for cam_%i", cam_id);

// skip all work if there is no subscribers
if (kImagePubs[cam_id].getNumSubscribers() == 0 && kInfoPubs[cam_id].getNumSubscribers() == 0)
if (kImagePubs[cam_id].getNumSubscribers() == 0 &&
kInfoPubs[cam_id].getNumSubscribers() == 0 &&
kClpeInfoPubs[cam_id].getNumSubscribers() == 0)
{
ROS_DEBUG("skipped publishing for cam_%i because there are no subscribers", cam_id);
return 0;
Expand All @@ -185,6 +200,7 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this<Clp
kCamInfos[cam_id].header.frame_id = frame_id;
kCamInfos[cam_id].header.stamp = stamp;
kInfoPubs[cam_id].publish(kCamInfos[cam_id]);
kClpeInfoPubs[cam_id].publish(kClpeCamInfos[cam_id]);
return 0;
},
static_cast<int>(this->cam_enabled_[0]), static_cast<int>(this->cam_enabled_[1]),
Expand All @@ -205,6 +221,8 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this<Clp
static std::unordered_map<int, image_transport::Publisher> kImagePubs;
static std::array<ros::Publisher, 4> kInfoPubs;
static std::array<sensor_msgs::CameraInfo, 4> kCamInfos;
static std::array<ros::Publisher, 4> kClpeInfoPubs;
static std::array<clpe_ros_msgs::ClpeCameraInfo, 4> kClpeCamInfos;

std::unique_ptr<image_transport::ImageTransport> transport_;
std::string encoding_;
Expand Down Expand Up @@ -273,6 +291,37 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this<Clp
}
cam_info.K = { eeprom_data.fx, 0, eeprom_data.cx, 0, eeprom_data.fy, eeprom_data.cy, 0, 0, 1 };
cam_info.D = { eeprom_data.k1, eeprom_data.k2, eeprom_data.p1, eeprom_data.p2, eeprom_data.k3, eeprom_data.k4 };
cam_info.P = {
eeprom_data.fx, 0, eeprom_data.cx, 0,
0, eeprom_data.fy, eeprom_data.cy, 0,
0, 0, 1, 0
};
return kNoError;
}

std::error_code GetClpeCameraInfo_(int cam_id, clpe_ros_msgs::ClpeCameraInfo& msg)
{
EepromData eeprom_data;
const auto result =
this->clpe_api.Clpe_GetEepromData(cam_id, reinterpret_cast<unsigned char*>(&eeprom_data));
if (result != 0) {
return std::error_code(result, GetEepromDataError::get());
}

msg.calibration_model = static_cast<uint32_t>(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;
}

Expand Down Expand Up @@ -329,11 +378,21 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this<Clp

template <typename ClpeClientApi>
ClpeNode<ClpeClientApi>* ClpeNode<ClpeClientApi>::kNode_;

template <typename ClpeClientApi>
std::unordered_map<int, image_transport::Publisher> ClpeNode<ClpeClientApi>::kImagePubs;

template <typename ClpeClientApi>
std::array<ros::Publisher, 4> ClpeNode<ClpeClientApi>::kInfoPubs;

template <typename ClpeClientApi>
std::array<sensor_msgs::CameraInfo, 4> ClpeNode<ClpeClientApi>::kCamInfos;

template <typename ClpeClientApi>
std::array<ros::Publisher, 4>
ClpeNode<ClpeClientApi>::kClpeInfoPubs;

template <typename ClpeClientApi>
std::array<clpe_ros_msgs::ClpeCameraInfo, 4> ClpeNode<ClpeClientApi>::kClpeCamInfos;

} // namespace clpe

0 comments on commit 6aabfc7

Please sign in to comment.