From 126124c4dafb572df22551d6273dae3be36beeb0 Mon Sep 17 00:00:00 2001 From: apoorva Date: Mon, 27 Mar 2023 16:11:52 +0530 Subject: [PATCH] tf broadcast for each object --- .../include/find-pose/FindObjectROS.h | 3 ++ ros2_ws/src/find-pose/src/FindObjectROS.cpp | 43 ++++++++++++++----- 2 files changed, 36 insertions(+), 10 deletions(-) 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 f7b31e90..0d14f466 100644 --- a/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h +++ b/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h @@ -73,6 +73,8 @@ private: const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const yolov3_msg::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); + private: rclcpp::Node * node_; @@ -90,6 +92,7 @@ private: image_transport::SubscriberFilter depthSub_; message_filters::Subscriber cameraInfoSub_; message_filters::Subscriber bboxSub_; + bool tf_broadcast_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::msg::Image, diff --git a/ros2_ws/src/find-pose/src/FindObjectROS.cpp b/ros2_ws/src/find-pose/src/FindObjectROS.cpp index 63be1d0b..2252b7bc 100644 --- a/ros2_ws/src/find-pose/src/FindObjectROS.cpp +++ b/ros2_ws/src/find-pose/src/FindObjectROS.cpp @@ -49,7 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. FindObjectROS::FindObjectROS(rclcpp::Node * node) : node_(node), objFramePrefix_("object"), - usePnP_(true) + usePnP_(true), + tf_broadcast_(true) { int queueSize = 10; tfBroadcaster_ = std::make_shared(node); @@ -78,7 +79,6 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const yolov3_msg::msg::BoundingBoxes::ConstSharedPtr bboxes) { - std::cout << "callback!!!!!!!!" << std::endl; if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0) { @@ -106,9 +106,11 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha } size_t num_detections = bboxes->bounding_boxes.size(); - std::cout << "num_detections: " << num_detections << std::endl; + std::cout << "Number of detections: " << num_detections << std::endl; geometry_msgs::msg::TransformStamped left_trail; geometry_msgs::msg::TransformStamped right_trail; + std::vector transforms; + std::vector> detection_names; for(size_t i = 0; i < num_detections ; i++) { yolov3_msg::msg::BoundingBox object_bbox = bboxes->bounding_boxes[i]; @@ -121,16 +123,22 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha cv::Point2d bottom_left = cv::Point2f(float(object_bbox.xmax), float(object_bbox.ymin)); std::string object_class = object_bbox.class_id; geometry_msgs::msg::TransformStamped pose = estimate_pose(image, ptrDepth->image, object_width, object_height, top_left, top_right, bottom_right, bottom_left); + detection_names.push_back(object_class); pose.child_frame_id = object_class; if (object_class == std::string("l_trail")) left_trail = pose; if (object_class == std::string("r_trail")) right_trail = pose; std::cout << "Pose for "<< object_class << " x: " << pose.transform.translation.x << " y: " << pose.transform.translation.y << " z: " << pose.transform.translation.z << std::endl; + transforms.push_back(pose); } - // geometry_msgs::msg::TransformStamped right_trail = estimate_pose(image, ptrDepth->image, 115, 116, cv::Point2f(819,600), cv::Point2f(974,600), cv::Point2f(974,716), cv::Point2f(819,716)); - // geometry_msgs::msg::TransformStamped left_trail = estimate_pose(image, ptrDepth->image, 160, 168, cv::Point2f(350,548), cv::Point2f(510,548), cv::Point2f(510,716), cv::Point2f(350,716)); - std::cout << "Distance in meters !!!!!!" << distance(left_trail, right_trail) << std::endl; + if(tf_broadcast_ && transforms.size()) + { + std::cout << "broadcast tf" << std::endl; + tfBroadcaster_->sendTransform(transforms); + } + std::cout << "Euclidean distance between two pipes in meters !!!!!!" << distance(left_trail, right_trail) << std::endl; + std::cout << "Angular distance between two pipes in radians !!!!!!" << angular_distance(left_trail, right_trail) << std::endl; std::cout << std::endl; } catch(const cv_bridge::Exception & e) @@ -142,9 +150,6 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha geometry_msgs::msg::TransformStamped FindObjectROS::estimate_pose(cv::Mat rgb_image, cv::Mat depth, float objectWidth, float objectHeight, cv::Point2f point_1, cv::Point2f point_2, cv::Point2f point_3, cv::Point2f point_4) { - - - std::vector transforms; // store bounding boxes. (This should come from yolo ideally) // float objectWidth = 160; // width of bounding box in pixels // float objectHeight = 168; // height of bounding box in pixels @@ -228,7 +233,7 @@ geometry_msgs::msg::TransformStamped FindObjectROS::estimate_pose(cv::Mat rgb_im q2.setRPY(CV_PI/2.0, CV_PI/2.0, 0); q *= q2; transform.transform.rotation = tf2::toMsg(q.normalized()); - transforms.push_back(transform); + return transform; // if(transforms.size()) // { @@ -310,3 +315,21 @@ double FindObjectROS::distance(geometry_msgs::msg::TransformStamped point1, geom vector.z = point2.transform.translation.z - point1.transform.translation.z; return std::sqrt(vector.x*vector.x + vector.y*vector.y + vector.z*vector.z); } + +double FindObjectROS::angular_distance(geometry_msgs::msg::TransformStamped point1, geometry_msgs::msg::TransformStamped point2) +{ + tf2::Quaternion quat1(point1.transform.rotation.x, point1.transform.rotation.y, point1.transform.rotation.z, point1.transform.rotation.w); + tf2::Quaternion quat2(point2.transform.rotation.x, point2.transform.rotation.y, point2.transform.rotation.z, point2.transform.rotation.w); + + // Assume that 'pos1' and 'pos2' are the positions of the two points + // and 'quat1' and 'quat2' are their corresponding quaternions + tf2::Vector3 diff; + diff[0] = point2.transform.translation.x - point1.transform.translation.x; + diff[1] = point2.transform.translation.y - point1.transform.translation.y; + diff[2] = point2.transform.translation.z - point1.transform.translation.z; + tf2::Quaternion rot = quat2 * quat1.inverse(); + tf2::Quaternion diff_quat(0, diff.getX(), diff.getY(), diff.getZ()); + tf2::Quaternion result = rot * diff_quat * rot.inverse(); + double angle = 2 * std::acos(result.getW()); + return angle; +}