Use tf2 within a dynamic reconfigure server
I'm trying to publish a TF of my calibration board in RVIZ. To initialize this frame I specified the pose an a .yaml
file. I load these parameters in my launch ile and based on these publish the frame using the transform library. After the frame is published, I use the dynamic_reconfigure library to correct for small errors in the position of this frame.
To do this, I created a launch file and a tf_publisher_node.py
(see the code below). When running the tf_publisher_node
using the rosrun my_package tf_publisher_node.py
command the frame is successfully published in rviz and I can change it using the rosrun rqt_gui rqt_gui -s reconfigure
GUI . Unfortunately, when trying to add the node to my launch file the frame doesn't show up unless I first open the GUI and change a parameter value. I suspect this has something to do with the tf_publisher_node
initialising the frame before rviz or the tf2 tree is loaded. Further the frame further also fades away as the dynamic reconfigure callback function is only called when you change the parameters.
Launchfile code
<launch>
<!-- Start the Kinect processing ROS nodes
The IAI_kinect2 package was used for the kinect processing.
-->
<include file="$(find panda_autograsp)/launch/kinect2_bridge.launch" >
<arg name="output" value="log" />
</include>
<!-- Load Rviz-->
<include file="$(find panda_autograsp)/launch/panda_autograsp_rviz.launch">
<arg name="output" value="log" />
</include>
<!-- Load dynamic reconfigure GUI -->
<node pkg="rqt_gui" type="rqt_gui" name="dyn_reconf_gui" args="-s reconfigure" />
<!-- Create static TF for the calibration board (CB)
The pose for this static TF can be set in the calib_frame_pose.yaml file.
TF broadcaster conventions:
yaw: is rotation about Z (blue axis)
pitch: is rotation about Y (red axis)
roll: is rotation about X (green axis)
-->
<rosparam command="load" file="$(find panda_autograsp)/cfg/calib_frame_pose.yaml"/> <!-- Load calibration parametesr -->
<node pkg="panda_autograsp" type="calib_frame_tf2_broadcaster.py" name="calib_frame_broadcaster" args="panda_link0 calib_frame" />
</launch>
tf_publisher_node.py code
#!/usr/bin/env python
""" Calib frame tf2 broadcaster node
This node publishes the calibframe position relative to the robot base (panda_link0).
It also includes a dynamic reconfiguration server such that this frame can be changed by the user.
"""
## Import standard library packages ##
import sys
## Import ros packages ##
import rospy
from dynamic_reconfigure.server import Server
from panda_autograsp.cfg import PandaAutoGraspConfig
import tf_conversions
import tf
import tf2_ros
import geometry_msgs.msg
#################################################
## calib_frame_tf2_broadcaster script ###########
#################################################
class calib_frame_tf2_broadcaster():
def __init__(self, parent_frame="panda_link0", child_id="calib_frame"):
"""Calib_frame_tf2_broadcaster class initialization.
Parameters
----------
parent_frame : str, optional
Name of the parent frame, by default "panda_link0"
child_id : str, optional
Name of the child frame, by default "calib_frame"
"""
## Retrieve parameters ##
self.parent_frame = parent_frame
self.child_id = child_id
## Create static broadcster ##
self.static_br = tf2_ros.StaticTransformBroadcaster()
## Initialize transform broardcaster ##
self.br = tf2_ros.TransformBroadcaster()
self.tf_msg = geometry_msgs.msg.TransformStamped()
## Initialize dynamic reconfigure server ##
self.srv = Server(PandaAutoGraspConfig, self.dync_reconf_callback)
def dync_reconf_callback(self, config, level):
"""Dynamic reconfigure callback function.
Parameters
----------
config : dict
Dictionary containing the dynamically reconfigured parameters.
level : int
A bitmask which will ...