From 42814614e3bb907d6941e37cb26004cc3964bbff Mon Sep 17 00:00:00 2001 From: apoorva Date: Tue, 28 Mar 2023 17:21:18 +0530 Subject: [PATCH] updated find pose --- ros2_ws/src/find-pose/CMakeLists.txt | 4 ++-- .../src/find-pose/include/find-pose/FindObjectROS.h | 10 +++++----- ros2_ws/src/find-pose/package.xml | 4 ++-- ros2_ws/src/find-pose/src/FindObjectROS.cpp | 4 ++-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ros2_ws/src/find-pose/CMakeLists.txt b/ros2_ws/src/find-pose/CMakeLists.txt index 1b1d8d3c..ff3b424f 100644 --- a/ros2_ws/src/find-pose/CMakeLists.txt +++ b/ros2_ws/src/find-pose/CMakeLists.txt @@ -59,7 +59,7 @@ find_package(sensor_msgs REQUIRED) find_package(pcl_msgs REQUIRED) find_package(image_transport REQUIRED) find_package(message_filters REQUIRED) -find_package(yolov3_msg REQUIRED) +find_package(pipe_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 pcl_conversions image_transport message_filters) @@ -78,7 +78,7 @@ 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 yolov3_msg cv_bridge sensor_msgs image_transport message_filters tf2_msgs tf2_geometry_msgs ${AMENT_LIBRARIES}) +ament_target_dependencies(find_pose_node rclcpp rclcpp_components pipe_msgs cv_bridge sensor_msgs image_transport message_filters tf2_msgs tf2_geometry_msgs ${AMENT_LIBRARIES}) target_link_libraries(find_pose_node Qt5::Core Qt5::Widgets ${OpenCV_LIBRARIES} ${LIBRARIES}) diff --git a/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h b/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h index 6090f9e1..f6dfcc96 100644 --- a/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h +++ b/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h @@ -49,8 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include "yolov3_msg/msg/bounding_boxes.hpp" -#include "yolov3_msg/msg/bounding_box.hpp" +#include "pipe_msgs/msg/bounding_boxes.hpp" +#include "pipe_msgs/msg/bounding_box.hpp" class FindObjectROS { @@ -71,7 +71,7 @@ private: void estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg, const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, - const yolov3_msg::msg::BoundingBoxes::ConstSharedPtr bboxes); + const pipe_msgs::msg::BoundingBoxes::ConstSharedPtr bboxes); double distance(geometry_msgs::msg::TransformStamped point1, geometry_msgs::msg::TransformStamped point2); double angular_distance(geometry_msgs::msg::TransformStamped point1, geometry_msgs::msg::TransformStamped point2); @@ -85,14 +85,14 @@ private: image_transport::SubscriberFilter rgbSub_; image_transport::SubscriberFilter depthSub_; message_filters::Subscriber cameraInfoSub_; - message_filters::Subscriber bboxSub_; + message_filters::Subscriber bboxSub_; bool tf_broadcast_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, - yolov3_msg::msg::BoundingBoxes> MyApproxSyncPolicy; + pipe_msgs::msg::BoundingBoxes> MyApproxSyncPolicy; message_filters::Synchronizer * approxSync_; diff --git a/ros2_ws/src/find-pose/package.xml b/ros2_ws/src/find-pose/package.xml index d54fc84b..8f5b2986 100644 --- a/ros2_ws/src/find-pose/package.xml +++ b/ros2_ws/src/find-pose/package.xml @@ -22,8 +22,8 @@ cv_bridge pcl_msgs pcl_msgs - yolov3_msg - yolov3_msg + pipe_msgs + pipe_msgs image_transport image_transport message_filters diff --git a/ros2_ws/src/find-pose/src/FindObjectROS.cpp b/ros2_ws/src/find-pose/src/FindObjectROS.cpp index a79c4932..686bf785 100644 --- a/ros2_ws/src/find-pose/src/FindObjectROS.cpp +++ b/ros2_ws/src/find-pose/src/FindObjectROS.cpp @@ -71,7 +71,7 @@ FindObjectROS::FindObjectROS(rclcpp::Node * node) : void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg, const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, - const yolov3_msg::msg::BoundingBoxes::ConstSharedPtr bboxes) + const pipe_msgs::msg::BoundingBoxes::ConstSharedPtr bboxes) { if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0) @@ -106,7 +106,7 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha std::vector transforms; for(size_t i = 0; i < num_detections ; i++) { - yolov3_msg::msg::BoundingBox object_bbox = bboxes->bounding_boxes[i]; + pipe_msgs::msg::BoundingBox object_bbox = bboxes->bounding_boxes[i]; float object_width = object_bbox.xmax - object_bbox.xmin; float object_height = object_bbox.ymax - object_bbox.ymin; // top left is the 0,0