static_transform_pose_stamped.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 <ros/ros.h>
37 #include <geometry_msgs/PoseStamped.h>
39 
40 namespace jsk_topic_tools
41 {
43  {
44  public:
46  PoseStampedTransformer(double x, double y, double z,
47  double yaw, double pitch, double roll,
48  std::string from_topic,
49  std::string to_topic);
50  virtual ~PoseStampedTransformer();
51  protected:
52  void transform(const geometry_msgs::PoseStamped::ConstPtr& pose_msg);
55  double x_;
56  double y_;
57  double z_;
58  double roll_;
59  double pitch_;
60  double yaw_;
61  private:
62 
63  };
64 
66  double x, double y, double z,
67  double yaw, double pitch, double roll,
68  std::string from_topic, std::string to_topic):
69  x_(x), y_(y), z_(z), yaw_(yaw), pitch_(pitch), roll_(roll)
70  {
71  ros::NodeHandle nh;
72  pub_ = nh.advertise<geometry_msgs::PoseStamped>(
73  to_topic, 1);
74  sub_ = nh.subscribe(from_topic, 1,
76  }
77 
79  {
80 
81  }
82 
84  const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
85  {
86  Eigen::Affine3d input_transform;
87  tf::poseMsgToEigen(pose_msg->pose, input_transform);
88  Eigen::AngleAxisd roll_angle(roll_, Eigen::Vector3d::UnitZ());
89  Eigen::AngleAxisd yaw_angle(yaw_, Eigen::Vector3d::UnitY());
90  Eigen::AngleAxisd pitch_angle(pitch_, Eigen::Vector3d::UnitX());
91 
92  Eigen::Affine3d transformation
93  = Eigen::Translation3d(x_, y_, z_) * roll_angle * yaw_angle * pitch_angle;
94  Eigen::Affine3d output_transform = input_transform * transformation;
95  geometry_msgs::PoseStamped output_pose_msg;
96  tf::poseEigenToMsg(output_transform, output_pose_msg.pose);
97  output_pose_msg.header = pose_msg->header;
98  pub_.publish(output_pose_msg);
99  }
100 
101 }
102 
103 int usage(char** argv)
104 {
105  std::cerr << "Usage:: " << argv[0]
106  << " x y z yaw pitch roll from_topic to_topic"
107  << std::endl;
108 }
109 
110 int main(int argc, char** argv)
111 {
112  ros::init(argc, argv, "static_transform_pose_stamped",
114  // x y z yaw pitch roll
115  if (argc != 9) {
116  usage(argv);
117  exit(1);
118  }
119  // parse argument
121  ::atof(argv[2]),
122  ::atof(argv[3]),
123  ::atof(argv[4]),
124  ::atof(argv[5]),
125  ::atof(argv[6]),
126  argv[7],
127  argv[8]);
128  ros::spin();
129 }
void publish(const boost::shared_ptr< M > &message) const
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int usage(char **argv)
ROSCPP_DECL void spin(Spinner &spinner)
PoseStampedTransformer(double x, double y, double z, double yaw, double pitch, double roll, std::string from_topic, std::string to_topic)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void transform(const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
boost::shared_ptr< PoseStampedTransformer > Ptr
int main(int argc, char **argv)


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Tue Feb 6 2018 03:45:19