diff --git a/ros2_ws/src/find-pose/CMakeLists.txt b/ros2_ws/src/find-pose/CMakeLists.txt new file mode 100644 index 00000000..d859e755 --- /dev/null +++ b/ros2_ws/src/find-pose/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.8) +project(find-pose) + + +# find dependencies +find_package(ament_cmake REQUIRED) + +find_package(Boost REQUIRED COMPONENTS system) # dependencies from PCL +find_package(PCL REQUIRED COMPONENTS common io) + +find_package(depth_image_proc REQUIRED) +include_directories(SYSTEM + ${PCL_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) +include_directories(${depth_image_proc_INCLUDE_DIRS}) + +SET(Libraries + pcl_conversions +) +####### DEPENDENCIES ####### +FIND_PACKAGE(OpenCV REQUIRED) # tested on 2.3.1 + +# check if version status is "-dev" (SIFT compatibility issue between 4.3.0 vs 4.3.0-dev) +FIND_FILE(OpenCV_VERSION_HPP opencv2/core/version.hpp + PATHS ${OpenCV_INCLUDE_DIRS} + NO_DEFAULT_PATH) +FILE(READ ${OpenCV_VERSION_HPP} TMPTXT) +STRING(FIND "${TMPTXT}" "-dev" matchres) +IF(NOT ${matchres} EQUAL -1) + add_definitions(-DOPENCV_DEV) +ENDIF(NOT ${matchres} EQUAL -1) + +SET(NONFREE 1) +IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)) + SET(NONFREE 0) +ELSEIF(OpenCV_VERSION VERSION_GREATER "3.4.2") + FIND_FILE(OpenCV_MODULES_HPP opencv2/opencv_modules.hpp + PATHS ${OpenCV_INCLUDE_DIRS} + NO_DEFAULT_PATH) + FILE(READ ${OpenCV_MODULES_HPP} TMPTXT) + STRING(FIND "${TMPTXT}" "#define OPENCV_ENABLE_NONFREE" matchres) + IF(${matchres} EQUAL -1) + SET(NONFREE 0) + ENDIF(${matchres} EQUAL -1) +ENDIF() + +############################################################################ + + +find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pcl_msgs REQUIRED) + +add_executable(convert_pointcloud_to_depth src/convert_pointcloud_to_depth.cpp) +ament_target_dependencies(convert_pointcloud_to_depth ${Libraries} rclcpp_components rclcpp sensor_msgs cv_bridge) +target_link_libraries(convert_pointcloud_to_depth ${PCL_LBRARIES} ${Boost_LIBRARIES}) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + SET(SRC_FILES + ${SRC_FILES} + src/FindObjectROS.cpp +) +find_package(Qt5 COMPONENTS Core Widgets REQUIRED) +include_directories(${Qt5Core_INCLUDE_DIRS} ${Qt5Widgets_INCLUDE_DIRS}) +#ADD_LIBRARY(find_object ${SRC_FILES}) +add_executable(find_pose_node src/FindObjectROS.cpp src/find_pose_node.cpp) +set_target_properties(find_pose_node PROPERTIES OUTPUT_NAME find_pose_node) +ament_target_dependencies(find_pose_node rclcpp rclcpp_components cv_bridge tf2_msgs tf2_geometry_msgs ${AMENT_LIBRARIES}) +target_link_libraries(find_pose_node Qt5::Core Qt5::Widgets ${OpenCV_LIBRARIES} ${LIBRARIES}) + + + +install(TARGETS + convert_pointcloud_to_depth + find_pose_node + DESTINATION lib/${PROJECT_NAME}) + + + + + + +ament_package() diff --git a/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h b/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h new file mode 100644 index 00000000..46e08f27 --- /dev/null +++ b/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h @@ -0,0 +1,75 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef FINDOBJECTROS_H_ +#define FINDOBJECTROS_H_ + +#include +#include +#include +// #include "find_object/FindObject.h" + +#include +// #include "find_object_2d/msg/objects_stamped.hpp" +// #include "find_object_2d/msg/detection_info.hpp" + +// #include +// #include +// #include +// #include +// #include + +class FindObjectROS +{ + // Q_OBJECT; + +public: + FindObjectROS(rclcpp::Node * node); + virtual ~FindObjectROS() {} + +// public Q_SLOTS: +// void publish(const find_object::DetectionInfo & info, const find_object::Header & header, const cv::Mat & depth, float depthConstant); + void estimate_pose(); +private: + cv::Vec3f getDepth(const cv::Mat & depthImage, + int x, int y, + float cx, float cy, + float fx, float fy); + +private: + rclcpp::Node * node_; + rclcpp::Publisher::SharedPtr pub_; + // rclcpp::Publisher::SharedPtr pubStamped_; + // rclcpp::Publisher::SharedPtr pubInfo_; + + std::string objFramePrefix_; + bool usePnP_; + std::shared_ptr tfBroadcaster_; + +}; + +#endif /* FINDOBJECTROS_H_ */ diff --git a/ros2_ws/src/find-pose/package.xml b/ros2_ws/src/find-pose/package.xml new file mode 100644 index 00000000..e75fc6c5 --- /dev/null +++ b/ros2_ws/src/find-pose/package.xml @@ -0,0 +1,31 @@ + + + + find-pose + 0.0.0 + TODO: Package description + parallels + TODO: License declaration + + ament_cmake + libpcl-all-dev + rclcpp + pcl + sensor_msgs + pcl_conversions + pcl_conversions + depth_image_proc + depth_image_proc + pcl_ros + cv_bridge + cv_bridge + pcl_msgs + pcl_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_ws/src/find-pose/src/FindObjectROS.cpp b/ros2_ws/src/find-pose/src/FindObjectROS.cpp new file mode 100644 index 00000000..9d31fc97 --- /dev/null +++ b/ros2_ws/src/find-pose/src/FindObjectROS.cpp @@ -0,0 +1,218 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "find-pose/FindObjectROS.h" + +#include +#include +#include +#include +#ifdef PRE_ROS_HUMBLE +#include +#else +#include +#endif + +#include +#include +#include +#include + +#include + +// using namespace find_object; + +FindObjectROS::FindObjectROS(rclcpp::Node * node) : + node_(node), + objFramePrefix_("object"), + usePnP_(true) +{ + tfBroadcaster_ = std::make_shared(node); + + objFramePrefix_ = node->declare_parameter("object_prefix", objFramePrefix_); + usePnP_ = node->declare_parameter("pnp", usePnP_); + RCLCPP_INFO(node->get_logger(), "object_prefix = %s", objFramePrefix_.c_str()); + RCLCPP_INFO(node->get_logger(), "pnp = %s", usePnP_?"true":"false"); + + // pub_ = node->create_publisher("objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1)); + // pubStamped_ = node->create_publisher("objectsStamped", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1)); + // pubInfo_ = node->create_publisher("info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1)); + + // this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float))); +} + +void FindObjectROS::estimate_pose() +{ + // Read RGB Image + cv::Mat rgb_image = cv::imread("/media/psf/freelancing/greenhouse/yolov3/runs/detect/exp8/stereo_image952.jpeg"); + // read depth Image + cv::Mat depth = cv::imread("/media/psf/freelancing/greenhouse/depth_try1/depth_img_952.jpeg"); + // bounding box dimensions + + std::vector transforms; + // store bounding boxes. (This should come from yolo ideally) + float objectWidth = 372; // width of bounding box in pixels + float objectHeight = 554; // height of bounding box in pixels + + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.x=0; + transform.transform.rotation.y=0; + transform.transform.rotation.z=0; + transform.transform.rotation.w=1; + transform.transform.translation.x=0; + transform.transform.translation.y=0; + transform.transform.translation.z=0; + transform.child_frame_id = "l_trail"; + transform.header.frame_id = "camera_link"; + // transform.header.stamp.sec = ros::Time::now(); + tf2::Quaternion q; + + std::vector objectPoints(4); + std::vector imagePoints(4); + + objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0); + objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0); + objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0); + objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0); + + // QPointF pt = iter->map(QPointF(0, 0)); + imagePoints[0] = cv::Point2f(246,665); + // pt = iter->map(QPointF(objectWidth, 0)); + imagePoints[1] = cv::Point2f(618, 665); + // pt = iter->map(QPointF(objectWidth, objectHeight)); + imagePoints[2] = cv::Point2f(618,111); + // pt = iter->map(QPointF(0, objectHeight)); + imagePoints[3] = cv::Point2f(246, 111); + + float depthConstant = 1.0f/641.587158203125; + cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1); + cameraMatrix.at(0,0) = 1.0f/depthConstant; + cameraMatrix.at(1,1) = 1.0f/depthConstant; + cameraMatrix.at(0,2) = float(depth.cols/2)-0.5f; + cameraMatrix.at(1,2) = float(depth.rows/2)-0.5f; + cv::Mat distCoeffs; + std::cout << "depth img cols: " << depth.cols << " rows: " << depth.rows << std::endl; + cv::Mat rvec(1,3, CV_64FC1); + cv::Mat tvec(1,3, CV_64FC1); + cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); + + std::cout << "Pose tvec x: " << tvec.at(0) << " y: " << tvec.at(1) << " z: " << tvec.at(2) << std::endl; + + cv::Mat R; + cv::Rodrigues(rvec, R); + tf2::Matrix3x3 rotationMatrix( + R.at(0,0), R.at(0,1), R.at(0,2), + R.at(1,0), R.at(1,1), R.at(1,2), + R.at(2,0), R.at(2,1), R.at(2,2)); + rotationMatrix.getRotation(q); + + QPointF center = QPointF(objectWidth/2, objectHeight/2); + cv::Vec3f center3D = this->getDepth(depth, + center.x()+0.5f, center.y()+0.5f, + float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, + 1.0f/depthConstant, 1.0f/depthConstant); + + transform.transform.translation.x = tvec.at(0)*(center3D.val[2]/tvec.at(2)); + transform.transform.translation.y = tvec.at(1)*(center3D.val[2]/tvec.at(2)); + transform.transform.translation.z = tvec.at(2)*(center3D.val[2]/tvec.at(2)); + std::cout << "Pose x: " << transform.transform.translation.x << " y: " << transform.transform.translation.y << " z: " << transform.transform.translation.z << std::endl; + // set x axis going front of the object, with z up and y left + tf2::Quaternion q2; + q2.setRPY(CV_PI/2.0, CV_PI/2.0, 0); + q *= q2; + transform.transform.rotation = tf2::toMsg(q.normalized()); + transforms.push_back(transform); + if(transforms.size()) + { + tfBroadcaster_->sendTransform(transforms); + } +} + +cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage, + int x, int y, + float cx, float cy, + float fx, float fy) +{ + if(!(x >=0 && x=0 && yget_logger(), "Point must be inside the image (x=%d, y=%d), image size=(%d,%d)", + x, y, + depthImage.cols, depthImage.rows); + return cv::Vec3f( + std::numeric_limits::quiet_NaN (), + std::numeric_limits::quiet_NaN (), + std::numeric_limits::quiet_NaN ()); + } + + + cv::Vec3f pt; + + // Use correct principal point from calibration + float center_x = cx; //cameraInfo.K.at(2) + float center_y = cy; //cameraInfo.K.at(5) + + std::cout << "cx: " << center_x << " cy: " << center_y << std::endl; + std::cout << "fx: " << fx << " fy: " << fy << std::endl; + bool isInMM = depthImage.type() == CV_16UC1; // is in mm? + + // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) + float unit_scaling = isInMM?0.001f:1.0f; + float constant_x = unit_scaling / fx; //cameraInfo.K.at(0) + float constant_y = unit_scaling / fy; //cameraInfo.K.at(4) + float bad_point = std::numeric_limits::quiet_NaN (); + + float depth; + bool isValid; + if(isInMM) + { + std::cout << "depth is in mm!!" << std::endl; + depth = (float)depthImage.at(y,x); + isValid = depth != 0.0f; + } + else + { + + depth = depthImage.at(y,x); + isValid = std::isfinite(depth) && depth > 0.0f; + std::cout << "depth is NOT in mm!! " << depth << std::endl; + } + + // Check for invalid measurements + if (!isValid) + { + pt.val[0] = pt.val[1] = pt.val[2] = bad_point; + } + else + { + // Fill in XYZ + pt.val[0] = (float(x) - center_x) * depth * constant_x; + pt.val[1] = (float(y) - center_y) * depth * constant_y; + pt.val[2] = depth*unit_scaling; + std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl; + } + return pt; +} diff --git a/ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp b/ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp new file mode 100644 index 00000000..47d7c360 --- /dev/null +++ b/ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp @@ -0,0 +1,176 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +int count = 0; +class PointCloudToDepthImageNode : public rclcpp::Node { +public: + PointCloudToDepthImageNode() : Node("pointcloud_to_depth_image") { + sub_rgb_ = create_subscription( + "/camera/color/image_raw", 10, std::bind(&PointCloudToDepthImageNode::rgb_callback, this, std::placeholders::_1)); + subscription_ = create_subscription( + "/camera/depth/color/points", 10, std::bind(&PointCloudToDepthImageNode::pc_callback, this, std::placeholders::_1)); + publisher_ = create_publisher("/depth_image_topic", rclcpp::QoS(10)); + } + +private: + // void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + // // Create a point cloud object from the message + + // pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + + // pcl::moveFromROSMsg(*msg, *cloud); + + // // Determine the size of the depth image (assume 640x480 resolution) + // const int width = 1280; + // const int height = 720; + + // // Create a depth image + // cv::Mat depth_image(height, width, CV_32F, cv::Scalar::all(0)); + + // // Convert the point cloud to a depth image + // for (int i = 0; i < cloud->points.size(); i++) { + // pcl::PointXYZ point = cloud->points[i]; + + // // Calculate the pixel coordinates of the point in the depth image + // int u = (int)(point.x / point.z * fx_ + cx_); + // int v = (int)(point.y / point.z * fy_ + cy_); + + // // Ignore points that are outside of the image bounds + // if (u < 0 || u >= width || v < 0 || v >= height) { + // continue; + // } + + // // Calculate the depth of the point + // float depth = sqrt(point.x * point.x + point.y * point.y + point.z * point.z); + + // // Update the depth image + // depth_image.at(v, u) = depth; + // } + + // // Convert the depth image to a ROS message and publish it + // sensor_msgs::msg::Image::SharedPtr msg_out = cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::TYPE_32FC1, depth_image).toImageMsg(); + // publisher_->publish(*msg_out); + // cv::imwrite("/media/psf/freelancing/greenhouse/depth_try1/depth_img_" + std::to_string(count) + ".jpeg" , depth_image); + // count++; + // } + + // second method to use both rgb and point cloud to create depth image + void rgb_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + std::cout << "rgb cb" << std::endl; + // Store the RGB image message + rgb_msg_ = msg; + } + + void pc_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + std::cout << "pc cb" << std::endl; + // Convert the point cloud message to PCL point cloud + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *cloud); + // if(cloud->isOrganized()) + // { + // std::cout << "isUnorganized"<< std::endl; + // } + + // Convert the RGB image message to OpenCV Mat + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(rgb_msg_, sensor_msgs::image_encodings::BGR8); + } + catch (const cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + // Create the depth image with the same size and type as the RGB image + cv::Mat depth_img(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1); + std::cout << "Depth img size: " << cv_ptr->image.rows << " " << cv_ptr->image.cols << std::endl; + std::cout << "total points: " << cloud->points.size() << std::endl; + for (int i = 0; i < cloud->points.size(); i++) { + pcl::PointXYZ point = cloud->points[i]; + + // Calculate the pixel coordinates of the point in the depth image + int u = (int)(point.x / point.z * fx_ + cx_); + int v = (int)(point.y / point.z * fy_ + cy_); + std::cout << "u: " << point.x << " v: " << point.y << std::endl; + + // Ignore points that are outside of the image bounds + if (u < 0 || u >= cv_ptr->image.rows || v < 0 || v >= cv_ptr->image.cols) { + continue; + } + + // Calculate the depth of the point + float depth = sqrt(point.x * point.x + point.y * point.y + point.z * point.z); + // Update the depth image + // depth_image.at(v, u) = depth; + } + // // Create a VoxelGrid filter object + // pcl::VoxelGrid voxel_grid; + + // // Set the leaf size of the filter + // voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f); // The leaf size determines the size of the grid cells + + // // Set the input cloud for the filter + // voxel_grid.setInputCloud(*cloud); + + // // Create an empty point cloud to store the output (organized) cloud + // pcl::PointCloud::Ptr cloud_organized(new pcl::PointCloud); + + // // Apply the filter + // voxel_grid.filter(*cloud_organized); + // int width = cloud_organized->width; // Width of the point cloud + // int height = cloud_organized->height; + // std::cout << "height: " << height <<" width: " << width << std::endl; + + // Iterate over each pixel in the RGB image and get the corresponding depth value from the point cloud + for (int i = 0; i < cv_ptr->image.rows; i++) + { + for (int j = 0; j < cv_ptr->image.cols; j++) + { + // Get the 3D point from the point cloud corresponding to the current pixel in the RGB image + pcl::PointXYZ pt = cloud->at(j, i); + + // Set the depth value of the current pixel in the depth image to the z-coordinate of the 3D point + depth_img.at(i, j) = pt.z; + std::cout << "pt z: " << pt.z << std::endl; + } + } + // Convert the depth image to ROS message and publish + sensor_msgs::msg::Image::SharedPtr depth_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "32FC1", depth_img).toImageMsg(); + publisher_->publish(*depth_msg); + } + + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr sub_rgb_; + rclcpp::Publisher::SharedPtr publisher_; + + sensor_msgs::msg::Image::SharedPtr rgb_msg_; + + // Camera intrinsic parameters (replace with actual values) + float fx_ = 643.008; + float fy_ = 642.201; + float cx_ = 648.812; + float cy_ = 360.262; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_ws/src/find-pose/src/find_pose_node.cpp b/ros2_ws/src/find-pose/src/find_pose_node.cpp new file mode 100644 index 00000000..42d1f1f5 --- /dev/null +++ b/ros2_ws/src/find-pose/src/find_pose_node.cpp @@ -0,0 +1,52 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "find-pose/FindObjectROS.h" + +class FindPoseNode : public rclcpp::Node { + + public: + FindPoseNode() : + rclcpp::Node("find_object_2d"), + findObjectROS_(0) + { + findObjectROS_ = new FindObjectROS(this); + findObjectROS_->estimate_pose(); + } + + private: + FindObjectROS * findObjectROS_; +}; + + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); +}