37 #include <boost/assign.hpp> 44 DiagnosticNodelet::onInit();
45 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
46 dynamic_reconfigure::Server<Config>::CallbackType
f =
48 srv_->setCallback (f);
50 pub_ = advertise<geometry_msgs::PointStamped>(*
pnh_,
"output", 1);
51 pub_vector_ = advertise<geometry_msgs::Vector3Stamped>(
52 *
pnh_,
"output/ray", 1);
62 ros::V_string names = boost::assign::list_of(
"~input")(
"~input/camera_info");
79 const sensor_msgs::CameraInfo::ConstPtr& msg)
86 const geometry_msgs::PointStamped::ConstPtr& msg)
92 "[ProjectImagePoint::project] camera info is not yet available");
98 cv::Point2d(msg->point.x, msg->point.y));
99 geometry_msgs::Vector3Stamped
vector;
101 vector.header = msg->header;
102 vector.vector.x = ray.x;
103 vector.vector.y = ray.y;
104 vector.vector.z = ray.z;
110 double alpha =
z_ / ray.z;
111 geometry_msgs::PointStamped
point;
112 point.header = msg->header;
114 point.point.x = ray.x * alpha;
115 point.point.y = ray.y * alpha;
116 point.point.z = ray.z * alpha;
virtual void project(const geometry_msgs::PointStamped::ConstPtr &msg)
sensor_msgs::CameraInfo::ConstPtr camera_info_
virtual void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr &msg)
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
virtual void configCallback(Config &config, uint32_t level)
PLUGINLIB_EXPORT_CLASS(jsk_perception::ProjectImagePoint, nodelet::Nodelet)
ProjectImagePointConfig Config
std::vector< std::string > V_string
ros::Subscriber sub_camera_info_
virtual void unsubscribe()
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
ros::Publisher pub_vector_