3dtopixel.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <opencv/cv.h>
00003 #include <image_geometry/pinhole_camera_model.h>
00004 #include <tf/transform_listener.h>
00005 #include "jsk_smart_gui/point2screenpoint.h"
00006 
00007 class TransformPixel
00008 {
00009   ros::NodeHandle nh_;
00010   ros::Subscriber camerainfosub_;
00011   ros::ServiceServer srv_;
00012   tf::TransformListener tf_listener_;
00013   image_geometry::PinholeCameraModel cam_model_;
00014   std::string camera_topic;
00015 
00016 public:
00017   TransformPixel()
00018   {
00019     camerainfosub_ = nh_.subscribe<sensor_msgs::CameraInfo>(nh_.resolveName("camera_info"), 100, &TransformPixel::camerainfoCb, this);
00020     srv_ = nh_.advertiseService("TransformPixelRequest", &TransformPixel::objectSrv,this);
00021   }
00022 
00023   bool objectSrv(jsk_smart_gui::point2screenpoint::Request &req,
00024                  jsk_smart_gui::point2screenpoint::Response &res)
00025   {
00026     ROS_INFO("3dtopixel request:x=%lf,y=%lf,z=%lf",req.point.point.x,req.point.point.y,req.point.point.z);
00027     geometry_msgs::PointStamped point_transformed;
00028     tf_listener_.transformPoint(camera_topic, req.point, point_transformed);
00029     cv::Point3d xyz; cv::Point2d uv_rect;
00030     xyz.x = point_transformed.point.x;
00031     xyz.y = point_transformed.point.y;
00032     xyz.z = point_transformed.point.z;
00033     uv_rect = cam_model_.project3dToPixel(xyz);
00034     res.x=uv_rect.x;res.y=uv_rect.y;
00035     return true;
00036   }
00037 
00038   void camerainfoCb(const sensor_msgs::CameraInfoConstPtr& info_msg)
00039   {
00040     ROS_INFO("infocallback :shutting down camerainfosub");
00041     cam_model_.fromCameraInfo(info_msg);
00042     camera_topic = info_msg->header.frame_id;
00043     camerainfosub_.shutdown();
00044   }
00045 };
00046 
00047 int
00048 main (int argc, char** argv)
00049 {
00050   ros::init(argc,argv,"threedtopixel");
00051   TransformPixel r;
00052   ros::spin();
00053 }
00054 


jsk_smart_gui
Author(s):
autogenerated on Thu Jun 6 2019 18:04:26