fake_localization.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
73 #include <ros/ros.h>
74 #include <ros/time.h>
75 
76 #include <nav_msgs/Odometry.h>
77 #include <geometry_msgs/PoseArray.h>
78 #include <geometry_msgs/PoseWithCovarianceStamped.h>
79 
80 #include <angles/angles.h>
81 
82 #include "ros/console.h"
83 
85 #include <tf2/convert.h>
87 #include <tf2_ros/buffer.h>
90 #include <tf2_ros/message_filter.h>
92 
94 {
95  public:
97  {
98  m_posePub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",1,true);
99  m_particlecloudPub = m_nh.advertise<geometry_msgs::PoseArray>("particlecloud",1,true);
101  m_tfBuffer = new tf2_ros::Buffer();
103 
104  m_base_pos_received = false;
105 
106  ros::NodeHandle private_nh("~");
107  private_nh.param("odom_frame_id", odom_frame_id_, std::string("odom"));
108  private_nh.param("base_frame_id", base_frame_id_, std::string("base_link"));
109  private_nh.param("global_frame_id", global_frame_id_, std::string("/map"));
110  private_nh.param("delta_x", delta_x_, 0.0);
111  private_nh.param("delta_y", delta_y_, 0.0);
112  private_nh.param("delta_yaw", delta_yaw_, 0.0);
113  private_nh.param("transform_tolerance", transform_tolerance_, 0.1);
114  m_particleCloud.header.stamp = ros::Time::now();
115  m_particleCloud.header.frame_id = global_frame_id_;
116  m_particleCloud.poses.resize(1);
117  ros::NodeHandle nh;
118 
119  tf2::Quaternion q;
120  q.setRPY(0.0, 0.0, -delta_yaw_);
121  m_offsetTf = tf2::Transform(q, tf2::Vector3(-delta_x_, -delta_y_, 0.0));
122 
123  stuff_sub_ = nh.subscribe("base_pose_ground_truth", 100, &FakeOdomNode::stuffFilter, this);
126  filter_->registerCallback(boost::bind(&FakeOdomNode::update, this, _1));
127 
128  // subscription to "2D Pose Estimate" from RViz:
131  m_initPoseFilter->registerCallback(boost::bind(&FakeOdomNode::initPoseReceived, this, _1));
132  }
133 
135  {
136  if (m_tfServer)
137  delete m_tfServer;
138  if (m_tfListener)
139  delete m_tfListener;
140  if (m_tfBuffer)
141  delete m_tfBuffer;
142  }
143 
144 
145  private:
157 
161 
162  nav_msgs::Odometry m_basePosMsg;
163  geometry_msgs::PoseArray m_particleCloud;
164  geometry_msgs::PoseWithCovarianceStamped m_currentPos;
166 
167  //parameter for what odom to use
168  std::string odom_frame_id_;
169  std::string base_frame_id_;
170  std::string global_frame_id_;
171 
172  public:
173  void stuffFilter(const nav_msgs::OdometryConstPtr& odom_msg){
174  //we have to do this to force the message filter to wait for transforms
175  //from odom_frame_id_ to base_frame_id_ to be available at time odom_msg.header.stamp
176  //really, the base_pose_ground_truth should come in with no frame_id b/c it doesn't make sense
177  boost::shared_ptr<nav_msgs::Odometry> stuff_msg(new nav_msgs::Odometry);
178  *stuff_msg = *odom_msg;
179  stuff_msg->header.frame_id = odom_frame_id_;
180  filter_->add(stuff_msg);
181  }
182 
183  void update(const nav_msgs::OdometryConstPtr& message){
184  tf2::Transform txi;
185  tf2::convert(message->pose.pose, txi);
186  txi = m_offsetTf * txi;
187 
188  geometry_msgs::TransformStamped odom_to_map;
189  try
190  {
191  geometry_msgs::TransformStamped txi_inv;
192  txi_inv.header.frame_id = base_frame_id_;
193  txi_inv.header.stamp = message->header.stamp;
194  tf2::convert(txi.inverse(), txi_inv.transform);
195 
196  m_tfBuffer->transform(txi_inv, odom_to_map, odom_frame_id_);
197  }
198  catch(tf2::TransformException &e)
199  {
200  ROS_ERROR("Failed to transform to %s from %s: %s\n", odom_frame_id_.c_str(), base_frame_id_.c_str(), e.what());
201  return;
202  }
203 
204  geometry_msgs::TransformStamped trans;
205  trans.header.stamp = message->header.stamp + ros::Duration(transform_tolerance_);
206  trans.header.frame_id = global_frame_id_;
207  trans.child_frame_id = message->header.frame_id;
208  tf2::Transform odom_to_map_tf2;
209  tf2::convert(odom_to_map.transform, odom_to_map_tf2);
210  tf2::Transform odom_to_map_inv = odom_to_map_tf2.inverse();
211  tf2::convert(odom_to_map_inv, trans.transform);
212  m_tfServer->sendTransform(trans);
213 
214  tf2::Transform current;
215  tf2::convert(message->pose.pose, current);
216 
217  //also apply the offset to the pose
218  current = m_offsetTf * current;
219 
220  geometry_msgs::Transform current_msg;
221  tf2::convert(current, current_msg);
222 
223  // Publish localized pose
224  m_currentPos.header = message->header;
225  m_currentPos.header.frame_id = global_frame_id_;
226  tf2::convert(current_msg.rotation, m_currentPos.pose.pose.orientation);
227  m_currentPos.pose.pose.position.x = current_msg.translation.x;
228  m_currentPos.pose.pose.position.y = current_msg.translation.y;
229  m_currentPos.pose.pose.position.z = current_msg.translation.z;
230  m_posePub.publish(m_currentPos);
231 
232  // The particle cloud is the current position. Quite convenient.
233  m_particleCloud.header = m_currentPos.header;
234  m_particleCloud.poses[0] = m_currentPos.pose.pose;
235  m_particlecloudPub.publish(m_particleCloud);
236  }
237 
238  void initPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){
239  tf2::Transform pose;
240  tf2::convert(msg->pose.pose, pose);
241 
242  if (msg->header.frame_id != global_frame_id_){
243  ROS_WARN("Frame ID of \"initialpose\" (%s) is different from the global frame %s", msg->header.frame_id.c_str(), global_frame_id_.c_str());
244  }
245 
246  // set offset so that current pose is set to "initialpose"
247  geometry_msgs::TransformStamped baseInMap;
248  try{
249  // just get the latest
250  baseInMap = m_tfBuffer->lookupTransform(base_frame_id_, global_frame_id_, ros::Time(0));
251  } catch(tf2::TransformException){
252  ROS_WARN("Failed to lookup transform!");
253  return;
254  }
255 
256  tf2::Transform baseInMapTf2;
257  tf2::convert(baseInMap.transform, baseInMapTf2);
258  tf2::Transform delta = pose * baseInMapTf2;
259  m_offsetTf = delta * m_offsetTf;
260 
261  }
262 };
263 
264 int main(int argc, char** argv)
265 {
266  ros::init(argc, argv, "fake_localization");
267 
268  FakeOdomNode odom;
269 
270  ros::spin();
271 
272  return 0;
273 }
std::string base_frame_id_
tf2_ros::MessageFilter< nav_msgs::Odometry > * filter_
tf2_ros::TransformBroadcaster * m_tfServer
ros::Publisher m_particlecloudPub
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
std::string odom_frame_id_
tf2_ros::MessageFilter< geometry_msgs::PoseWithCovarianceStamped > * m_initPoseFilter
void publish(const boost::shared_ptr< M > &message) const
tf2::Transform m_offsetTf
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
nav_msgs::Odometry m_basePosMsg
geometry_msgs::PoseWithCovarianceStamped m_currentPos
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
geometry_msgs::PoseArray m_particleCloud
ros::Subscriber stuff_sub_
#define ROS_WARN(...)
ros::Publisher m_posePub
ROSCPP_DECL void spin(Spinner &spinner)
void add(const MEvent &evt)
tf2_ros::Buffer * m_tfBuffer
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int main(int argc, char **argv)
Transform inverse() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void sendTransform(const geometry_msgs::TransformStamped &transform)
tf2_ros::TransformListener * m_tfListener
void initPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
void update(const nav_msgs::OdometryConstPtr &message)
static Time now()
void convert(const A &a, B &b)
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
std::string global_frame_id_
message_filters::Subscriber< nav_msgs::Odometry > * filter_sub_
void stuffFilter(const nav_msgs::OdometryConstPtr &odom_msg)
ros::NodeHandle m_nh
#define ROS_ERROR(...)
message_filters::Subscriber< geometry_msgs::PoseWithCovarianceStamped > * m_initPoseSub
Connection registerCallback(const C &callback)


fake_localization
Author(s): Ioan A. Sucan, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:34