how do I get my ABB IRB 120 robot link transformations to be exported?

asked 2020-09-07 09:22:42 -0500

whitecatshiro gravatar image

updated 2020-09-07 10:28:38 -0500

gvdhoorn gravatar image

Hi,

So i'm currently working with the ABB IRB 120 Robot and I have a package that bridges the connection between ROS MoveIt! and a program called 3D Slicer. The idea is to export the link transformations in MoveIt! and send these transformations of a ROS-IGTL-BRIDGE to the 3D slicer software to view the movement of the robot in 3D Slicer. The rqt_graph is shown below image description

The script for the IGTL_EXPORTER that should export transformations from MoveIt! can be seen below:

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
import math
import tf
import geometry_msgs.msg
from ros_igtl_bridge.msg import igtlstring
from ros_igtl_bridge.msg import igtltransform
from geometry_msgs.msg import Transform


def push_transform(pub, name, trans, rot):

    transform = Transform()
    transform.translation.x = trans[0] * 1000
    transform.translation.y = trans[1] * 1000
    transform.translation.z = trans[2] * 1000
    transform.rotation.x = rot[0]
    transform.rotation.y = rot[1]
    transform.rotation.z = rot[2]    
    transform.rotation.w = rot[3]

    transmsg = igtltransform()
    transmsg.name = name
    transmsg.transform = transform

    pub.publish(transmsg)


def igtl_exporter():

    base_link_name = 'base_link'

    link_name = [
        'link_1',
        'link_2',
        'link_3', 
        'link_4', 
        'link_5', 
        'link_6', 
        'link_6', 
        'link_6',
        'tool0',
        'drillbit',
        'needle'
    ]

    rospy.init_node('igtl_exporter', anonymous=True)

    pub_igtl_transform_out = rospy.Publisher('IGTL_TRANSFORM_OUT', igtltransform, queue_size=10)    

    listener = tf.TransformListener()
    rate = rospy.Rate(10) # 10hz

    n_link = len(link_name)

    trans = [[0]*3]*n_link
    rot = [[0]*4]*n_link

    while not rospy.is_shutdown():

        try:
            for i in range(n_link):
                (trans[i],rot[i]) = listener.lookupTransform(base_link_name, link_name[i], rospy.Time())
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        for i in range(n_link):
            push_transform(pub_igtl_transform_out, link_name[i], trans[i], rot[i])

        #log_str = "igtl_exporter %s" % rospy.get_time()
        #rospy.loginfo(log_str)
        #pub.publish(log_str)
        rate.sleep()


if __name__ == '__main__':

    igtl_exporter()

The links of my ABB IRB 120 Robot, including the end effector, are named link_1, link_2, link_3, link_4, link_5, link_6 and drillbit. The ROS_IGTL_BRIDGE_NODE works on its own already without running the execution of the IGTL_EXPORTER (have tested it by sending over an image from ROS to 3D Slicer), but now I'm having issues sending over the MoveIt! link transformations.

Would anyone know what the possible cause of this issue is? (if there is any other information you need please let me know and I will supply you with what I can)

edit retag flag offensive close merge delete

Comments

now I'm having issues sending over the MoveIt! link transformations.

there is no such thing as "MoveIt link transformations". So this part of your question is not something we can understand.

Additionally "having issues" is insufficient information to be able to help you.

Without a detailed description of what is happening, what you expect to happen and your current hypothesis for why this isn't happening, we cannot help you.

gvdhoorn gravatar image gvdhoorn  ( 2020-09-07 10:29:46 -0500 )edit