From 0e1db823a82e601a277ab53a595c865879fe3ba6 Mon Sep 17 00:00:00 2001 From: apoorva Date: Wed, 22 Mar 2023 11:22:45 +0530 Subject: [PATCH] pose estination updated --- ros2_ws/src/find-pose/src/FindObjectROS.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/ros2_ws/src/find-pose/src/FindObjectROS.cpp b/ros2_ws/src/find-pose/src/FindObjectROS.cpp index aa5a870e..63be1d0b 100644 --- a/ros2_ws/src/find-pose/src/FindObjectROS.cpp +++ b/ros2_ws/src/find-pose/src/FindObjectROS.cpp @@ -59,9 +59,9 @@ FindObjectROS::FindObjectROS(rclcpp::Node * node) : RCLCPP_INFO(node->get_logger(), "object_prefix = %s", objFramePrefix_.c_str()); RCLCPP_INFO(node->get_logger(), "pnp = %s", usePnP_?"true":"false"); image_transport::TransportHints hints(node); - rgbSub_.subscribe(node, "/camera/color/image_raw", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); - depthSub_.subscribe(node, "/camera/aligned_depth_to_color/image_raw", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(node, "/camera/color/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); + rgbSub_.subscribe(node, "rgb_img", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); + depthSub_.subscribe(node, "/depth_img", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); + cameraInfoSub_.subscribe(node, "/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); bboxSub_.subscribe(node, "/bboxes", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_, bboxSub_); @@ -78,6 +78,7 @@ 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,7 +107,8 @@ 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::vector pose_vec; + geometry_msgs::msg::TransformStamped left_trail; + geometry_msgs::msg::TransformStamped right_trail; for(size_t i = 0; i < num_detections ; i++) { yolov3_msg::msg::BoundingBox object_bbox = bboxes->bounding_boxes[i]; @@ -120,12 +122,15 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha 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); pose.child_frame_id = object_class; - pose_vec.push_back(pose); + 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; } // 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; + std::cout << "Distance in meters !!!!!!" << distance(left_trail, right_trail) << std::endl; std::cout << std::endl; } catch(const cv_bridge::Exception & e) @@ -195,7 +200,7 @@ geometry_msgs::msg::TransformStamped FindObjectROS::estimate_pose(cv::Mat rgb_im 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; +// 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);