camera_info_publisher.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
39 
40 namespace jsk_interactive_marker
41 {
43  {
44  ros::NodeHandle nh, pnh("~");
45 
46  latest_pose_.orientation.w = 1.0;
48  pub_camera_info_ = pnh.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
49  if (!pnh.getParam("yaml_filename", yaml_filename_)) {
50  yaml_filename_ = "";
51  ROS_WARN("~yaml_fliename is not specified, use default camera info parameters");
52  }
53  else {
54  camera_info_yaml_ = YAML::LoadFile(yaml_filename_);
55  }
56 
57  // setup dynamic reconfigure
58  srv_ = std::make_shared <dynamic_reconfigure::Server<Config> > (pnh);
59  dynamic_reconfigure::Server<Config>::CallbackType f =
60  boost::bind (
62  srv_->setCallback (f);
63 
64  // read parameters
65  if (!pnh.getParam("frame_id", frame_id_)) {
66  ROS_WARN("~frame_id is not specified, use camera as frame_id");
67  frame_id_ = "camera";
68  }
69  if (!pnh.getParam("parent_frame_id", parent_frame_id_)) {
70  ROS_WARN("~parent_frame_id is not specified, use base_link as parent_frame_id");
71  parent_frame_id_ = "base_link";
72  }
73 
74  // interactive marker
78  bool sync_pointcloud;
79  bool sync_image;
80 
81  if (!pnh.getParam("sync_pointcloud", sync_pointcloud)) {
82  sync_pointcloud = false;
83  }
84  if (sync_pointcloud) {
85  ROS_INFO("~sync_pointcloud is specified, synchronize ~camera_info to pointcloud");
86  sub_sync_ = pnh.subscribe(
87  "input", 1, &CameraInfoPublisher::pointcloudCallback, this);
88  }
89  else {
90  if (!pnh.getParam("sync_image", sync_image)) {
91  sync_image = false;
92  }
93  if (sync_image) {
94  ROS_INFO("~sync_image is specified, synchronize ~camera_info to image");
95  sub_sync_ = pnh.subscribe(
96  "input", 1, &CameraInfoPublisher::imageCallback, this);
97  }
98  else {
99  ROS_INFO("~sync_image or ~sync_pointcloud are not specified, use static_rate");
100  double static_rate;
101  pnh.param("static_rate", static_rate, 30.0); // defaults to 30 Hz
102  timer_ = nh.createTimer(
103  ros::Duration( 1 / static_rate ),
105  this, _1));
106  }
107  }
108  }
109 
111  {
112 
113  }
114 
116  {
117  visualization_msgs::InteractiveMarker int_marker;
118  int_marker.header.frame_id = parent_frame_id_;
119  int_marker.name = "camera info";
120  im_helpers::add6DofControl(int_marker, false);
121  server_->insert(int_marker,
122  boost::bind(&CameraInfoPublisher::processFeedback, this, _1));
123  server_->applyChanges();
124  }
125 
127  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
128  {
129  boost::mutex::scoped_lock lock(mutex_);
130  geometry_msgs::PoseStamped new_pose, transformed_pose;
131  new_pose.pose = feedback->pose;
132  new_pose.header = feedback->header;
133  try {
134  tf_listener_->transformPose(
136  new_pose, transformed_pose);
137  latest_pose_ = transformed_pose.pose;
138  }
139  catch (...) {
140  ROS_FATAL("tf exception");
141  return;
142  }
143  }
144 
145 
146  void CameraInfoPublisher::configCallback(Config &config, uint32_t level)
147  {
148  boost::mutex::scoped_lock lock(mutex_);
149  width_ = config.width;
150  height_ = config.height;
151  f_ = config.f;
152  }
153 
155  {
156  boost::mutex::scoped_lock lock(mutex_);
157  sensor_msgs::CameraInfo camera_info;
158  camera_info.header.stamp = stamp;
159  camera_info.header.frame_id = frame_id_;
160  if (yaml_filename_ != "") {
161  camera_info.height = camera_info_yaml_["image_height"].as<uint32_t>();
162  camera_info.width = camera_info_yaml_["image_width"].as<uint32_t>();
163  camera_info.distortion_model =
164  camera_info_yaml_["camera_model"].as<std::string>();
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>>();
169  K = camera_info_yaml_["camera_matrix"]["data"].as<std::vector<double>>();
170  std::memcpy(&Kl[0], &K[0], sizeof(double)*9);
171  R = camera_info_yaml_["rectification_matrix"]["data"].as<std::vector<double>>();
172  std::memcpy(&Rl[0], &R[0], sizeof(double)*9);
173  P = camera_info_yaml_["projection_matrix"]["data"].as<std::vector<double>>();
174  std::memcpy(&Pl[0], &P[0], sizeof(double)*12);
175  camera_info.D = D;
176  camera_info.K = Kl;
177  camera_info.R = Rl;
178  camera_info.P = Pl;
179  }
180  else {
181  camera_info.height = height_;
182  camera_info.width = width_;
183  camera_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
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_;
189 
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;
195  }
196  pub_camera_info_.publish(camera_info);
198  tf::Transform transform;
199  transform.setOrigin(tf::Vector3(latest_pose_.position.x,
200  latest_pose_.position.y,
201  latest_pose_.position.z));
202  tf::Quaternion q(latest_pose_.orientation.x,
203  latest_pose_.orientation.y,
204  latest_pose_.orientation.z,
205  latest_pose_.orientation.w);
206  transform.setRotation(q);
207  br.sendTransform(tf::StampedTransform(transform, stamp,
209  }
210 
212  const sensor_msgs::PointCloud2::ConstPtr& msg)
213  {
214  publishCameraInfo(msg->header.stamp);
215  }
216 
218  const sensor_msgs::Image::ConstPtr& msg)
219  {
220  publishCameraInfo(msg->header.stamp);
221  }
222 
224  const ros::TimerEvent& event)
225  {
227  }
228 
229 }
230 
231 
232 int main(int argc, char** argv)
233 {
234  ros::init(argc, argv, "camera_info_publisher");
236  ros::spin();
237  return 0;
238 }
f
std::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
#define ROS_FATAL(...)
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())
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_
#define ROS_WARN(...)
virtual void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
D
virtual void publishCameraInfo(const ros::Time &stamp)
ROSCPP_DECL void spin(Spinner &spinner)
q
int main(int argc, char **argv)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void sendTransform(const StampedTransform &transform)
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)
mutex_t * lock
br
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
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)


jsk_interactive_marker
Author(s): furuta
autogenerated on Sat Mar 20 2021 03:03:33