diff --git a/perception/object_detector/object_detector.cpp b/perception/object_detector/object_detector.cpp index 41eb375d..7e8962ea 100644 --- a/perception/object_detector/object_detector.cpp +++ b/perception/object_detector/object_detector.cpp @@ -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(this); + auto paramSub = std::make_shared(this); - std::vector 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 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}; @@ -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("/camera/left/points", 1, [this](sensor_msgs::msg::PointCloud2::UniquePtr const& msg){ - StereoObjectDetector::pointCloudCallback(msg); - }); + StereoObjectDetector::StereoObjectDetector() { + mSensorSub = create_subscription("/camera/left/points", 1, [this](sensor_msgs::msg::PointCloud2::UniquePtr const& msg) { + StereoObjectDetector::pointCloudCallback(msg); + }); } } // namespace mrover diff --git a/perception/object_detector/object_detector.hpp b/perception/object_detector/object_detector.hpp index 23567416..66854466 100644 --- a/perception/object_detector/object_detector.hpp +++ b/perception/object_detector/object_detector.hpp @@ -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 mTfBuffer = std::make_unique(get_clock()); - std::shared_ptr mTfBroadcaster = std::make_shared(this); - std::shared_ptr mTfListener = std::make_shared(*mTfBuffer); + std::unique_ptr mTfBuffer = std::make_unique(get_clock()); + std::shared_ptr mTfBroadcaster = std::make_shared(this); + std::shared_ptr mTfListener = std::make_shared(*mTfBuffer); std::string mCameraFrame; std::string mWorldFrame; @@ -26,8 +25,8 @@ namespace mrover { cv::Mat mRgbImage, mImageBlob; sensor_msgs::msg::Image mDetectionsImageMessage; - rclcpp::Publisher::SharedPtr mDebugImgPub; - rclcpp::Subscription::SharedPtr mSensorSub; + rclcpp::Publisher::SharedPtr mDebugImgPub; + rclcpp::Subscription::SharedPtr mSensorSub; // TODO(quintin): Do not hard code exactly two classes std::vector mObjectHitCounts{0, 0}; @@ -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; diff --git a/perception/object_detector/object_detector.processing.cpp b/perception/object_detector/object_detector.processing.cpp index bfe79234..7ffa678b 100644 --- a/perception/object_detector/object_detector.processing.cpp +++ b/perception/object_detector/object_detector.processing.cpp @@ -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);