51 ROS_WARN(
"~yaml_fliename is not specified, use default camera info parameters");
58 srv_ = std::make_shared <dynamic_reconfigure::Server<Config> > (pnh);
59 dynamic_reconfigure::Server<Config>::CallbackType
f =
62 srv_->setCallback (f);
66 ROS_WARN(
"~frame_id is not specified, use camera as frame_id");
70 ROS_WARN(
"~parent_frame_id is not specified, use base_link as parent_frame_id");
81 if (!pnh.
getParam(
"sync_pointcloud", sync_pointcloud)) {
82 sync_pointcloud =
false;
84 if (sync_pointcloud) {
85 ROS_INFO(
"~sync_pointcloud is specified, synchronize ~camera_info to pointcloud");
90 if (!pnh.
getParam(
"sync_image", sync_image)) {
94 ROS_INFO(
"~sync_image is specified, synchronize ~camera_info to image");
99 ROS_INFO(
"~sync_image or ~sync_pointcloud are not specified, use static_rate");
101 pnh.
param(
"static_rate", static_rate, 30.0);
117 visualization_msgs::InteractiveMarker int_marker;
119 int_marker.name =
"camera info";
127 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
130 geometry_msgs::PoseStamped new_pose, transformed_pose;
131 new_pose.pose = feedback->pose;
132 new_pose.header = feedback->header;
136 new_pose, transformed_pose);
157 sensor_msgs::CameraInfo camera_info;
158 camera_info.header.stamp =
stamp;
163 camera_info.distortion_model =
165 std::vector<double>
D,
K,
R,
P;
166 boost::array<double, 9ul> Kl, Rl;
167 boost::array<double, 12ul> Pl;
168 D =
camera_info_yaml_[
"distortion_coefficients"][
"data"].as<std::vector<double>>();
170 std::memcpy(&Kl[0], &K[0],
sizeof(
double)*9);
172 std::memcpy(&Rl[0], &R[0],
sizeof(
double)*9);
174 std::memcpy(&Pl[0], &P[0],
sizeof(
double)*12);
182 camera_info.width =
width_;
184 camera_info.D.resize(5, 0);
185 camera_info.K.assign(0.0);
186 camera_info.R.assign(0.0);
187 camera_info.P.assign(0.0);
188 camera_info.K[0] = camera_info.K[4] =
f_;
190 camera_info.K[0] = camera_info.P[0] = camera_info.K[4] = camera_info.P[5] =
f_;
191 camera_info.K[2] = camera_info.P[2] =
width_ / 2.0;
192 camera_info.K[5] = camera_info.P[6] =
height_ / 2.0;
193 camera_info.K[8] = camera_info.P[10] = 1.0;
194 camera_info.R[0] = camera_info.R[4] = camera_info.R[8] = 1.0;
212 const sensor_msgs::PointCloud2::ConstPtr& msg)
218 const sensor_msgs::Image::ConstPtr& msg)
232 int main(
int argc,
char** argv)
234 ros::init(argc, argv,
"camera_info_publisher");
std::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
YAML::Node camera_info_yaml_
std::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
void publish(const boost::shared_ptr< M > &message) const
virtual void configCallback(Config &config, uint32_t level)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual ~CameraInfoPublisher()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
std::shared_ptr< tf::TransformListener > tf_listener_
virtual void initializeInteractiveMarker()
virtual void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
virtual void publishCameraInfo(const ros::Time &stamp)
ROSCPP_DECL void spin(Spinner &spinner)
int main(int argc, char **argv)
geometry_msgs::Pose latest_pose_
ros::Publisher pub_camera_info_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
const std::string PLUMB_BOB
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string parent_frame_id_
ros::Subscriber sub_sync_
bool getParam(const std::string &key, std::string &s) const
void add6DofControl(visualization_msgs::InteractiveMarker &msg, bool fixed=false)
virtual void staticRateCallback(const ros::TimerEvent &event)
virtual void imageCallback(const sensor_msgs::Image::ConstPtr &msg)
jsk_interactive_marker::CameraInfoPublisherConfig Config
std::string yaml_filename_
virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)