Skip to content

Commit

Permalink
SE-2737 (#23)
Browse files Browse the repository at this point in the history 
* Synchronize FW staging with defaults on ROS
* Disabled feature match output until parser in SE-2730/samples is fixed as reference
  • Loading branch information
@treideme
treideme committed Mar 7, 2024
1 parent a009f71 commit 063db16
Show file tree
Hide file tree
Showing 4 changed files with 113 additions and 99 deletions.
14 changes: 7 additions & 7 deletions include/bottlenose_chunk_parser.hpp
View file
Expand Up @@ -173,13 +173,13 @@ bool chunkDecodeBoundingBoxes(PvBuffer *buffer, bboxes_t &bboxes);
*/
bool chunkDecodePointCloud(PvBuffer *buffer, pointcloud_t &pointcloud);

/**
* Decode matches from image stream, if present.
* @param buffer Buffer received on GEV interface
* @param matches Decoded matches
* @return true if present, false if not present or corrupted.
*/
bool chunkDecodeMatches(PvBuffer *buffer, matches_t &matches);
///**
// * Decode matches from image stream, if present.
// * @param buffer Buffer received on GEV interface
// * @param matches Decoded matches
// * @return true if present, false if not present or corrupted.
// */
//bool chunkDecodeMatches(PvBuffer *buffer, matches_t &matches);

/**
* Count the number of valid matches in the matches structure.
Expand Down
8 changes: 4 additions & 4 deletions include/bottlenose_parameters.hpp
View file
Expand Up @@ -104,10 +104,10 @@ const parameter_t bottlenose_parameters[] = {
{"AKAZEWindow", rclcpp::ParameterValue(20)},
{"HAMATXOffset", rclcpp::ParameterValue(0)},
{"HAMATYOffset", rclcpp::ParameterValue(0)},
{"HAMATRect1X", rclcpp::ParameterValue(64)},
{"HAMATRect1Y", rclcpp::ParameterValue(64)},
{"HAMATRect2X", rclcpp::ParameterValue(128)},
{"HAMATRect2Y", rclcpp::ParameterValue(128)},
{"HAMATRect1X", rclcpp::ParameterValue(500)},
{"HAMATRect1Y", rclcpp::ParameterValue(4)},
{"HAMATRect2X", rclcpp::ParameterValue(500)},
{"HAMATRect2Y", rclcpp::ParameterValue(4)},
{"HAMATMinThreshold", rclcpp::ParameterValue(500)},
{"HAMATRatioThreshold",rclcpp::ParameterValue(1023)},

Expand Down
144 changes: 79 additions & 65 deletions src/bottlenose_camera_driver.cpp
View file
Expand Up @@ -145,7 +145,7 @@ CameraDriver::CameraDriver(const rclcpp::NodeOptions &node_options)

m_pointcloud = create_publisher<sensor_msgs::msg::PointCloud2>("pointcloud", 10);

m_matches = create_publisher<sensor_msgs::msg::PointCloud2>("matches", 10);
// m_matches = create_publisher<sensor_msgs::msg::PointCloud2>("matches", 10);

if(!is_ebus_loaded()) {
RCLCPP_ERROR(get_logger(), "The eBus Driver is not loaded, please reinstall the driver!");
Expand Down Expand Up @@ -562,24 +562,29 @@ bool CameraDriver::set_ccm_custom() {
}

bool CameraDriver::set_stereo() {
// Ignore for bottlenose mono
if(!is_stereo()) {
return true;
}
bool enable_stereo = get_parameter("stereo").as_bool();
PvGenBoolean *multipart = dynamic_cast<PvGenBoolean *>( m_device->GetParameters()->Get("GevSCCFGMultiPartEnabled"));
if(multipart == nullptr) {
RCLCPP_ERROR(get_logger(), "Could not configure stereo");
return false;
// Ignore for bottlenose mono
if(!is_stereo()) {
if(get_parameter("stereo").as_bool()) {
RCLCPP_ERROR(get_logger(), "Stereo is not supported on this device");
return false;
} else {
return true;
}
}
bool enable_stereo = get_parameter("stereo").as_bool();
PvGenBoolean *multipart = dynamic_cast<PvGenBoolean *>( m_device->GetParameters()->Get("GevSCCFGMultiPartEnabled"));
if(multipart == nullptr) {
RCLCPP_ERROR(get_logger(), "Could not configure stereo");
return false;
}

PvResult res = multipart->SetValue(enable_stereo);
if(!res.IsOK())
RCLCPP_ERROR_STREAM(get_logger(), "Could not configure stereo, cause: " << res.GetDescription().GetAscii());
else
RCLCPP_DEBUG_STREAM(get_logger(), "Configured stereo to " << enable_stereo);
PvResult res = multipart->SetValue(enable_stereo);
if(!res.IsOK())
RCLCPP_ERROR_STREAM(get_logger(), "Could not configure stereo, cause: " << res.GetDescription().GetAscii());
else
RCLCPP_DEBUG_STREAM(get_logger(), "Configured stereo to " << enable_stereo);

return res.IsOK();
return res.IsOK();
}

bool CameraDriver::set_auto_exposure() {
Expand Down Expand Up @@ -900,7 +905,7 @@ void CameraDriver::management_thread() {
bboxes_t bboxes = {};
uint64_t timestamp = 0;
pointcloud_t point_cloud = {};
matches_t matches = {};
// matches_t matches = {};
switch ( buffer->GetPayloadType() ) {
case PvPayloadTypeImage:
img0 = buffer->GetImage();
Expand All @@ -921,9 +926,9 @@ void CameraDriver::management_thread() {
if(chunkDecodePointCloud(buffer, point_cloud)) {
this->publish_pointcloud(point_cloud, timestamp);
}
if(chunkDecodeMatches(buffer, matches)) {
this->publish_matches(matches, timestamp);
}
// if(chunkDecodeMatches(buffer, matches)) {
// this->publish_matches(matches, timestamp);
// }
m_image_msg = convertFrameToMessage(img0, timestamp);

if(m_image_msg != nullptr) {
Expand Down Expand Up @@ -963,9 +968,9 @@ void CameraDriver::management_thread() {
if(chunkDecodePointCloud(buffer, point_cloud)) {
this->publish_pointcloud(point_cloud, timestamp);
}
if(chunkDecodeMatches(buffer, matches)) {
this->publish_matches(matches, timestamp);
}
// if(chunkDecodeMatches(buffer, matches)) {
// this->publish_matches(matches, timestamp);
// }
m_image_msg = convertFrameToMessage(img0, timestamp);
m_image_msg_1 = convertFrameToMessage(img1, timestamp);

Expand Down Expand Up @@ -1107,43 +1112,43 @@ void CameraDriver::publish_pointcloud(const pointcloud_t &pointcloud, const uint
RCLCPP_DEBUG(get_logger(), "Decoded point cloud with %u points", pointcloud.count);
}

void CameraDriver::publish_matches(const matches_t &matches, const uint64_t &timestamp) {
auto ts = convertTimestamp(timestamp);

size_t offset = 0;
sensor_msgs::msg::PointCloud2 msg;
for(auto name : {"x", "y", "x1", "y1", "d2", "d1", "n2", "n1"}) {
sensor_msgs::msg::PointField field;
field.name = name;
field.offset = offset; // Assuming 'x' is the first field, so offset is 0
field.datatype = sensor_msgs::msg::PointField::UINT16;
field.count = 1; // Typically, each coordinate is just one element
offset+=sizeof(uint16_t);
// Define fields
msg.fields.push_back(field);
}
assert(offset == sizeof(hamat_matches_8xu16));
msg.height = 1; // If your cloud is unordered, height is 1
msg.width = validMatches(matches);
// Point step is the size of a point in bytes
msg.point_step = offset;
msg.row_step = msg.point_step * msg.width; // Row step is point step times the width
msg.is_dense = false; // If there are no invalid (NaN, Inf) points, set to true
msg.data.resize(msg.row_step * msg.height);

size_t valid_out = 0;
for(size_t i = 0; i < matches.count; i++) {
if(matches.points[i].x != matches.unmatched) {
std::memcpy(&msg.data[valid_out * msg.point_step], &matches.points[i], offset);
valid_out++;
}
}
msg.header.frame_id = this->get_parameter("frame_id").as_string();
msg.header.stamp.sec = ts.first;
msg.header.stamp.nanosec = ts.second;
m_matches->publish(msg);
RCLCPP_DEBUG(get_logger(), "Decoded %lu valid matches", valid_out);
}
//void CameraDriver::publish_matches(const matches_t &matches, const uint64_t &timestamp) {
// auto ts = convertTimestamp(timestamp);
//
// size_t offset = 0;
// sensor_msgs::msg::PointCloud2 msg;
// for(auto name : {"x", "y", "x1", "y1", "d2", "d1", "n2", "n1"}) {
// sensor_msgs::msg::PointField field;
// field.name = name;
// field.offset = offset; // Assuming 'x' is the first field, so offset is 0
// field.datatype = sensor_msgs::msg::PointField::UINT16;
// field.count = 1; // Typically, each coordinate is just one element
// offset+=sizeof(uint16_t);
// // Define fields
// msg.fields.push_back(field);
// }
// assert(offset == sizeof(hamat_matches_8xu16));
// msg.height = 1; // If your cloud is unordered, height is 1
// msg.width = validMatches(matches);
// // Point step is the size of a point in bytes
// msg.point_step = offset;
// msg.row_step = msg.point_step * msg.width; // Row step is point step times the width
// msg.is_dense = false; // If there are no invalid (NaN, Inf) points, set to true
// msg.data.resize(msg.row_step * msg.height);
//
// size_t valid_out = 0;
// for(size_t i = 0; i < matches.count; i++) {
// if(matches.points[i].x != matches.unmatched) {
// std::memcpy(&msg.data[valid_out * msg.point_step], &matches.points[i], offset);
// valid_out++;
// }
// }
// msg.header.frame_id = this->get_parameter("frame_id").as_string();
// msg.header.stamp.sec = ts.first;
// msg.header.stamp.nanosec = ts.second;
// m_matches->publish(msg);
// RCLCPP_DEBUG(get_logger(), "Decoded %lu valid matches", valid_out);
//}

bool CameraDriver::is_streaming() {
return !done && m_device != nullptr && m_stream != nullptr && m_stream->IsOpen();
Expand Down Expand Up @@ -1270,8 +1275,16 @@ bool CameraDriver::configure_feature_points() {

bool CameraDriver::configure_point_cloud() {
if(!is_stereo()) {
if(get_parameter("sparse_point_cloud").as_bool()) {
RCLCPP_ERROR(get_logger(), "Sparse point cloud is only available on Bottlenose Stereo models");
return false;
}
return true;
}
if(!get_parameter("stereo").as_bool()) {
RCLCPP_ERROR(get_logger(), "Sparse point cloud is only available when stereo mode is enabled. Please enable the \"stereo\" parameter");
return false;
}

bool enabled = get_parameter("sparse_point_cloud").as_bool();
if(!set_chunk("SparsePointCloud", enabled)) {
Expand Down Expand Up @@ -1310,11 +1323,12 @@ bool CameraDriver::configure_point_cloud() {
RCLCPP_ERROR(get_logger(), "Could not configure HAMATOutputFormat");
return false;
}
// Enable matches chunk for debugging
if(!set_chunk("FeatureMatches", enabled)) {
RCLCPP_ERROR(get_logger(), "Could not enable feature FeatureMatches chunk");
return false;
}
// FIXME: Disabled until after accuracy via SE-2730 is confirmed
// // Enable matches chunk for debugging
// if(!set_chunk("FeatureMatches", enabled)) {
// RCLCPP_ERROR(get_logger(), "Could not enable feature FeatureMatches chunk");
// return false;
// }
}
RCLCPP_DEBUG_STREAM(get_logger(), "Sparse point cloud configured to " << enabled);

Expand Down
46 changes: 23 additions & 23 deletions src/bottlenose_chunk_parser.cpp
View file
Expand Up @@ -244,29 +244,29 @@ size_t validMatches(const matches_t &matches) {
return count;
}

bool chunkDecodeMatches(PvBuffer *buffer, matches_t &matches) {
uint8_t *data = getChunkRawData(buffer, CHUNK_ID_MATCHES);
if(data == nullptr) return false;

bzero(&matches, sizeof(matches_t));

matches.count = uintFromBytes(data, 4, true);
matches.layout = uintFromBytes(&data[4], 4, true);
matches.unmatched = uintFromBytes(&data[8], 4, true);

uint32_t offset = 3 * sizeof(uint32_t);
if(matches.layout < 2){
point_u16_t *points = (point_u16_t*)&data[offset];
for(uint32_t i = 0; i < matches.count; ++i){
matches.points[i].x = points[i].x;
matches.points[i].y = points[i].y;
}
} else {
memcpy(matches.points, &data[offset], sizeof(hamat_matches_8xu16_t) * matches.count);
}

return true;
}
//bool chunkDecodeMatches(PvBuffer *buffer, matches_t &matches) {
// uint8_t *data = getChunkRawData(buffer, CHUNK_ID_MATCHES);
// if(data == nullptr) return false;
//
// bzero(&matches, sizeof(matches_t));
//
// matches.count = uintFromBytes(data, 4, true);
// matches.layout = uintFromBytes(&data[4], 4, true);
// matches.unmatched = uintFromBytes(&data[8], 4, true);
//
// uint32_t offset = 3 * sizeof(uint32_t);
// if(matches.layout < 2){
// point_u16_t *points = (point_u16_t*)&data[offset];
// for(uint32_t i = 0; i < matches.count; ++i){
// matches.points[i].x = points[i].x;
// matches.points[i].y = points[i].y;
// }
// } else {
// memcpy(matches.points, &data[offset], sizeof(hamat_matches_8xu16_t) * matches.count);
// }
//
// return true;
//}

std::string ms_to_date_string(uint64_t ms) {
// Convert milliseconds to seconds
Expand Down

0 comments on commit 063db16

Please sign in to comment.