Ask Your Question
1

openni_tracker-x_y_z-coordinates

asked 2013-05-31 04:42:32 -0500

acp gravatar image

Hi Everybody

I have running openni_tracker in Xtion pro live, I can see all the frames in rviz.

My issue now is that I would like to get the (x,y,z) coordinates.

Any ideas,

Cheers

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2013-06-01 02:34:11 -0500

Philip gravatar image
edit flag offensive delete link more

Comments

Thank you, I saw it and now is working quite nice :)

acp gravatar imageacp ( 2013-06-04 02:36:38 -0500 )edit

Perfect, glad it worked! Can you accept the answer (by clicking the grey circle with check mark on the left), so the question can be closed? Regards, Philip

Philip gravatar imagePhilip ( 2013-06-04 03:20:53 -0500 )edit
0

answered 2016-03-30 03:35:23 -0500

PKumars gravatar image

Here is the Final answer where I'm extracting all the joint coordinates and then publishing them in each topic.

#include <ros ros.h=""> #include <tf transform_listener.h=""> #include <geometry_msgs twist.h="">

int main(int argc, char** argv) { ros::init(argc, argv, "my_skeleton_tf_listener");

ros::NodeHandle node;

// publisher declaration
ros::Publisher neck_joint = node.advertise<geometry_msgs::Point>("neck_joint", 1);
ros::Publisher head_joint = node.advertise<geometry_msgs::Point>("head_joint", 1);
ros::Publisher torso_joint = node.advertise<geometry_msgs::Point>("torso_joint", 1);
ros::Publisher left_shoulder_joint = node.advertise<geometry_msgs::Point>("left_shoulder_joint", 1);
ros::Publisher left_elbow_joint = node.advertise<geometry_msgs::Point>("left_elbow_joint", 1);
ros::Publisher left_hand_joint = node.advertise<geometry_msgs::Point>("left_hand_joint", 1);
ros::Publisher right_shoulder_joint = node.advertise<geometry_msgs::Point>("right_shoulder_joint", 1);
ros::Publisher right_elbow_joint = node.advertise<geometry_msgs::Point>("right_elbow_joint", 1);
ros::Publisher right_hand_joint = node.advertise<geometry_msgs::Point>("right_hand_joint", 1);
ros::Publisher left_hip_joint = node.advertise<geometry_msgs::Point>("left_hip_joint", 1);
ros::Publisher left_knee_joint = node.advertise<geometry_msgs::Point>("left_knee_joint", 1);
ros::Publisher left_foot_joint = node.advertise<geometry_msgs::Point>("left_foot_joint", 1);
ros::Publisher right_hip_joint = node.advertise<geometry_msgs::Point>("right_hip_joint", 1);
ros::Publisher right_knee_joint = node.advertise<geometry_msgs::Point>("right_knee_joint", 1);
ros::Publisher right_foot_joint = node.advertise<geometry_msgs::Point>("right_foot_joint", 1);

// listener 
tf::TransformListener listener;

ros::Rate rate(50.0); // frequency of operation

while (node.ok())
{
    // Transforms declared for each joint
    tf::StampedTransform transform_neck, transform_head, transform_torso, 
                            transform_left_shoulder, transform_left_elbow, transform_left_hand, 
                                transform_right_shoulder, transform_right_elbow, transform_right_hand, 
                                    transform_left_hip, transform_left_knee, transform_left_foot,
                                        transform_right_hip, transform_right_knee, transform_right_foot;
    try
    {
        // each joint frame to reference frame transforms 
        listener.lookupTransform("/neck_1", "/openni_depth_frame",ros::Time(0), transform_neck);
        listener.lookupTransform("/head_1", "/openni_depth_frame",ros::Time(0), transform_head);
        listener.lookupTransform("/torso_1", "/openni_depth_frame",ros::Time(0), transform_torso);
        listener.lookupTransform("/left_shoulder_1", "/openni_depth_frame",ros::Time(0), transform_left_shoulder);
        listener.lookupTransform("/left_elbow_1", "/openni_depth_frame",ros::Time(0), transform_left_elbow);
        listener.lookupTransform("/left_hand_1", "/openni_depth_frame",ros::Time(0), transform_left_hand);
        listener.lookupTransform("/right_shoulder_1", "/openni_depth_frame",ros::Time(0), transform_right_shoulder);
        listener.lookupTransform("/right_elbow_1", "/openni_depth_frame",ros::Time(0), transform_right_elbow);
        listener.lookupTransform("/right_hand_1", "/openni_depth_frame",ros::Time(0), transform_right_hand);
        listener.lookupTransform("/left_hip_1", "/openni_depth_frame",ros::Time(0), transform_left_hip);
        listener.lookupTransform("/left_knee_1", "/openni_depth_frame",ros::Time(0), transform_left_knee);
        listener.lookupTransform("/left_foot_1", "/openni_depth_frame",ros::Time(0), transform_left_foot);
        listener.lookupTransform("/right_hip_1", "/openni_depth_frame",ros::Time(0), transform_right_hip);
        listener.lookupTransform("/right_knee_1", "/openni_depth_frame",ros::Time(0), transform_right_knee);
        listener.lookupTransform("/right_foot_1", "/openni_depth_frame",ros::Time(0), transform_right_foot);

    }
        catch (tf::TransformException &ex) 
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(0.10).sleep();
        continue;
    }

    // geometry points declaration for storing 3D coordinates of joints and then published later 
    geometry_msgs::Point neck_pose, head_pose, torso_pose, 
                            left_shoulder_pose, left_elbow_pose, left_hand_pose,
                                right_shoulder_pose, right_elbow_pose, right_hand_pose, 
                                    left_hip_pose, left_knee_pose, left_foot_pose, 
                                        right_hip_pose, right_knee_pose, right_foot_pose;

    // joint position extraction and store
    // neck joint                                   
    neck_pose.x = transform_neck.getOrigin().x();
    neck_pose.y = transform_neck.getOrigin().y();
    neck_pose.z = transform_neck.getOrigin().z();
    // head joint
    head_pose.x = transform_head.getOrigin().x();
    head_pose.y = transform_head.getOrigin().y();
    head_pose.z = transform_head.getOrigin().z();
    // torso joint
    torso_pose.x = transform_torso.getOrigin().x();
    torso_pose.y = transform_torso.getOrigin().y();
    torso_pose.z = transform_torso.getOrigin().z();
    // left shoulder joint 
    left_shoulder_pose.x = transform_left_shoulder.getOrigin().x();
    left_shoulder_pose.y = transform_left_shoulder.getOrigin().y();
    left_shoulder_pose.z = transform_left_shoulder.getOrigin().z();
    // left elbow joint
    left_elbow_pose.x = transform_left_elbow.getOrigin().x();
    left_elbow_pose.y = transform_left_elbow.getOrigin().y();
    left_elbow_pose.z = transform_left_elbow.getOrigin().z();
    // left hand joint
    left_hand_pose.x = transform_left_hand.getOrigin().x();
    left_hand_pose.y = transform_left_hand.getOrigin().y();
    left_hand_pose.z = transform_left_hand ...
(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

Stats

Asked: 2013-05-31 04:42:32 -0500

Seen: 220 times

Last updated: Mar 30 '16