interactive_cuboid_likelihood_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, 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/or other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab 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 
36 #define BOOST_PARAMETER_MAX_ARITY 7
37 #include "jsk_pcl_ros/interactive_cuboid_likelihood.h"
38 #include "jsk_recognition_utils/pcl_conversion_util.h"
39 #include <jsk_topic_tools/rosparam_utils.h>
40 
41 namespace jsk_pcl_ros
42 {
43  void InteractiveCuboidLikelihood::onInit()
44  {
45  DiagnosticNodelet::onInit();
46  tf_ = TfListenerSingleton::getInstance();
47  pub_ = pnh_->advertise<std_msgs::Float32>("output", 1);
48  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
49  pnh_->param("frame_id", frame_id_, std::string("odom"));
50  pnh_->param("sensor_frame", sensor_frame_, std::string("odom"));
51 
52  std::vector<double> initial_pos;
53  std::vector<double> initial_rot;
54  if (!jsk_topic_tools::readVectorParameter(*pnh_, "initial_pos", initial_pos)) {
55  initial_pos.push_back(0);
56  initial_pos.push_back(0);
57  initial_pos.push_back(0);
58  }
59  if (!jsk_topic_tools::readVectorParameter(*pnh_, "initial_rot", initial_rot)) {
60  initial_rot.push_back(0);
61  initial_rot.push_back(0);
62  initial_rot.push_back(0);
63  }
64  particle_.x = initial_pos[0];
65  particle_.y = initial_pos[1];
66  particle_.z = initial_pos[2];
67  particle_.roll = initial_rot[0];
68  particle_.pitch = initial_rot[1];
69  particle_.yaw = initial_rot[2];
70  typename dynamic_reconfigure::Server<Config>::CallbackType f =
71  boost::bind (&InteractiveCuboidLikelihood::configCallback, this, _1, _2);
72  srv_->setCallback (f);
73  sub_ = pnh_->subscribe("input", 1, &InteractiveCuboidLikelihood::likelihood, this);
74  // Cuboid
75  server_.reset(new interactive_markers::InteractiveMarkerServer(getName()));
76  visualization_msgs::InteractiveMarker int_marker = particleToInteractiveMarker(particle_);
77  server_->insert(int_marker, boost::bind(&InteractiveCuboidLikelihood::processFeedback, this, _1));
78  server_->applyChanges();
79 
80  // SupportPlane
81  plane_server_.reset(new interactive_markers::InteractiveMarkerServer(getName() + "_plane"));
82  plane_pose_ = Eigen::Affine3f::Identity();
83  visualization_msgs::InteractiveMarker plane_int_marker = planeInteractiveMarker();
84  plane_server_->insert(plane_int_marker, boost::bind(&InteractiveCuboidLikelihood::processPlaneFeedback, this, _1));
85  plane_server_->applyChanges();
86  onInitPostProcess();
87  }
88 
89  void InteractiveCuboidLikelihood::subscribe()
90  {
91 
92  }
93 
94  void InteractiveCuboidLikelihood::unsubscribe()
95  {
96 
97  }
98 
99  void InteractiveCuboidLikelihood::processFeedback(
100  const visualization_msgs::InteractiveMarkerFeedback::ConstPtr& feedback)
101  {
102  boost::mutex::scoped_lock lock(mutex_);
103  Eigen::Affine3f pose;
104  tf::poseMsgToEigen(feedback->pose, pose);
105  particle_.fromEigen(pose);
106  }
107 
108  void InteractiveCuboidLikelihood::processPlaneFeedback(
109  const visualization_msgs::InteractiveMarkerFeedback::ConstPtr& feedback)
110  {
111  boost::mutex::scoped_lock lock(mutex_);
112  tf::poseMsgToEigen(feedback->pose, plane_pose_);
113  }
114 
115  void InteractiveCuboidLikelihood::likelihood(
116  const sensor_msgs::PointCloud2::ConstPtr& msg)
117  {
118  boost::mutex::scoped_lock lock(mutex_);
119  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
120  pcl::fromROSMsg(*msg, *cloud);
121  tf::StampedTransform transform
122  = lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
123  ros::Time(0.0),
124  ros::Duration(0.0));
125  Eigen::Vector3f vp;
126  tf::vectorTFToEigen(transform.getOrigin(), vp);
127  jsk_recognition_utils::Vertices vertices;
128  vertices.push_back(Eigen::Vector3f(plane_pose_ * (Eigen::Vector3f::UnitX() + Eigen::Vector3f::UnitY())));
129  vertices.push_back(Eigen::Vector3f(plane_pose_ * (- Eigen::Vector3f::UnitX() + Eigen::Vector3f::UnitY())));
130  vertices.push_back(Eigen::Vector3f(plane_pose_ * (- Eigen::Vector3f::UnitX() - Eigen::Vector3f::UnitY())));
131  vertices.push_back(Eigen::Vector3f(plane_pose_ * (Eigen::Vector3f::UnitX() - Eigen::Vector3f::UnitY())));
132  Polygon::Ptr plane(new Polygon(vertices));
133  //particle_.plane = plane;
134  particle_.plane_index = 0;
135  std::vector<Polygon::Ptr> polygons;
136  polygons.push_back(plane);
137  // for (size_t i = 0; i < vertices.size(); i++) {
138  // ROS_INFO("v: [%f, %f, %f]", vertices[i][0], vertices[i][1], vertices[i][2]);
139  // }
140  pcl::KdTreeFLANN<pcl::PointXYZ> tree;
141  tree.setInputCloud(cloud);
142  std::vector<float> polygon_likelihood(1, 1.0);
143  double l = computeLikelihood(particle_, cloud, tree, vp, polygons, polygon_likelihood, config_);
144  NODELET_INFO("likelihood: %f", l);
145  std_msgs::Float32 float_msg;
146  float_msg.data = l;
147  pub_.publish(float_msg);
148  }
149 
150  void InteractiveCuboidLikelihood::configCallback(
151  Config& config, uint32_t level)
152  {
153  boost::mutex::scoped_lock lock(mutex_);
154  config_ = config;
155  particle_.dx = config_.dx;
156  particle_.dy = config_.dy;
157  particle_.dz = config_.dz;
158  if (server_) {
159  visualization_msgs::InteractiveMarker int_marker = particleToInteractiveMarker(particle_);
160  server_->insert(int_marker, boost::bind(&InteractiveCuboidLikelihood::processFeedback, this, _1));
161  server_->applyChanges();
162  }
163  }
164 
165  visualization_msgs::InteractiveMarker
166  InteractiveCuboidLikelihood::planeInteractiveMarker()
167  {
168  visualization_msgs::InteractiveMarker int_marker;
169  int_marker.header.frame_id = frame_id_;
170  int_marker.header.stamp = ros::Time::now();
171  int_marker.name = getName() + "_plane";
172  visualization_msgs::InteractiveMarkerControl control;
173  control.orientation.w = 1;
174  control.orientation.x = 1;
175  control.orientation.y = 0;
176  control.orientation.z = 0;
177  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
178  int_marker.controls.push_back(control);
179  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
180  int_marker.controls.push_back(control);
181 
182  control.orientation.w = 1;
183  control.orientation.x = 0;
184  control.orientation.y = 1;
185  control.orientation.z = 0;
186  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
187  int_marker.controls.push_back(control);
188  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
189  int_marker.controls.push_back(control);
190 
191  control.orientation.w = 1;
192  control.orientation.x = 0;
193  control.orientation.y = 0;
194  control.orientation.z = 1;
195  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
196  int_marker.controls.push_back(control);
197  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
198  int_marker.controls.push_back(control);
199  visualization_msgs::InteractiveMarkerControl box_control;
200  box_control.always_visible = true;
201  visualization_msgs::Marker plane_marker;
202  plane_marker.type = visualization_msgs::Marker::CUBE;
203  plane_marker.scale.x = 1.0;
204  plane_marker.scale.y = 1.0;
205  plane_marker.scale.z = 0.01;
206  plane_marker.color.r = 1.0;
207  plane_marker.color.a = 1.0;
208  plane_marker.pose.orientation.w = 1.0;
209  box_control.markers.push_back(plane_marker);
210  int_marker.controls.push_back(box_control);
211  return int_marker;
212  }
213 
214  visualization_msgs::Marker
215  InteractiveCuboidLikelihood::particleToMarker(
216  const Particle& p)
217  {
218  visualization_msgs::Marker marker;
219  marker.type = visualization_msgs::Marker::CUBE;
220  marker.scale.x = p.dx;
221  marker.scale.y = p.dy;
222  marker.scale.z = p.dz;
223  marker.pose.orientation.w = 1.0;
224  marker.color.r = 0.5;
225  marker.color.g = 0.5;
226  marker.color.b = 0.5;
227  marker.color.a = 1.0;
228  return marker;
229  }
230 
231  visualization_msgs::InteractiveMarker InteractiveCuboidLikelihood::particleToInteractiveMarker(const Particle& p)
232  {
233  visualization_msgs::InteractiveMarker int_marker;
234  int_marker.header.frame_id = frame_id_;
235  int_marker.header.stamp = ros::Time::now();
236  int_marker.name = getName();
237  Eigen::Affine3f pose = particle_.toEigenMatrix();
238  tf::poseEigenToMsg(pose, int_marker.pose);
239 
240  visualization_msgs::InteractiveMarkerControl control;
241  control.orientation.w = 1;
242  control.orientation.x = 1;
243  control.orientation.y = 0;
244  control.orientation.z = 0;
245  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
246  int_marker.controls.push_back(control);
247  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
248  int_marker.controls.push_back(control);
249 
250  control.orientation.w = 1;
251  control.orientation.x = 0;
252  control.orientation.y = 1;
253  control.orientation.z = 0;
254  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
255  int_marker.controls.push_back(control);
256  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
257  int_marker.controls.push_back(control);
258 
259  control.orientation.w = 1;
260  control.orientation.x = 0;
261  control.orientation.y = 0;
262  control.orientation.z = 1;
263  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
264  int_marker.controls.push_back(control);
265  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
266  int_marker.controls.push_back(control);
267  visualization_msgs::InteractiveMarkerControl box_control;
268  box_control.always_visible = true;
269  box_control.markers.push_back(particleToMarker(particle_));
270  int_marker.controls.push_back(box_control);
271  return int_marker;
272  }
273 
274 
275 
276 }
277 
278 #include <pluginlib/class_list_macros.h>
279 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::InteractiveCuboidLikelihood, nodelet::Nodelet);
jsk_pcl_ros::InteractiveCuboidLikelihood::processPlaneFeedback
virtual void processPlaneFeedback(const visualization_msgs::InteractiveMarkerFeedback::ConstPtr &feedback)
Definition: interactive_cuboid_likelihood_nodelet.cpp:108
tf::poseMsgToEigen
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
polygons
polygons
interactive_cuboid_likelihood.h
jsk_pcl_ros::InteractiveCuboidLikelihood::unsubscribe
virtual void unsubscribe()
Definition: interactive_cuboid_likelihood_nodelet.cpp:94
pcl::tracking::_ParticleCuboid::pitch
float pitch
Definition: particle_cuboid.h:151
depth_error_calibration.lock
lock
Definition: depth_error_calibration.py:32
l
long l
_2
boost::arg< 2 > _2
marker
marker
boost::shared_ptr
jsk_pcl_ros::InteractiveCuboidLikelihood::mutex_
boost::mutex mutex_
Definition: interactive_cuboid_likelihood.h:132
jsk_pcl_ros::InteractiveCuboidLikelihood::likelihood
virtual void likelihood(const sensor_msgs::PointCloud2::ConstPtr &msg)
Definition: interactive_cuboid_likelihood_nodelet.cpp:115
pcl::tracking::_ParticleCuboid::dy
float dy
Definition: particle_cuboid.h:154
p
p
pcl::tracking::_ParticleCuboid::roll
float roll
Definition: particle_cuboid.h:150
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
tf::vectorTFToEigen
void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3f &e)
jsk_pcl_ros::InteractiveCuboidLikelihood::configCallback
virtual void configCallback(Config &config, uint32_t level)
Definition: interactive_cuboid_likelihood_nodelet.cpp:150
jsk_recognition_utils::Polygon
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
tf::StampedTransform
jsk_pcl_ros::InteractiveCuboidLikelihood::particle_
Particle particle_
Definition: interactive_cuboid_likelihood.h:137
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
pcl::tracking::_ParticleCuboid::yaw
float yaw
Definition: particle_cuboid.h:152
pcl::tracking::_ParticleCuboid::dx
float dx
Definition: particle_cuboid.h:153
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
cloud
cloud
jsk_pcl_ros::InteractiveCuboidLikelihood::plane_server_
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > plane_server_
Definition: interactive_cuboid_likelihood.h:143
jsk_pcl_ros::InteractiveCuboidLikelihood::sensor_frame_
std::string sensor_frame_
Definition: interactive_cuboid_likelihood.h:141
pcl::tracking::ParticleCuboid::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: particle_cuboid.h:167
pose
pose
class_list_macros.h
jsk_pcl_ros::InteractiveCuboidLikelihood::processFeedback
virtual void processFeedback(const visualization_msgs::InteractiveMarkerFeedback::ConstPtr &feedback)
Definition: interactive_cuboid_likelihood_nodelet.cpp:99
tree
tree
pcl::tracking::ParticleCuboid
Definition: particle_cuboid.h:134
jsk_pcl_ros
Definition: add_color_from_image.h:51
jsk_pcl_ros::InteractiveCuboidLikelihood::srv_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
Definition: interactive_cuboid_likelihood.h:144
jsk_pcl_ros::InteractiveCuboidLikelihood::pub_
ros::Publisher pub_
Definition: interactive_cuboid_likelihood.h:133
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::InteractiveCuboidLikelihood, nodelet::Nodelet)
_1
boost::arg< 1 > _1
jsk_pcl_ros::InteractiveCuboidLikelihood::server_
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
Definition: interactive_cuboid_likelihood.h:142
tf::poseEigenToMsg
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
jsk_pcl_ros::computeLikelihood
double computeLikelihood(const pcl::tracking::ParticleCuboid &p, pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud, pcl::KdTreeFLANN< pcl::PointXYZ > &tree, const Eigen::Vector3f &viewpoint, const std::vector< Polygon::Ptr > &polygons, const std::vector< float > &polygon_likelihood, const Config &config)
Definition: plane_supported_cuboid_estimator.h:276
jsk_pcl_ros::InteractiveCuboidLikelihood::subscribe
virtual void subscribe()
Definition: interactive_cuboid_likelihood_nodelet.cpp:89
pcl_conversion_util.h
interactive_markers::InteractiveMarkerServer
jsk_pcl_ros::InteractiveCuboidLikelihood::onInit
virtual void onInit()
Definition: interactive_cuboid_likelihood_nodelet.cpp:43
NODELET_INFO
#define NODELET_INFO(...)
pcl::tracking::_ParticleCuboid::dz
float dz
Definition: particle_cuboid.h:155
jsk_pcl_ros::InteractiveCuboidLikelihood::particleToMarker
virtual visualization_msgs::Marker particleToMarker(const Particle &p)
Definition: interactive_cuboid_likelihood_nodelet.cpp:215
jsk_pcl_ros::InteractiveCuboidLikelihood::Config
InteractiveCuboidLikelihoodConfig Config
Definition: interactive_cuboid_likelihood.h:117
jsk_pcl_ros::InteractiveCuboidLikelihood::plane_pose_
Eigen::Affine3f plane_pose_
Definition: interactive_cuboid_likelihood.h:135
nodelet::Nodelet
ros::Time
pcl::tracking::_ParticleCuboid::plane_index
int plane_index
Definition: particle_cuboid.h:163
jsk_pcl_ros::InteractiveCuboidLikelihood
Definition: interactive_cuboid_likelihood.h:81
pcl::tracking::ParticleCuboid::fromEigen
void fromEigen(const Eigen::Affine3f &pose)
Definition: particle_cuboid.h:172
dump_depth_error.f
f
Definition: dump_depth_error.py:39
jsk_pcl_ros::InteractiveCuboidLikelihood::tf_
tf::TransformListener * tf_
Definition: interactive_cuboid_likelihood.h:139
jsk_pcl_ros::InteractiveCuboidLikelihood::sub_
ros::Subscriber sub_
Definition: interactive_cuboid_likelihood.h:134
jsk_pcl_ros::InteractiveCuboidLikelihood::planeInteractiveMarker
virtual visualization_msgs::InteractiveMarker planeInteractiveMarker()
Definition: interactive_cuboid_likelihood_nodelet.cpp:166
jsk_recognition_utils::TfListenerSingleton::getInstance
static tf::TransformListener * getInstance()
jsk_pcl_ros::InteractiveCuboidLikelihood::frame_id_
std::string frame_id_
Definition: interactive_cuboid_likelihood.h:136
lookupTransformWithDuration
tf::StampedTransform lookupTransformWithDuration(tf::TransformListener *listener, const std::string &to_frame, const std::string &from_frame, const ros::Time &stamp, ros::Duration duration)
config
config
ros::Duration
pose_with_covariance_sample.msg
msg
Definition: pose_with_covariance_sample.py:22
jsk_pcl_ros::InteractiveCuboidLikelihood::particleToInteractiveMarker
virtual visualization_msgs::InteractiveMarker particleToInteractiveMarker(const Particle &p)
Definition: interactive_cuboid_likelihood_nodelet.cpp:231
jsk_pcl_ros::InteractiveCuboidLikelihood::config_
Config config_
Definition: interactive_cuboid_likelihood.h:138
ros::Time::now
static Time now()
jsk_recognition_utils::Vertices
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices

jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jan 7 2025 04:05:45