29 #include <tf/transform_broadcaster.h>
30 #include <geometry_msgs/TransformStamped.h>
31 #include <image_transport/image_transport.h>
32 #include <cv_bridge/cv_bridge.h>
33 #include <sensor_msgs/image_encodings.h>
34 #include <opencv2/imgproc/imgproc.hpp>
35 #include <opencv2/highgui/highgui.hpp>
36 #include <sensor_msgs/image_encodings.h>
37 #include <driver_base/driver.h>
38 #include <camera_info_manager/camera_info_manager.h>
39 #include <tf/transform_broadcaster.h>
51 ros::NodeHandle private_node(
"~");
53 string camera_calibration_file;
54 private_node.param(
"camera_calibration_file",camera_calibration_file,
string(
"image calibration file not set"));
57 private_node.param(
"camera_name",camera_name,
string(
""));
59 info_manager =
new camera_info_manager::CameraInfoManager(ros::NodeHandle(nh, camera_name),camera_name,camera_calibration_file);
63 publisher = it.advertiseCamera(
"new_info/image_raw", 1);
69 info = info_manager->getCameraInfo();
70 publisher.publish(image, info, image.header.stamp);
74 image_transport::ImageTransport
it;
78 sensor_msgs::CameraInfo
info;
87 int main(
int argc,
char** argv)
89 ros::init(argc, argv,
"camera_info_republisher");
void imageHandler(const sensor_msgs::ImageConstPtr &msg)
image_transport::Subscriber subscriber
CameraInfoRepublisher(ros::NodeHandle &nh_)
image_transport::ImageTransport it
int main(int argc, char **argv)
image_transport::CameraPublisher publisher
camera_info_manager::CameraInfoManager * info_manager
sensor_msgs::CameraInfo info