ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Hi everyone...
I had the same problem. I will try to explain what I do, in case of someone is interested in try it out (this will be a long answer!)..
First, i will you you the launch files that i use, and then some of the nodes I write in order to make it work... The launch files are:
<?xml version="1.0" ?>
<launch>
<arg name="r1_x" default="1" />
<arg name="r1_y" default="1" />
<arg name="r2_x" default="-1" />
<arg name="r2_y" default="1" />
<!-- start world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- -->
<arg name="use_sim_time" value="true"/>
<arg name="debug" value="true"/>
<arg name="gui" value="true"/> <!-- graphic interface -->
<arg name="headless" value="false"/>
<arg name="world_name" value="$(find tb_tables)/worlds/tres_mesas.world"/>
</include>
<!-- include robot description -->
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/kobuki_hexagons_kinect.urdf.xacro'" />
<!-- BEGIN ROBOT 0-->
<group ns="robot0">
<param name="tf_prefix" value="robot0_tf" />
<include file="$(find tb_tables)/launch/simulation/includes/turtlebot1.launch" >
<arg name="init_pose" value="-x $(arg r2_x) -y $(arg r2_y) -z 0" />
<arg name="robot_name" value="robot0" />
</include>
</group>
<!-- BEGIN ROBOT 1-->
<group ns="robot1">
<param name="tf_prefix" value="robot1_tf" />
<include file="$(find tb_tables)/launch/simulation/includes/turtlebot1.launch" >
<arg name="init_pose" value="-x $(arg r1_x) -y $(arg r1_y) -z 0" />
<arg name="robot_name" value="robot1" />
</include>
</group>
</launch>
This is the auxiliar launch file (called above, inside a namespace).
<?xml version="1.0" ?>
<launch>
<arg name="robot_name"/>
<arg name="init_pose"/>
<!-- odom publisher -->
<param name="robot_name" value= "$(arg robot_name)" />
<node pkg="tb_tables" type="odom_sim_2" name="odom_sim"/>
<!-- Gazebo model spawner -->
<node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
args="$(arg init_pose) -unpause -urdf -param /robot_description -model $(arg robot_name) -robotNamespace $(arg robot_name) -namespace $(arg robot_name)"/>
<!-- robot state publisher (at 1 Hz) -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1.0" />
</node>
<!-- Publish tf info -->
<node pkg="tb_tables" name="tb_tf_broadcaster" type="tb_tf_can2">
</node>
</launch>
And this launch file is for running the navigation stack for both robots:
<?xml version="1.0" ?>
<launch>
<arg name="map_file" default="$(find tb_tables)/maps/blank_map.yaml"/>
<arg name="rviz_robot" default="robot1"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" >
</node>
<!-- BEGIN ROBOT 0-->
<group ns="robot0">
<param name="tf_prefix" value="robot0_tf" />
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<node pkg="tb_tables" name="fake_localization" type="fake_localiza2" output="screen"/>
<include file="$(find tb_tables)/launch/simulation/includes/move_base_sim.launch.xml" />
</group>
<!-- BEGIN ROBOT 1-->
<group ns="robot1">
<param name="tf_prefix" value="robot1_tf" />
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<node pkg="tb_tables" name="fake_localization" type="fake_localiza2" output="screen"/>
<include file="$(find tb_tables)/launch/simulation/includes/move_base_sim.launch.xml" />
</group>
</launch>
You can see in this launch files that I use a node named "odom_sim". As some people have noticed, namespaces in the kobuki gazebo plugin doesn't work for many topics (being odometry one of them), so I program this node (odom_sim) in order to publish the odometry information in an namespaced topic ("robot1/odom" & "robot2/odom"). For obtaining the information of the robot position, I use the "/gazebo/get_model_state" service. The code I use is this:
#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <control_msgs/JointControllerState.h>
#include <gazebo_msgs/GetModelState.h>
#include <gazebo_msgs/ModelState.h>
#include <ros/param.h>
#include <boost/assign/list_of.hpp>
int main(int argc, char** argv){
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
ros::Publisher odom_pub;
ros::Rate r(25.0);
while(n.ok()){
ros::spinOnce(); // check for incoming messages
//
odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
std::string name;
ros::param::get( "robot_name", name);
//std::cout << name << std::endl;
// take pose info from gazebo
ros::ServiceClient client = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
gazebo_msgs::GetModelState getmodelstate;
client.waitForExistence();
getmodelstate.request.model_name = name;
client.call(getmodelstate);
//odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
//set the position
odom.pose.pose.position.x = getmodelstate.response.pose.position.x;
odom.pose.pose.position.y = getmodelstate.response.pose.position.y;
odom.pose.pose.orientation.z = getmodelstate.response.pose.orientation.z;
odom.pose.pose.orientation.w = getmodelstate.response.pose.orientation.w;
odom.pose.covariance = boost::assign::list_of(1e-1) (0) (0) (0) (0) (0)
(0) (1e-1) (0) (0) (0) (0)
(0) (0) (1e6) (0) (0) (0)
(0) (0) (0) (1e6) (0) (0)
(0) (0) (0) (0) (1e6) (0)
(0) (0) (0) (0) (0) (5e-2) ;
//set the velocity
odom.child_frame_id = "base_footprint"; //base_footprint base_link
odom.twist.twist.linear.x = getmodelstate.response.twist.linear.x;
odom.twist.twist.angular.z = getmodelstate.response.twist.angular.z;
//publish the message
odom_pub.publish(odom);
r.sleep();
}
}
Now that we have odometry information of each robot, we can write a node that publish the "odom" TF transform (I called this node "tb_tf_broadcaster" in the launch file) The code is:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <ros/param.h>
#include <tf/tf.h>
void odom_callback(const nav_msgs::OdometryConstPtr& msg){
// Publish /odom to base_footprint transform
static tf::TransformBroadcaster br;
tf::Transform BaseFootprintTransf;
BaseFootprintTransf.setOrigin(tf::Vector3(msg->pose.pose.position.x,msg->pose.pose.position.y, 0.0));
tf::Quaternion q;
tf::quaternionMsgToTF(msg->pose.pose.orientation, q);
BaseFootprintTransf.setRotation(q);
std::string tf_name;
ros::param::get( "tf_prefix", tf_name);
std::string odom_ = std::string(tf_name) + "/odom";
std::string base_footprint_ = std::string(tf_name) + "/base_footprint";
br.sendTransform(tf::StampedTransform(BaseFootprintTransf, ros::Time::now(),odom_, base_footprint_));
// Publish base_footprint transform to odom
//static tf::TransformBroadcaster br2;
//tf::Transform OdomTransf;
//br2.sendTransform(tf::StampedTransform(OdomTransf, ros::Time::now(),"/origin", odom_));
}
int main(int argc, char** argv){
ros::init(argc, argv, "turtlebot_tf_broadcaster");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("odom", 100, &odom_callback);
ros::spin();
return 0;
};
Finally, if you want to simulate de AMCL localization, you can use instead the "Fake_localization" node. This node is computationally less expensive than the AMCL node but it also works perfectly with the rest of the navigation stack modules (move_base). As AMCL this node publish TF transformation between map and robot1 and odom
In my case i had to modified the original code in order to work with two robots
#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"
#include <iostream>
class FakeOdomNode
{
public:
FakeOdomNode(void)
{
m_posePub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",1,true);
m_particlecloudPub = m_nh.advertise<geometry_msgs::PoseArray>("particlecloud",1,true);
m_tfServer = new tf::TransformBroadcaster();
m_tfListener = new tf::TransformListener();
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_link"));
private_nh.param("global_frame_id", global_frame_id_, std::string("/map")); // "/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);
private_nh.param("transform_tolerance", transform_tolerance_, 0.1);
// get our tf prefix
std::string tf_prefix = tf::getPrefixParam(private_nh);
global_frame_id_ = tf::resolve(tf_prefix, global_frame_id_);
base_frame_id_ = tf::resolve(tf_prefix, base_frame_id_);
odom_frame_id_ = tf::resolve(tf_prefix, odom_frame_id_);
m_particleCloud.header.stamp = ros::Time::now();
m_particleCloud.header.frame_id = global_frame_id_;
m_particleCloud.poses.resize(1);
ros::NodeHandle nh;
// messages (erase)
ROS_INFO("odom_frame_id: %s", odom_frame_id_.c_str());
ROS_INFO("base_frame_id: %s", base_frame_id_.c_str());
ROS_INFO("global_frame_id: %s", global_frame_id_.c_str());
m_offsetTf = tf::Transform(tf::createQuaternionFromRPY(0, 0, -delta_yaw_ ), tf::Point(-delta_x_, -delta_y_, 0.0));
stuff_sub_ = nh.subscribe("odom", 100, &FakeOdomNode::stuffFilter, this); //base_pose_ground_truth
filter_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh, "", 100); //""
filter_ = new tf::MessageFilter<nav_msgs::Odometry>(*filter_sub_, *m_tfListener, base_frame_id_, 100);
filter_->registerCallback(boost::bind(&FakeOdomNode::update, this, _1));
// 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;
}
private:
ros::NodeHandle m_nh;
ros::Publisher m_posePub;
ros::Publisher m_particlecloudPub;
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_;
ros::Subscriber stuff_sub_;
message_filters::Subscriber<nav_msgs::Odometry>* filter_sub_;
double delta_x_, delta_y_, delta_yaw_;
bool m_base_pos_received;
double transform_tolerance_;
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_;
public:
void stuffFilter(const nav_msgs::OdometryConstPtr& odom_msg){
//we have to do this to force the message filter to wait for transforms
//from odom_frame_id_ to base_frame_id_ to be available at time odom_msg.header.stamp
//really, the base_pose_ground_truth should come in with no frame_id b/c it doesn't make sense
boost::shared_ptr<nav_msgs::Odometry> stuff_msg(new nav_msgs::Odometry);
*stuff_msg = *odom_msg;
stuff_msg->header.frame_id = odom_frame_id_;
filter_->add(stuff_msg);
}
void update(const nav_msgs::OdometryConstPtr& message){
// Callback to register with tf::MessageFilter to be called when transforms are available
tf::Pose txi;
tf::poseMsgToTF(message->pose.pose, txi);
txi = m_offsetTf * txi;
tf::Stamped<tf::Pose> odom_to_map;
try
{
m_tfListener->transformPose(odom_frame_id_, tf::Stamped<tf::Pose>(txi.inverse(), message->header.stamp, base_frame_id_), odom_to_map);
}
catch(tf::TransformException &e)
{
ROS_ERROR("Failed to transform to %s from %s: %s\n", odom_frame_id_.c_str(), base_frame_id_.c_str(), e.what());
return;
}
m_tfServer->sendTransform(tf::StampedTransform(odom_to_map.inverse(),
message->header.stamp + ros::Duration(transform_tolerance_),
global_frame_id_, odom_frame_id_));
tf::Pose current;
tf::poseMsgToTF(message->pose.pose, current);
//also apply the offset to the pose
current = m_offsetTf * current;
geometry_msgs::Pose current_msg;
tf::poseTFToMsg(current, current_msg);
// Publish localized pose
m_currentPos.header = message->header;
m_currentPos.header.frame_id = global_frame_id_;
m_currentPos.pose.pose = current_msg;
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;
}
I hope that someone found this useful...
Greetings!
2 | No.2 Revision |
Hi everyone...
I had the same problem. problem, and I found a solution (at least for my case). I will try to explain what I do, in case of someone is interested in try it out (this will be a long answer!)..
First, i will you you the launch files that i use, and then some of the nodes I write in order to make it work... The launch files are:
<?xml version="1.0" ?>
<launch>
<arg name="r1_x" default="1" />
<arg name="r1_y" default="1" />
<arg name="r2_x" default="-1" />
<arg name="r2_y" default="1" />
<!-- start world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- -->
<arg name="use_sim_time" value="true"/>
<arg name="debug" value="true"/>
<arg name="gui" value="true"/> <!-- graphic interface -->
<arg name="headless" value="false"/>
<arg name="world_name" value="$(find tb_tables)/worlds/tres_mesas.world"/>
</include>
<!-- include robot description -->
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/kobuki_hexagons_kinect.urdf.xacro'" />
<!-- BEGIN ROBOT 0-->
<group ns="robot0">
<param name="tf_prefix" value="robot0_tf" />
<include file="$(find tb_tables)/launch/simulation/includes/turtlebot1.launch" >
<arg name="init_pose" value="-x $(arg r2_x) -y $(arg r2_y) -z 0" />
<arg name="robot_name" value="robot0" />
</include>
</group>
<!-- BEGIN ROBOT 1-->
<group ns="robot1">
<param name="tf_prefix" value="robot1_tf" />
<include file="$(find tb_tables)/launch/simulation/includes/turtlebot1.launch" >
<arg name="init_pose" value="-x $(arg r1_x) -y $(arg r1_y) -z 0" />
<arg name="robot_name" value="robot1" />
</include>
</group>
</launch>
This is the auxiliar launch file (called above, inside a namespace).
<?xml version="1.0" ?>
<launch>
<arg name="robot_name"/>
<arg name="init_pose"/>
<!-- odom publisher -->
<param name="robot_name" value= "$(arg robot_name)" />
<node pkg="tb_tables" type="odom_sim_2" name="odom_sim"/>
<!-- Gazebo model spawner -->
<node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
args="$(arg init_pose) -unpause -urdf -param /robot_description -model $(arg robot_name) -robotNamespace $(arg robot_name) -namespace $(arg robot_name)"/>
<!-- robot state publisher (at 1 Hz) -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1.0" />
</node>
<!-- Publish tf info -->
<node pkg="tb_tables" name="tb_tf_broadcaster" type="tb_tf_can2">
</node>
</launch>
And this launch file is for running the navigation stack for both robots:
<?xml version="1.0" ?>
<launch>
<arg name="map_file" default="$(find tb_tables)/maps/blank_map.yaml"/>
<arg name="rviz_robot" default="robot1"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" >
</node>
<!-- BEGIN ROBOT 0-->
<group ns="robot0">
<param name="tf_prefix" value="robot0_tf" />
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<node pkg="tb_tables" name="fake_localization" type="fake_localiza2" output="screen"/>
<include file="$(find tb_tables)/launch/simulation/includes/move_base_sim.launch.xml" />
</group>
<!-- BEGIN ROBOT 1-->
<group ns="robot1">
<param name="tf_prefix" value="robot1_tf" />
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<node pkg="tb_tables" name="fake_localization" type="fake_localiza2" output="screen"/>
<include file="$(find tb_tables)/launch/simulation/includes/move_base_sim.launch.xml" />
</group>
</launch>
You can see in this launch files that I use a node named "odom_sim". As some people have noticed, namespaces in the kobuki gazebo plugin doesn't work for many topics (being odometry one of them), so I program this node (odom_sim) in order to publish the odometry information in an namespaced topic ("robot1/odom" & "robot2/odom"). For obtaining the information of the robot position, I use the "/gazebo/get_model_state" service. The code I use is this:
#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <control_msgs/JointControllerState.h>
#include <gazebo_msgs/GetModelState.h>
#include <gazebo_msgs/ModelState.h>
#include <ros/param.h>
#include <boost/assign/list_of.hpp>
int main(int argc, char** argv){
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
ros::Publisher odom_pub;
ros::Rate r(25.0);
while(n.ok()){
ros::spinOnce(); // check for incoming messages
//
odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
std::string name;
ros::param::get( "robot_name", name);
//std::cout << name << std::endl;
// take pose info from gazebo
ros::ServiceClient client = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
gazebo_msgs::GetModelState getmodelstate;
client.waitForExistence();
getmodelstate.request.model_name = name;
client.call(getmodelstate);
//odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
//set the position
odom.pose.pose.position.x = getmodelstate.response.pose.position.x;
odom.pose.pose.position.y = getmodelstate.response.pose.position.y;
odom.pose.pose.orientation.z = getmodelstate.response.pose.orientation.z;
odom.pose.pose.orientation.w = getmodelstate.response.pose.orientation.w;
odom.pose.covariance = boost::assign::list_of(1e-1) (0) (0) (0) (0) (0)
(0) (1e-1) (0) (0) (0) (0)
(0) (0) (1e6) (0) (0) (0)
(0) (0) (0) (1e6) (0) (0)
(0) (0) (0) (0) (1e6) (0)
(0) (0) (0) (0) (0) (5e-2) ;
//set the velocity
odom.child_frame_id = "base_footprint"; //base_footprint base_link
odom.twist.twist.linear.x = getmodelstate.response.twist.linear.x;
odom.twist.twist.angular.z = getmodelstate.response.twist.angular.z;
//publish the message
odom_pub.publish(odom);
r.sleep();
}
}
Now that we have odometry information of each robot, we can write a node that publish the "odom" TF transform (I called this node "tb_tf_broadcaster" in the launch file) The code is:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <ros/param.h>
#include <tf/tf.h>
void odom_callback(const nav_msgs::OdometryConstPtr& msg){
// Publish /odom to base_footprint transform
static tf::TransformBroadcaster br;
tf::Transform BaseFootprintTransf;
BaseFootprintTransf.setOrigin(tf::Vector3(msg->pose.pose.position.x,msg->pose.pose.position.y, 0.0));
tf::Quaternion q;
tf::quaternionMsgToTF(msg->pose.pose.orientation, q);
BaseFootprintTransf.setRotation(q);
std::string tf_name;
ros::param::get( "tf_prefix", tf_name);
std::string odom_ = std::string(tf_name) + "/odom";
std::string base_footprint_ = std::string(tf_name) + "/base_footprint";
br.sendTransform(tf::StampedTransform(BaseFootprintTransf, ros::Time::now(),odom_, base_footprint_));
// Publish base_footprint transform to odom
//static tf::TransformBroadcaster br2;
//tf::Transform OdomTransf;
//br2.sendTransform(tf::StampedTransform(OdomTransf, ros::Time::now(),"/origin", odom_));
}
int main(int argc, char** argv){
ros::init(argc, argv, "turtlebot_tf_broadcaster");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("odom", 100, &odom_callback);
ros::spin();
return 0;
};
Finally, if you want to simulate de AMCL localization, you can use instead the "Fake_localization" node. This node is computationally less expensive than the AMCL node but it also works perfectly with the rest of the navigation stack modules (move_base). As AMCL this node publish TF transformation between map and robot1 and odom
In my case i had to modified the original code in order to work with two robots
#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"
#include <iostream>
class FakeOdomNode
{
public:
FakeOdomNode(void)
{
m_posePub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",1,true);
m_particlecloudPub = m_nh.advertise<geometry_msgs::PoseArray>("particlecloud",1,true);
m_tfServer = new tf::TransformBroadcaster();
m_tfListener = new tf::TransformListener();
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_link"));
private_nh.param("global_frame_id", global_frame_id_, std::string("/map")); // "/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);
private_nh.param("transform_tolerance", transform_tolerance_, 0.1);
// get our tf prefix
std::string tf_prefix = tf::getPrefixParam(private_nh);
global_frame_id_ = tf::resolve(tf_prefix, global_frame_id_);
base_frame_id_ = tf::resolve(tf_prefix, base_frame_id_);
odom_frame_id_ = tf::resolve(tf_prefix, odom_frame_id_);
m_particleCloud.header.stamp = ros::Time::now();
m_particleCloud.header.frame_id = global_frame_id_;
m_particleCloud.poses.resize(1);
ros::NodeHandle nh;
// messages (erase)
ROS_INFO("odom_frame_id: %s", odom_frame_id_.c_str());
ROS_INFO("base_frame_id: %s", base_frame_id_.c_str());
ROS_INFO("global_frame_id: %s", global_frame_id_.c_str());
m_offsetTf = tf::Transform(tf::createQuaternionFromRPY(0, 0, -delta_yaw_ ), tf::Point(-delta_x_, -delta_y_, 0.0));
stuff_sub_ = nh.subscribe("odom", 100, &FakeOdomNode::stuffFilter, this); //base_pose_ground_truth
filter_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh, "", 100); //""
filter_ = new tf::MessageFilter<nav_msgs::Odometry>(*filter_sub_, *m_tfListener, base_frame_id_, 100);
filter_->registerCallback(boost::bind(&FakeOdomNode::update, this, _1));
// 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;
}
private:
ros::NodeHandle m_nh;
ros::Publisher m_posePub;
ros::Publisher m_particlecloudPub;
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_;
ros::Subscriber stuff_sub_;
message_filters::Subscriber<nav_msgs::Odometry>* filter_sub_;
double delta_x_, delta_y_, delta_yaw_;
bool m_base_pos_received;
double transform_tolerance_;
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_;
public:
void stuffFilter(const nav_msgs::OdometryConstPtr& odom_msg){
//we have to do this to force the message filter to wait for transforms
//from odom_frame_id_ to base_frame_id_ to be available at time odom_msg.header.stamp
//really, the base_pose_ground_truth should come in with no frame_id b/c it doesn't make sense
boost::shared_ptr<nav_msgs::Odometry> stuff_msg(new nav_msgs::Odometry);
*stuff_msg = *odom_msg;
stuff_msg->header.frame_id = odom_frame_id_;
filter_->add(stuff_msg);
}
void update(const nav_msgs::OdometryConstPtr& message){
// Callback to register with tf::MessageFilter to be called when transforms are available
tf::Pose txi;
tf::poseMsgToTF(message->pose.pose, txi);
txi = m_offsetTf * txi;
tf::Stamped<tf::Pose> odom_to_map;
try
{
m_tfListener->transformPose(odom_frame_id_, tf::Stamped<tf::Pose>(txi.inverse(), message->header.stamp, base_frame_id_), odom_to_map);
}
catch(tf::TransformException &e)
{
ROS_ERROR("Failed to transform to %s from %s: %s\n", odom_frame_id_.c_str(), base_frame_id_.c_str(), e.what());
return;
}
m_tfServer->sendTransform(tf::StampedTransform(odom_to_map.inverse(),
message->header.stamp + ros::Duration(transform_tolerance_),
global_frame_id_, odom_frame_id_));
tf::Pose current;
tf::poseMsgToTF(message->pose.pose, current);
//also apply the offset to the pose
current = m_offsetTf * current;
geometry_msgs::Pose current_msg;
tf::poseTFToMsg(current, current_msg);
// Publish localized pose
m_currentPos.header = message->header;
m_currentPos.header.frame_id = global_frame_id_;
m_currentPos.pose.pose = current_msg;
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;
}
I hope that someone found this useful...
Greetings!