objectdetection_transform_echo.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 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 #include <string>
37 
41 #include <posedetection_msgs/ObjectDetection.h>
42 #include <ros/ros.h>
43 #include <tf/transform_listener.h>
45 #include <tf2/exceptions.h>
46 
49 std::string frame_id_;
50 
51 void getTransformation(const geometry_msgs::Pose& pose1,
52  const geometry_msgs::Pose& pose2,
53  Eigen::Vector3d& pos,
54  Eigen::Quaterniond& rot)
55 {
56  Eigen::Affine3d affine1, affine2;
57  tf::poseMsgToEigen(pose1, affine1);
58  tf::poseMsgToEigen(pose2, affine2);
59 
60  // compute transformation between two transform...
61  Eigen::Affine3d transform = affine1 * affine2.inverse();
62  pos = Eigen::Vector3d(transform.translation());
63  rot = Eigen::Quaterniond(transform.rotation());
64 }
65 
66 
67 void callback(const posedetection_msgs::ObjectDetection::ConstPtr& detection1,
68  const posedetection_msgs::ObjectDetection::ConstPtr& detection2)
69 {
70  if (detection1->objects.size() > 0 && detection2->objects.size() > 0) {
71  geometry_msgs::Pose pose1 = detection1->objects[0].pose;
72  geometry_msgs::Pose pose2 = detection2->objects[0].pose;
73  Eigen::Vector3d pos;
74  Eigen::Quaterniond rot;
75  getTransformation(pose1, pose2, pos, rot);
76  ROS_INFO("without tf resolving::");
77  ROS_INFO(" pos: [%f %f %f]", pos[0], pos[1], pos[2]);
78  ROS_INFO(" rot: [%f %f %f %f]", rot.x(), rot.y(), rot.z(), rot.w());
79  if (detection1->header.frame_id != detection2->header.frame_id) {
80  geometry_msgs::PoseStamped pose2_stamped, pose2_stamped_transformed;
81  pose2_stamped.header = detection2->header;
82  pose2_stamped.pose = pose2;
83  try {
84  g_listener_->transformPose(detection1->header.frame_id,
85  pose2_stamped,
86  pose2_stamped_transformed);
87  geometry_msgs::Pose pose2_transformed = pose2_stamped_transformed.pose;
88  Eigen::Vector3d pos_transformed;
89  Eigen::Quaterniond rot_transformed;
90  getTransformation(pose1, pose2_transformed, pos_transformed, rot_transformed);
91 
92  geometry_msgs::PoseStamped pose_out;
93  pose_out.header.stamp = detection2->header.stamp;
94  pose_out.header.frame_id = frame_id_;
95  pose_out.pose.position.x = pos_transformed[0];
96  pose_out.pose.position.y = pos_transformed[1];
97  pose_out.pose.position.z = pos_transformed[2];
98  pose_out.pose.orientation.x = rot_transformed.x();
99  pose_out.pose.orientation.y = rot_transformed.y();
100  pose_out.pose.orientation.z = rot_transformed.z();
101  pose_out.pose.orientation.w = rot_transformed.w();
102  pub_pose_.publish(pose_out);
103 
104  ROS_INFO("with tf resolving::");
105  ROS_INFO(" pos: [%f %f %f]", pos_transformed[0], pos_transformed[1], pos_transformed[2]);
106  ROS_INFO(" rot: [%f %f %f %f]", rot_transformed.x(), rot_transformed.y(), rot_transformed.z(), rot_transformed.w());
107  }
108  catch(tf2::ConnectivityException &e) {
109  ROS_ERROR("failed to resolve tf: %s", e.what());
110  }
112  ROS_ERROR("failed to resolve tf: %s", e.what());
113  }
114  catch(tf2::TransformException &e) {
115  ROS_ERROR("failed to resolve tf: %s", e.what());
116  }
117  }
118 
119  }
120 }
121 
122 
123 int main(int argc, char** argv)
124 {
125  ros::init(argc, argv, "objectdetection_transform_echo");
126  g_listener_ = new tf::TransformListener();
128  posedetection_msgs::ObjectDetection, posedetection_msgs::ObjectDetection>
129  SyncPolicy;
130 
131  ros::NodeHandle nh;
132 
133  nh.param<std::string>("frame_id", frame_id_, "");
134 
135  pub_pose_ = nh.advertise<geometry_msgs::PoseStamped>("pose", 1);
136 
138  detection1_sub(nh, "detection1", 1);
140  detection2_sub(nh, "detection2", 1);
141 
143  SyncPolicy(1000),
144  detection1_sub, detection2_sub);
145  sync.registerCallback(boost::bind(&callback, _1, _2));
146  ros::spin();
147  return 0;
148 }
pos
void publish(const boost::shared_ptr< M > &message) const
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void callback(const posedetection_msgs::ObjectDetection::ConstPtr &detection1, const posedetection_msgs::ObjectDetection::ConstPtr &detection2)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
Connection registerCallback(C &callback)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher pub_pose_
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
std::string frame_id_
tf::TransformListener * g_listener_
void getTransformation(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, Eigen::Vector3d &pos, Eigen::Quaterniond &rot)
#define ROS_ERROR(...)
int main(int argc, char **argv)


checkerboard_detector
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Mon May 3 2021 03:03:00