apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

tf broadcast for each object

Browse Source
This commit is contained in:
Apoorva Gupta 2023-03-27 16:11:52 +05:30
parent b3d2fa0ee6
commit 126124c4da
2 changed files with 36 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h
View File

@ -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<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<yolov3_msg::msg::BoundingBoxes> bboxSub_;
bool tf_broadcast_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::msg::Image,

43
ros2_ws/src/find-pose/src/FindObjectROS.cpp
View File

@ -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<tf2_ros::TransformBroadcaster>(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<geometry_msgs::msg::TransformStamped> transforms;
std::vector<std::string>> 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<geometry_msgs::msg::TransformStamped> 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;
}