fake_localization.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00073 #include <ros/ros.h>
00074 #include <ros/time.h>
00075 
00076 #include <nav_msgs/Odometry.h>
00077 #include <geometry_msgs/PoseArray.h>
00078 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00079 
00080 #include <angles/angles.h>
00081 
00082 #include "ros/console.h"
00083 
00084 #include "tf/transform_broadcaster.h"
00085 #include "tf/transform_listener.h"
00086 #include "tf/message_filter.h"
00087 #include "message_filters/subscriber.h"
00088 
00089 
00090 class FakeOdomNode
00091 {
00092   public:
00093     FakeOdomNode(void)
00094     {
00095       m_posePub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",1,true);
00096       m_particlecloudPub = m_nh.advertise<geometry_msgs::PoseArray>("particlecloud",1,true);
00097       m_tfServer = new tf::TransformBroadcaster();      
00098       m_tfListener = new tf::TransformListener();
00099 
00100       m_base_pos_received = false;
00101 
00102       ros::NodeHandle private_nh("~");
00103       private_nh.param("odom_frame_id", odom_frame_id_, std::string("odom"));
00104       private_nh.param("base_frame_id", base_frame_id_, std::string("base_link")); 
00105       private_nh.param("global_frame_id", global_frame_id_, std::string("/map"));
00106       private_nh.param("delta_x", delta_x_, 0.0);
00107       private_nh.param("delta_y", delta_y_, 0.0);
00108       private_nh.param("delta_yaw", delta_yaw_, 0.0);      
00109       private_nh.param("transform_tolerance", transform_tolerance_, 0.1);      
00110       m_particleCloud.header.stamp = ros::Time::now();
00111       m_particleCloud.header.frame_id = global_frame_id_;
00112       m_particleCloud.poses.resize(1);
00113       ros::NodeHandle nh;
00114 
00115       m_offsetTf = tf::Transform(tf::createQuaternionFromRPY(0, 0, -delta_yaw_ ), tf::Point(-delta_x_, -delta_y_, 0.0));
00116 
00117       stuff_sub_ = nh.subscribe("base_pose_ground_truth", 100, &FakeOdomNode::stuffFilter, this);
00118       filter_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh, "", 100);
00119       filter_ = new tf::MessageFilter<nav_msgs::Odometry>(*filter_sub_, *m_tfListener, base_frame_id_, 100);
00120       filter_->registerCallback(boost::bind(&FakeOdomNode::update, this, _1));
00121 
00122       // subscription to "2D Pose Estimate" from RViz:
00123       m_initPoseSub = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(nh, "initialpose", 1);
00124       m_initPoseFilter = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*m_initPoseSub, *m_tfListener, global_frame_id_, 1);
00125       m_initPoseFilter->registerCallback(boost::bind(&FakeOdomNode::initPoseReceived, this, _1));
00126     }
00127 
00128     ~FakeOdomNode(void)
00129     {
00130       if (m_tfServer)
00131         delete m_tfServer; 
00132     }
00133 
00134 
00135   private:
00136     ros::NodeHandle m_nh;
00137     ros::Publisher m_posePub;
00138     ros::Publisher m_particlecloudPub;
00139     message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseSub;
00140     tf::TransformBroadcaster       *m_tfServer;
00141     tf::TransformListener          *m_tfListener;
00142     tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseFilter;
00143     tf::MessageFilter<nav_msgs::Odometry>* filter_;
00144     ros::Subscriber stuff_sub_; 
00145     message_filters::Subscriber<nav_msgs::Odometry>* filter_sub_;
00146 
00147     double                         delta_x_, delta_y_, delta_yaw_;
00148     bool                           m_base_pos_received;
00149     double transform_tolerance_;
00150 
00151     nav_msgs::Odometry  m_basePosMsg;
00152     geometry_msgs::PoseArray      m_particleCloud;
00153     geometry_msgs::PoseWithCovarianceStamped      m_currentPos;
00154     tf::Transform m_offsetTf;
00155 
00156     //parameter for what odom to use
00157     std::string odom_frame_id_;
00158     std::string base_frame_id_;
00159     std::string global_frame_id_;
00160 
00161   public:
00162     void stuffFilter(const nav_msgs::OdometryConstPtr& odom_msg){
00163       //we have to do this to force the message filter to wait for transforms
00164       //from odom_frame_id_ to base_frame_id_ to be available at time odom_msg.header.stamp
00165       //really, the base_pose_ground_truth should come in with no frame_id b/c it doesn't make sense
00166       boost::shared_ptr<nav_msgs::Odometry> stuff_msg(new nav_msgs::Odometry);
00167       *stuff_msg = *odom_msg;
00168       stuff_msg->header.frame_id = odom_frame_id_;
00169       filter_->add(stuff_msg);
00170     }
00171 
00172     void update(const nav_msgs::OdometryConstPtr& message){
00173       tf::Pose txi;
00174       tf::poseMsgToTF(message->pose.pose, txi);
00175       txi = m_offsetTf * txi;
00176 
00177       tf::Stamped<tf::Pose> odom_to_map;
00178       try
00179       {
00180         m_tfListener->transformPose(odom_frame_id_, tf::Stamped<tf::Pose>(txi.inverse(), message->header.stamp, base_frame_id_), odom_to_map);
00181       }
00182       catch(tf::TransformException &e)
00183       {
00184         ROS_ERROR("Failed to transform to %s from %s: %s\n", odom_frame_id_.c_str(), base_frame_id_.c_str(), e.what());
00185         return;
00186       }
00187       m_tfServer->sendTransform(tf::StampedTransform(odom_to_map.inverse(),
00188                                                      message->header.stamp + ros::Duration(transform_tolerance_),
00189                                                      global_frame_id_, message->header.frame_id));
00190 
00191       tf::Pose current;
00192       tf::poseMsgToTF(message->pose.pose, current);
00193 
00194       //also apply the offset to the pose
00195       current = m_offsetTf * current;
00196 
00197       geometry_msgs::Pose current_msg;
00198       tf::poseTFToMsg(current, current_msg);
00199 
00200       // Publish localized pose
00201       m_currentPos.header = message->header;
00202       m_currentPos.header.frame_id = global_frame_id_;
00203       m_currentPos.pose.pose = current_msg;
00204       m_posePub.publish(m_currentPos);
00205 
00206       // The particle cloud is the current position. Quite convenient.
00207       m_particleCloud.header = m_currentPos.header;
00208       m_particleCloud.poses[0] = m_currentPos.pose.pose;
00209       m_particlecloudPub.publish(m_particleCloud);
00210     }
00211 
00212     void initPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){
00213       tf::Pose pose;
00214       tf::poseMsgToTF(msg->pose.pose, pose);
00215 
00216       if (msg->header.frame_id != global_frame_id_){
00217         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());
00218       }
00219 
00220       // set offset so that current pose is set to "initialpose"    
00221       tf::StampedTransform baseInMap;
00222       try{
00223         // just get the latest
00224         m_tfListener->lookupTransform(base_frame_id_, global_frame_id_, ros::Time(0), baseInMap);
00225       } catch(tf::TransformException){
00226         ROS_WARN("Failed to lookup transform!");
00227         return;
00228       }
00229 
00230       tf::Transform delta = pose * baseInMap;
00231       m_offsetTf = delta * m_offsetTf;
00232 
00233     }
00234 };
00235 
00236 int main(int argc, char** argv)
00237 {
00238   ros::init(argc, argv, "fake_localization");
00239 
00240   FakeOdomNode odom;
00241 
00242   ros::spin();
00243 
00244   return 0;
00245 }


fake_localization
Author(s): Ioan A. Sucan, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:00