Ask Your Question
0

tf broadcaster is not working properly on RViz

asked 2019-07-01 15:11:35 -0600

RayROS gravatar image

updated 2019-07-06 02:11:28 -0600

gvdhoorn gravatar image

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

pf

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

edit retag flag offensive close merge delete

Comments

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 gravatar imageChuiV ( 2019-07-01 18:36:34 -0600 )edit

@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?

RayROS gravatar imageRayROS ( 2019-07-05 11:33:52 -0600 )edit

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 the ColumnVector 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:

  1. Transform all scan points into base_link frame, then run the transformed points through your pf. Now your pose solution is in map frame.
  2. Subtract off ...
(more)
ChuiV gravatar imageChuiV ( 2019-07-07 16:39:09 -0600 )edit

@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 been pose_msg.header.frame_id = "world_frame";. I am going to post the working solution implementing your advise. Thanks again for your time.

RayROS gravatar imageRayROS ( 2019-07-11 17:53:15 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-07-11 17:55:27 -0600

RayROS gravatar image

Thanks to the suggestions of ChuiV, the correct answer to this question is the following:

void publishPose() // publish of the estimate of the state
{            
    Pdf<ColumnVector> * posterior = filter->PostGet();
    ColumnVector pose = posterior->ExpectedValueGet();
    SymmetricMatrix pose_cov = posterior->CovarianceGet();

    geometry_msgs::PoseStamped pose_msg;
    pose_msg.header.frame_id = "world_frame";
    pose_msg.pose.position.x = pose(1);
    pose_msg.pose.position.y = pose(2);
    pose_msg.pose.position.z = pose(3);

    static tf2_ros::TransformBroadcaster br;
    geometry_msgs::TransformStamped transformStamped;

    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "world_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);
    tf2::Quaternion q;
    q.setRPY(0, 0, 0);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

    br.sendTransform(transformStamped);
    pose_pub.publish(pose_msg);
}

I hope this could be helpful for other developers in case needed.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-07-01 15:11:35 -0600

Seen: 115 times

Last updated: Jul 11 '19