Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SE-2737 #23

Merged
merged 2 commits into from
Mar 7, 2024
Merged

SE-2737 #23

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c0fbaba
Synchronize FW staging with defaults on ROS
treideme Mar 7, 2024
7268b95
Disabled feature match output until parser in SE-2730/samples is fixe…
treideme Mar 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions include/bottlenose_chunk_parser.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Loading