how get pose(x,y,z) of links and plot in rqt? [closed]

edit

i add static_transform_publisher node in launch file :

 <node name="rob2cambase" pkg="tf" type="static_transform_publisher" args="1.58 0.0 0 0 0.0 0.06 /base_link /link_U26_1  100"/>

and write track_kin.py node in node folder :

    #!/usr/bin/env python
import roslib; roslib.load_manifest('tf_workshop')
import rospy
import math
import tf
from geometry_msgs.msg import PoseStamped
from gtk.keysyms import Pause

def callback(data):

    x=data.pose.position.x/1000;
    y=data.pose.position.y/1000;
    z=data.pose.position.z/1000
    quat=tf.transformations.quaternion_from_euler( data.pose.orientation.x*3.1415926/180, data.pose.orientation.y*3.1415926/180,     data.pose.orientation.z*3.1415926/180, 'sxyz')

# Enter here the transform to send:
    br.sendTransform(x,y,z, quat, rospy.Time.now(),"link_U26_1", "base_link")    



def listener():

    global br
    tflistener = tf.TransformListener()
    rospy.Subscriber("armpose", PoseStamped, callback)
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)

    rospy.spin()

if __name__ == '__main__':
    rospy.init_node('arm_listener', anonymous=True)
    listener()

and write object_tracker.cpp in src folder :

    #include <stdio.h>
#include <ros/ros.h>
#include <cstdlib>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Vector3.h>
#include <math.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/PointCloud2.h>

#define pi 3.14159265

class Ktracker{

public:
    Ktracker();
    ros::Subscriber trkmarker_sub, pose_sub;
    ros::Publisher objectpc_pub;

    void trackCallback(const visualization_msgs::Marker&);
    void poseCallback(const geometry_msgs::PoseStamped&);

    tf::TransformListener listener;
    tf::StampedTransform transform;
    tf::TransformBroadcaster br;
    tf::Quaternion qtf;

private:
    sensor_msgs::PointCloud objectpc;


};

Ktracker::Ktracker(){

    ros::NodeHandle n;
    objectpc_pub=n.advertise<sensor_msgs::PointCloud>("objpc",1000);
}


void Ktracker::trackCallback(const visualization_msgs::Marker &msg){


    objectpc.set_points_size(msg.get_points_size());

    int i;
    for (i=0;i<(int) msg.get_points_size();i++){
        objectpc.points[i].x=msg.points[i].x;
        objectpc.points[i].y=msg.points[i].y;
        objectpc.points[i].z=msg.points[i].z;
    }

    ros::Time now = ros::Time::now();
    transform.setOrigin(tf::Vector3(msg.pose.position.x,msg.pose.position.y,msg.pose.position.z));
    tf::quaternionMsgToTF(msg.pose.orientation,qtf);
    transform.setRotation(qtf);
    br.sendTransform(tf::StampedTransform(transform, msg.header.stamp, "/link_U26_1","/object_frame"));
    objectpc.header.frame_id="/object_frame";

    try
    {
// Enter the correct transformation to track the object 


        now=ros::Time::now();
        listener.waitForTransform("/object_frame", "/link_U26_1",ros::Time::now(), ros::Duration(0.2));
        listener.lookupTransform("/object_frame", "/link_U26_1",now, transform);
        listener.transformPointCloud("/link_U26_1",objectpc,objectpc);
        objectpc.header.frame_id="/link_U26_1";
        objectpc_pub.publish(objectpc);

    }
    catch (tf::TransformException ex)
    {
        ROS_WARN("TF Exception: %s", ex.what());
    }
}


void Ktracker::poseCallback(const geometry_msgs::PoseStamped &msg){
    objectpc_pub.publish(objectpc);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "armtrack");
    Ktracker ktracker;
    ros::Rate loop_rate(20);
    visualization_msgs::Marker bluem;
    ros::NodeHandle n;
    ktracker.trkmarker_sub=n.subscribe("markers_out", 1000, &Ktracker::trackCallback,&ktracker);
    ktracker.pose_sub=n.subscribe("armpose",1000, &Ktracker::poseCallback,&ktracker);

    ros::spin();

}

and build package by : rosmake wrist_moveit_config

how plot pose link_U26_1 in rqt ?? all steps are true ??