Hi!
For planning without running a full localization, I found
fake_localization quite useful. I fixed some problems and added a new
functionality, which could be integrated back if others agree that it's
useful. The full changed file and a patch against trunk is attached.
There are some things (hacks?) that I did not fully understand, so I
left most of fake_localization.cpp untouched ;)
The things I changed are:
* frames_ids are now config options (not every robot has a
base_footprint, and shouldn't this be base_link instead?)
* added a callback to geometry_msgs::PoseWithCovarianceStamped
("initialpose"), so you can (re)set the localized pose in RViz.
That works nice so far, and I wanted to get this one step further, as a
really simple fake localization e.g. to interact directly with planning
algorithms by just using RViz (setting start and goal pose). For this, I
would want it to run even when there are no odometry messages. A user
could then set the robot pose with the "initialpose" topic and the
localization would put the robot there by publishing the corresponding
TFs as long as no odometry arrives. Right now everything is triggered by
odometry, and I don't fully understand the subscriptions there. There is
one connected to a tf filter with no topic (empty string), and another
one that only adds its message to the filter... shouldn't one
subscription be sufficient?
Best regards,
Armin
--
Armin Hornung Albert-Ludwigs-Universität
www.informatik.uni-freiburg.de/~hornunga Dept. of Computer Science
HornungA@informatik.uni-freiburg.de Humanoid Robots Lab
Tel.: +49 (0)761-203-8010 Georges-Köhler-Allee 79
Fax : +49 (0)761-203-8007 D-79110 Freiburg, Germany
Index: fake_localization.cpp
===================================================================
--- fake_localization.cpp (revision 29634)
+++ fake_localization.cpp (working copy)
@@ -102,18 +102,28 @@
ros::NodeHandle private_nh("~");
private_nh.param("odom_frame_id", odom_frame_id_, std::string("odom"));
+ private_nh.param("base_frame_id", base_frame_id_, std::string("base_footprint")); // shouldn't this be base_link?
+ private_nh.param("global_frame_id", global_frame_id_, std::string("map"));
private_nh.param("delta_x", delta_x_, 0.0);
private_nh.param("delta_y", delta_y_, 0.0);
private_nh.param("delta_yaw", delta_yaw_, 0.0);
m_particleCloud.header.stamp = ros::Time::now();
- m_particleCloud.header.frame_id = "/map";
+ m_particleCloud.header.frame_id = global_frame_id_;
m_particleCloud.set_poses_size(1);
ros::NodeHandle nh;
+
+ m_offsetTf = tf::Transform(tf::createQuaternionFromRPY(0, 0, -delta_yaw_ ), tf::Point(-delta_x_, -delta_y_, 0.0));
+
tf_prefix_ = tf::getPrefixParam(nh);
filter_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh, "", 100);
filter_ = new tf::MessageFilter<nav_msgs::Odometry>(*filter_sub_, *m_tfListener, odom_frame_id_, 100);
filter_->registerCallback(boost::bind(&FakeOdomNode::update, this, _1));
m_groundTruthSub = m_nh.subscribe("base_pose_ground_truth", 1, &FakeOdomNode::basePosReceived, this);
+
+ // subscription to "2D Pose Estimate" from RViz:
+ m_initPoseSub = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(nh, "initialpose", 1);
+ m_initPoseFilter = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*m_initPoseSub, *m_tfListener, global_frame_id_, 1);
+ m_initPoseFilter->registerCallback(boost::bind(&FakeOdomNode::initPoseReceived, this, _1));
}
~FakeOdomNode(void)
@@ -124,6 +134,7 @@
// Just kill time as spin is not working!
+ // This seems to be obsolete?
void run(void)
{
// A duration for sleeping will be 100 ms
@@ -141,8 +152,10 @@
ros::Publisher m_posePub;
ros::Publisher m_particlecloudPub;
ros::Subscriber m_groundTruthSub;
+ message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseSub;
tf::TransformBroadcaster *m_tfServer;
tf::TransformListener *m_tfListener;
+ tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseFilter;
tf::MessageFilter<nav_msgs::Odometry>* filter_;
message_filters::Subscriber<nav_msgs::Odometry>* filter_sub_;
@@ -154,15 +167,18 @@
nav_msgs::Odometry m_basePosMsg;
geometry_msgs::PoseArray m_particleCloud;
geometry_msgs::PoseWithCovarianceStamped m_currentPos;
+ tf::Transform m_offsetTf;
//parameter for what odom to use
std::string odom_frame_id_;
+ std::string base_frame_id_;
+ std::string global_frame_id_;
std::string tf_prefix_;
void basePosReceived(const nav_msgs::OdometryConstPtr& msg)
{
m_basePosMsg = *msg;
- m_basePosMsg.header.frame_id = tf::resolve(tf_prefix_, "base_footprint"); //hack to make the filter do what I want (changed back later)
+ m_basePosMsg.header.frame_id = tf::resolve(tf_prefix_, base_frame_id_); //hack to make the filter do what I want (changed back later)
boost::shared_ptr<nav_msgs::Odometry> message(new nav_msgs::Odometry);
*message = m_basePosMsg;
filter_->add(message);
@@ -170,24 +186,10 @@
}
public:
void update(const nav_msgs::OdometryConstPtr& message){
- tf::Quaternion delta_orientation = tf::createQuaternionFromRPY(0, 0, -delta_yaw_ );
- tf::Quaternion orientation(message->pose.pose.orientation.x,
- message->pose.pose.orientation.y,
- message->pose.pose.orientation.z,
- message->pose.pose.orientation.w);
- orientation *= delta_orientation;
- tf::Transform txi(orientation,
- tf::Point(message->pose.pose.position.x - delta_x_,
- message->pose.pose.position.y - delta_y_,
- 0.0 * message->pose.pose.position.z)); // zero height for base_footprint
-
- double x = txi.getOrigin().x();
- double y = txi.getOrigin().y();
-
- double yaw, pitch, roll;
- txi.getBasis().getEulerYPR(yaw, pitch, roll);
- yaw = angles::normalize_angle(yaw);
-
+ tf::Pose txi;
+ tf::poseMsgToTF(message->pose.pose, txi);
+
+ txi = m_offsetTf * txi;
tf::Transform txo = txi;
//tf::Transform txIdentity(tf::Quaternion(0, 0, 0), tf::Point(0, 0, 0));
@@ -205,7 +207,7 @@
try
{
m_tfListener->transformPose(odom_frame_id_,
- tf::Stamped<tf::Pose> (txo.inverse(), message->header.stamp, "base_footprint"),
+ tf::Stamped<tf::Pose> (txo.inverse(), message->header.stamp, base_frame_id_),
odom_to_map);
}
catch(tf::TransformException &e){
@@ -217,16 +219,15 @@
m_tfServer->sendTransform(tf::StampedTransform
(odom_to_map.inverse(),
message->header.stamp,
- "/map", odom_frame_id_));
+ global_frame_id_, odom_frame_id_));
// Publish localized pose
m_currentPos.header = message->header;
- m_currentPos.header.frame_id = "/map"; ///\todo fixme hack
- m_currentPos.pose.pose.position.x = x;
- m_currentPos.pose.pose.position.y = y;
- // Leave z as zero
- tf::quaternionTFToMsg(tf::createQuaternionFromYaw(yaw),
- m_currentPos.pose.pose.orientation);
+ m_currentPos.header.frame_id = global_frame_id_;
+
+ tf::poseTFToMsg(txi, m_currentPos.pose.pose);
+ // Rely on correct z value from odometry...
+
// Leave covariance as zero
m_posePub.publish(m_currentPos);
@@ -234,7 +235,29 @@
m_particleCloud.header = m_currentPos.header;
m_particleCloud.poses[0] = m_currentPos.pose.pose;
m_particlecloudPub.publish(m_particleCloud);
- }
+ }
+
+ void initPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){
+ tf::Pose pose;
+ tf::poseMsgToTF(msg->pose.pose, pose);
+
+ if (msg->header.frame_id != global_frame_id_){
+ 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());
+ }
+
+ // set offset so that current pose is set to "initialpose"
+ tf::StampedTransform baseInMap;
+ try{
+ m_tfListener->lookupTransform(base_frame_id_, global_frame_id_, msg->header.stamp, baseInMap);
+ } catch(tf::TransformException){
+ ROS_WARN("Failed to lookup transform!");
+ return;
+ }
+
+ tf::Transform delta = pose * baseInMap;
+ m_offsetTf = delta * m_offsetTf;
+
+ }
};
int main(int argc, char** argv)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/** \author Ioan Sucan */
/**
@mainpage
@htmlinclude manifest.html
@b odom_localization Takes in ground truth pose information for a robot
base (e.g., from a simulator or motion capture system) and republishes
it as if a localization system were in use.
<hr>
@section usage Usage
@verbatim
$ fake_localization
@endverbatim
<hr>
@section topic ROS topics
Subscribes to (name/type):
- @b "base_pose_ground_truth" nav_msgs/Odometry : robot's odometric pose. Only the position information is used (velocity is ignored).
Publishes to (name / type):
- @b "amcl_pose" geometry_msgs/PoseWithCovarianceStamped : robot's estimated pose in the map, with covariance
- @b "particlecloud" geometry_msgs/PoseArray : fake set of poses being maintained by the filter (one paricle only).
<hr>
@section parameters ROS parameters
- "~odom_frame_id" (string) : The odometry frame to be used, default: "odom"
**/
#include <ros/ros.h>
#include <ros/time.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <angles/angles.h>
#include "ros/console.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"
class FakeOdomNode
{
public:
FakeOdomNode(void)
{
m_posePub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",1);
m_particlecloudPub = m_nh.advertise<geometry_msgs::PoseArray>("particlecloud",1);
m_tfServer = new tf::TransformBroadcaster();
m_tfListener = new tf::TransformListener();
m_lastUpdate = ros::Time::now();
m_base_pos_received = false;
ros::NodeHandle private_nh("~");
private_nh.param("odom_frame_id", odom_frame_id_, std::string("odom"));
private_nh.param("base_frame_id", base_frame_id_, std::string("base_footprint")); // shouldn't this be base_link?
private_nh.param("global_frame_id", global_frame_id_, std::string("map"));
private_nh.param("delta_x", delta_x_, 0.0);
private_nh.param("delta_y", delta_y_, 0.0);
private_nh.param("delta_yaw", delta_yaw_, 0.0);
m_particleCloud.header.stamp = ros::Time::now();
m_particleCloud.header.frame_id = global_frame_id_;
m_particleCloud.set_poses_size(1);
ros::NodeHandle nh;
m_offsetTf = tf::Transform(tf::createQuaternionFromRPY(0, 0, -delta_yaw_ ), tf::Point(-delta_x_, -delta_y_, 0.0));
tf_prefix_ = tf::getPrefixParam(nh);
filter_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh, "", 100);
filter_ = new tf::MessageFilter<nav_msgs::Odometry>(*filter_sub_, *m_tfListener, odom_frame_id_, 100);
filter_->registerCallback(boost::bind(&FakeOdomNode::update, this, _1));
m_groundTruthSub = m_nh.subscribe("base_pose_ground_truth", 1, &FakeOdomNode::basePosReceived, this);
// subscription to "2D Pose Estimate" from RViz:
m_initPoseSub = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(nh, "initialpose", 1);
m_initPoseFilter = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*m_initPoseSub, *m_tfListener, global_frame_id_, 1);
m_initPoseFilter->registerCallback(boost::bind(&FakeOdomNode::initPoseReceived, this, _1));
}
~FakeOdomNode(void)
{
if (m_tfServer)
delete m_tfServer;
}
// Just kill time as spin is not working!
// This seems to be obsolete?
void run(void)
{
// A duration for sleeping will be 100 ms
ros::Duration snoozer;
snoozer.fromSec(0.1);
while(true){
snoozer.sleep();
}
}
private:
ros::NodeHandle m_nh;
ros::Publisher m_posePub;
ros::Publisher m_particlecloudPub;
ros::Subscriber m_groundTruthSub;
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseSub;
tf::TransformBroadcaster *m_tfServer;
tf::TransformListener *m_tfListener;
tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseFilter;
tf::MessageFilter<nav_msgs::Odometry>* filter_;
message_filters::Subscriber<nav_msgs::Odometry>* filter_sub_;
ros::Time m_lastUpdate;
double m_maxPublishFrequency;
double delta_x_, delta_y_, delta_yaw_;
bool m_base_pos_received;
nav_msgs::Odometry m_basePosMsg;
geometry_msgs::PoseArray m_particleCloud;
geometry_msgs::PoseWithCovarianceStamped m_currentPos;
tf::Transform m_offsetTf;
//parameter for what odom to use
std::string odom_frame_id_;
std::string base_frame_id_;
std::string global_frame_id_;
std::string tf_prefix_;
void basePosReceived(const nav_msgs::OdometryConstPtr& msg)
{
m_basePosMsg = *msg;
m_basePosMsg.header.frame_id = tf::resolve(tf_prefix_, base_frame_id_); //hack to make the filter do what I want (changed back later)
boost::shared_ptr<nav_msgs::Odometry> message(new nav_msgs::Odometry);
*message = m_basePosMsg;
filter_->add(message);
// update();
}
public:
void update(const nav_msgs::OdometryConstPtr& message){
tf::Pose txi;
tf::poseMsgToTF(message->pose.pose, txi);
txi = m_offsetTf * txi;
tf::Transform txo = txi;
//tf::Transform txIdentity(tf::Quaternion(0, 0, 0), tf::Point(0, 0, 0));
// Here we directly publish a transform from Map to base_link. We will skip the intermediate step of publishing a transform
// to the base footprint, as it seems unnecessary. However, if down the road we wish to use the base footprint data,
// and some other component is publishing it, this should change to publish the map -> base_footprint instead.
// A hack links the two frames.
// m_tfServer->sendTransform(tf::Stamped<tf::Transform>
// (txIdentity.inverse(),
// message->header.stamp,
// "base_footprint", "base_link")); // this is published by base controller
// subtracting base to ?odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
m_tfListener->transformPose(odom_frame_id_,
tf::Stamped<tf::Pose> (txo.inverse(), message->header.stamp, base_frame_id_),
odom_to_map);
}
catch(tf::TransformException &e){
ROS_DEBUG("Failed to transform to odom %s\n",e.what());
return;
}
m_tfServer->sendTransform(tf::StampedTransform
(odom_to_map.inverse(),
message->header.stamp,
global_frame_id_, odom_frame_id_));
// Publish localized pose
m_currentPos.header = message->header;
m_currentPos.header.frame_id = global_frame_id_;
tf::poseTFToMsg(txi, m_currentPos.pose.pose);
// Rely on correct z value from odometry...
// Leave covariance as zero
m_posePub.publish(m_currentPos);
// The particle cloud is the current position. Quite convenient.
m_particleCloud.header = m_currentPos.header;
m_particleCloud.poses[0] = m_currentPos.pose.pose;
m_particlecloudPub.publish(m_particleCloud);
}
void initPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){
tf::Pose pose;
tf::poseMsgToTF(msg->pose.pose, pose);
if (msg->header.frame_id != global_frame_id_){
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());
}
// set offset so that current pose is set to "initialpose"
tf::StampedTransform baseInMap;
try{
m_tfListener->lookupTransform(base_frame_id_, global_frame_id_, msg->header.stamp, baseInMap);
} catch(tf::TransformException){
ROS_WARN("Failed to lookup transform!");
return;
}
tf::Transform delta = pose * baseInMap;
m_offsetTf = delta * m_offsetTf;
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "fake_localization");
FakeOdomNode odom;
ros::spin();
return 0;
}
(text/plain)