Ask Your Question
1

Use tf2 within a dynamic reconfigure server

asked 2019-09-24 10:49:07 -0500

rickstaa gravatar image

updated 2019-09-25 02:30:32 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-09-25 02:33:27 -0500

rickstaa gravatar image

updated 2019-09-25 02:39:03 -0500

I was able to solve my problem using a rospy.timer() class as explained in this blog post. I added my full solution below.

#!/usr/bin/env python
""" tf2 broadcaster node
This node broadcasts a number of tf2 frames and ensures these frames can be updated using a dynamic reconfigure server.
"""

## 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

#################################################
## tf2_broadcaster class ########################
#################################################
class 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

        ## Set member variables ##
        self.x_pos = rospy.get_param("x_pos")
        self.y_pos = rospy.get_param("y_pos")
        self.z_pos = rospy.get_param("z_pos")
        self.yaw = rospy.get_param("yaw")
        self.pitch = rospy.get_param("pitch")
        self.roll = rospy.get_param("roll")

        ## Create static broadcster ##
        self.static_br = tf2_ros.StaticTransformBroadcaster()

        ## Initialize transform broardcaster ##
        self.timer = rospy.Timer(rospy.Duration(1.0/10.0),
                                 self.tf2_broadcaster_callback)
        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 later be passed to the dynamic reconfigure callback.
            When the callback is called all of the level values for parameters that have
            been changed are ORed together and the resulting value is passed to the callback.
        """

        ## Print information ##
        rospy.loginfo("""Reconfigure Request: {x_pos}, {y_pos},\ 
            , {z_pos}, {yaw}, {pitch} & {roll}""".format(**config))

        ## Read dynamic parameter values ##
        self.x_pos = config["x_pos"]
        self.y_pos = config["y_pos"]
        self.z_pos = config["z_pos"]
        self.yaw = config["yaw"]
        self.pitch = config["pitch"]
        self.roll = config["roll"]

        ## Update parameters in the parameter server ##
        rospy.set_param('x_pos', config["x_pos"])
        rospy.set_param('y_pos', config["y_pos"])
        rospy.set_param('z_pos', config["z_pos"])
        rospy.set_param('yaw', config["yaw"])
        rospy.set_param('pitch', config["pitch"])
        rospy.set_param('roll', config["roll"])

        ## Return possibly updated configuration ##
        return config

    def tf2_broadcaster_callback(self, event=None):
        """TF2 broadcaster callback function. This function broadcast the tf2 frames.

        Parameters
        ----------
        event : rospy.timer.TimerEvent, optional
            Structure passed in provides you timing information that can be useful when debugging or profiling. , by default None
        """
        ## Generate TF message ##
        self.tf_msg.header.stamp = rospy.Time.now()
        self.tf_msg.header.frame_id = self.parent_frame
        self.tf_msg.child_frame_id = self.child_id
        self.tf_msg.transform.translation.x = self.x_pos
        self.tf_msg.transform.translation.y = self.y_pos
        self.tf_msg.transform.translation.z = self.z_pos
        quat = tf.transformations.quaternion_from_euler(
            float(self.yaw), float(self.pitch), float(self.roll), axes='rzyx')
        self.tf_msg.transform.rotation.x = quat[0]
        self.tf_msg.transform.rotation.y = quat[1]
        self.tf_msg.transform.rotation.z = quat[2]
        self.tf_msg.transform.rotation.w = quat[3]

        ## Send tf message ##
        self.br.sendTransform(self.tf_msg)

#################################################
## Main script ##################################
#################################################
if __name__ == "__main__":

    ## Initialize TF2 broadcaster node ##
    rospy.init_node('calib_frame', anonymous=False)

    ## Check if enough arguments are supplied ##
    if len(sys ...
(more)
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

2 followers

Stats

Asked: 2019-09-24 10:49:07 -0500

Seen: 22 times

Last updated: Sep 25