Skip to content

Commit

Permalink
CLANG FORMAT IS CRACCCCCCKKKKKKED bro :))))))))))))
Browse files Browse the repository at this point in the history
  • Loading branch information
jbrhm committed Sep 15, 2024
1 parent 4e78274 commit ba98358
Show file tree
Hide file tree
Showing 3 changed files with 28 additions and 30 deletions.
39 changes: 19 additions & 20 deletions perception/object_detector/object_detector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,27 +2,26 @@

namespace mrover {

ObjectDetectorBase::ObjectDetectorBase() : rclcpp::Node(NODE_NAME), mLoopProfiler{get_logger()}{
ObjectDetectorBase::ObjectDetectorBase() : rclcpp::Node(NODE_NAME), mLoopProfiler{get_logger()} {

auto paramSub = std::make_shared<rclcpp::ParameterEventHandler>(this);
auto paramSub = std::make_shared<rclcpp::ParameterEventHandler>(this);

std::vector<ParameterWrapper> params{
{"camera_frame", mCameraFrame},
{"world_frame", mWorldFrame},
{"increment_weight", mObjIncrementWeight},
{"decrement_weight", mObjDecrementWeight},
{"hitcount_threshold", mObjHitThreshold},
{"hitcount_max", mObjMaxHitcount},
{"model_name", mModelName},
{"model_score_threshold", mModelScoreThreshold},
{"model_nms_threshold", mModelNmsThreshold}
};
std::vector<ParameterWrapper> params{
{"camera_frame", mCameraFrame},
{"world_frame", mWorldFrame},
{"increment_weight", mObjIncrementWeight},
{"decrement_weight", mObjDecrementWeight},
{"hitcount_threshold", mObjHitThreshold},
{"hitcount_max", mObjMaxHitcount},
{"model_name", mModelName},
{"model_score_threshold", mModelScoreThreshold},
{"model_nms_threshold", mModelNmsThreshold}};

ParameterWrapper::declareParameters(this, paramSub, params);
ParameterWrapper::declareParameters(this, paramSub, params);

std::string packagePath{"/home/john/ros2_ws/src/mrover"};
std::string packagePath{"/home/john/ros2_ws/src/mrover"};

RCLCPP_INFO_STREAM(get_logger(), "Opening Model " << mModelName);
RCLCPP_INFO_STREAM(get_logger(), "Opening Model " << mModelName);

mLearning = Learning{mModelName, packagePath};

Expand All @@ -31,10 +30,10 @@ namespace mrover {
RCLCPP_INFO_STREAM(get_logger(), std::format("Object detector initialized with model: {} and thresholds: {} and {}", mModelName, mModelScoreThreshold, mModelNmsThreshold));
}

StereoObjectDetector::StereoObjectDetector(){
mSensorSub = create_subscription<sensor_msgs::msg::PointCloud2>("/camera/left/points", 1, [this](sensor_msgs::msg::PointCloud2::UniquePtr const& msg){
StereoObjectDetector::pointCloudCallback(msg);
});
StereoObjectDetector::StereoObjectDetector() {
mSensorSub = create_subscription<sensor_msgs::msg::PointCloud2>("/camera/left/points", 1, [this](sensor_msgs::msg::PointCloud2::UniquePtr const& msg) {
StereoObjectDetector::pointCloudCallback(msg);
});
}
} // namespace mrover

Expand Down
17 changes: 8 additions & 9 deletions perception/object_detector/object_detector.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,11 @@ namespace mrover {
class ObjectDetectorBase : public rclcpp::Node {

protected:
static constexpr char const* NODE_NAME = "object_detector";

static constexpr char const* NODE_NAME = "object_detector";

std::unique_ptr<tf2_ros::Buffer> mTfBuffer = std::make_unique<tf2_ros::Buffer>(get_clock());
std::shared_ptr<tf2_ros::TransformBroadcaster> mTfBroadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
std::shared_ptr<tf2_ros::TransformListener> mTfListener = std::make_shared<tf2_ros::TransformListener>(*mTfBuffer);
std::unique_ptr<tf2_ros::Buffer> mTfBuffer = std::make_unique<tf2_ros::Buffer>(get_clock());
std::shared_ptr<tf2_ros::TransformBroadcaster> mTfBroadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
std::shared_ptr<tf2_ros::TransformListener> mTfListener = std::make_shared<tf2_ros::TransformListener>(*mTfBuffer);

std::string mCameraFrame;
std::string mWorldFrame;
Expand All @@ -26,8 +25,8 @@ namespace mrover {
cv::Mat mRgbImage, mImageBlob;
sensor_msgs::msg::Image mDetectionsImageMessage;

rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr mDebugImgPub;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr mSensorSub;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr mDebugImgPub;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr mSensorSub;

// TODO(quintin): Do not hard code exactly two classes
std::vector<int> mObjectHitCounts{0, 0};
Expand Down Expand Up @@ -58,8 +57,8 @@ namespace mrover {
};

class StereoObjectDetector final : public ObjectDetectorBase {
public:
explicit StereoObjectDetector();
public:
explicit StereoObjectDetector();

static auto convertPointCloudToRGB(sensor_msgs::msg::PointCloud2::UniquePtr const& msg, cv::Mat const& image) -> void;

Expand Down
2 changes: 1 addition & 1 deletion perception/object_detector/object_detector.processing.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ namespace mrover {
mDetectionsImageMessage.header.stamp = get_clock()->now();
mDetectionsImageMessage.height = image.rows();
mDetectionsImageMessage.width = image.cols();
mDetectionsImageMessage.encoding = sensor_msgs::image_encodings::BGRA8;
mDetectionsImageMessage.encoding = sensor_msgs::image_encodings::BGRA8;
mDetectionsImageMessage.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__;
mDetectionsImageMessage.step = 4 * mDetectionsImageMessage.width;
mDetectionsImageMessage.data.resize(mDetectionsImageMessage.step * mDetectionsImageMessage.height);
Expand Down

0 comments on commit ba98358

Please sign in to comment.