tf broadcaster is not working properly on RViz
Hello, I am trying to use tf broadcaster
to associate the movement of a particle cloud filter I built using the orocos-bfl
with my urdf
. Currently everything works except that my urdf
is not moving withing the particle cloud of the filter.
From here I know that I need to create a tf broadcaster
for that. I followed the tutorial on the official documentation and created the broadcaster but nothing is happening.
I am trying to transform from /base_link
to /particle_filter_frame
and publish the /particle_filter_frame
Despite many different options I tried, nothing is moving on RViz
. What am I missing in the small piece of code that is keeping my urdf
model to move?
UPDATES
Error thrown
[pf_node-5] process has died [pid 12842, exit code -6, cmd /home/emanuele/catkin_ws/devel/lib/ros_float/pf_second_order_bis_node
__name:=pf_node __log:=/home/emanuele/.ros/log/d22eb78a-9f41-11e9-9be3-38dead5eed36/pf_node-5.log]. log file: /home/emanuele/.ros/log/d22eb78a-9f41-11e9-9be3-38dead5eed36/pf_node-5*.log
In addition to that rosrun rqt_tf_tree rqt_tf_tree
is throwing errors that I do not understand:
PluginHandlerDirect._restore_settings() plugin "rqt_tf_tree/RosTfTree#0" raised an exception:
Traceback (most recent call last):
File "/opt/ros/melodic/lib/python2.7/dist-packages/qt_gui/plugin_handler_direct.py", line 116, in _restore_settings
self._plugin.restore_settings(plugin_settings_plugin, instance_settings_plugin)
File "/opt/ros/melodic/lib/python2.7/dist-packages/rqt_tf_tree/tf_tree.py", line 131, in restore_settings
self._refresh_tf_graph()
File "/opt/ros/melodic/lib/python2.7/dist-packages/rqt_tf_tree/tf_tree.py", line 143, in _refresh_tf_graph
self._update_graph_view(self._generate_dotcode())
File "/opt/ros/melodic/lib/python2.7/dist-packages/rqt_tf_tree/tf_tree.py", line 152, in _generate_dotcode
force_refresh=force_refresh)
File "/opt/ros/melodic/lib/python2.7/dist-packages/rqt_tf_tree/dotcode_tf.py", line 97, in generate_dotcode
self.graph = self.generate(data, timer.now().to_sec())
File "/opt/ros/melodic/lib/python2.7/dist-packages/rqt_tf_tree/dotcode_tf.py", line 136, in generate
root,
UnboundLocalError: local variable 'root' referenced before assignment
pf_second_order.h
MyParticleFilter::MyParticleFilter(ros::NodeHandle &n):_n(n)
{
// system noise
//.... operations
// measurement model
//.... operations
// init ROS stuff
particle_pub = _n.advertise<geometry_msgs::PoseArray>("/particle_cloud", 1);
pose_pub = n.advertise<geometry_msgs::PoseStamped>("/estimate",1);
particleTimer = n.createTimer(ros::Duration(1), &MyParticleFilter::timerCallback, this);
}
pf_second_order.cpp
#include <tf/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
// ....
// ....
private:
void timerCallback(const ros::TimerEvent&)
{
publishParticles();
publishPose();
}
void publishPose() // publish of the estimate of the state
{
static tf2_ros::TransformBroadcaster br;
Pdf<ColumnVector> * posterior = filter->PostGet();
ColumnVector pose = posterior->ExpectedValueGet();
SymmetricMatrix pose_cov = posterior->CovarianceGet();
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = "/base_link";
pose_msg.pose.position.x = pose(1);
pose_msg.pose.position.y = pose(2);
pose_msg.pose.position.z = pose(3);
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "/particle_filter_frame";
transformStamped.child_frame_id = "/base_link";
transformStamped.transform.translation.x = pose(1);
transformStamped.transform.translation.y = pose(2);
transformStamped.transform.translation.z = pose(3);
const geometry_msgs::PoseConstPtr msg;
tf2::Quaternion q(msg->orientation.x, msg->orientation.y,
msg->orientation.z, msg->orientation.w);
q.setRPY(roll, pitch, yaw);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
br.sendTransform(transformStamped);
pose_pub.publish(pose_msg);
}
};
Thanks for helping shedding light on this issue
Can you show your full tf tree (rqt wil help with that). Typically
/base_link
is the frame that your vehicle is in, but it almost looks like you are treating/base_link
as the world frame where the/particle_filter_frame
is moving around with respect to/base_link
. This would explain why your urdf isn't moving because it is probably all defined with respect to/base_link
(which is the frame rviz is showing, so it doesn't move) If the lidar sensor is rigidly attached to your urdf, I think you simply need to start a static transform publisher.@ChuiV, thanks for taking the time to read my question. After I posted the question the project actually stopped working without any particular explanation or reason as a fresh compilation of
catkin_make
. I updated the question with the error thrown by the compiler. Any idea of what is happening?As far as running rqt tf, I usually just run
rqt
, then add the tf into it. I don't usually bother putting gui stuff into launch files.When you run your pf_second_order node by itself, what error outputs in the terminal. (or look in the log file specified by roslaunch)
If I understand you question, I still don't think that
pose_msg.header.frame_id = "/base_link";
is right. Again, if I understand what your code is doing theColumnVector pose = posterior->ExpectedValueGet();
is "with respect to the map, where am I?" Since the solution here is providing a pose with respect to the map frame, you can't specify that the solution is in the base_link frame. You'll need to do one of two things:- Transform all scan points into base_link frame, then run the transformed points through your pf. Now your pose solution is in map frame.
- Subtract off ...
(more)@ChuiV, thanks for the better explanation. I understand what was not working and as you were saying
pose_msg.header.frame_id = "/base_link";
it was not right because it should have beenpose_msg.header.frame_id = "world_frame";
. I am going to post the working solution implementing your advise. Thanks again for your time.