test_glove_fob.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "asr_msgs/AsrGlove.h"
#include "asr_msgs/AsrObject.h"
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

ros::Subscriber sub_states;
ros::Publisher pub_states;
sensor_msgs::JointState joint_state;

ros::Subscriber sub_tf;
tf::TransformBroadcaster *pub_tf;
geometry_msgs::TransformStamped odom_trans;

void callback(const asr_msgs::AsrGloveConstPtr& msg)
{
    joint_state.header.stamp = ros::Time::now();

    // wrist: not needed because used in combination with asr_flock_of_birds
    joint_state.position[0] = 0;
    joint_state.position[1] = 0;

    // some values are manually adjusted since its difficult to calibrate
    // TODO: correct mapping of thumb joints
    joint_state.position[2] = 0.17;
    joint_state.position[3] = 0.02;
    joint_state.position[4] = 0.17;
    joint_state.position[5] = msg->data[5];

    joint_state.position[6] = msg->data[6];
    joint_state.position[7] = msg->data[7];
    joint_state.position[8] = msg->data[8];
    joint_state.position[9] = msg->data[9] * (-1);

    joint_state.position[10] = msg->data[10] - 0.1;
    joint_state.position[11] = msg->data[11];
    joint_state.position[12] = msg->data[12];
    joint_state.position[13] = msg->data[13] - 0.5;

    joint_state.position[14] = msg->data[14];
    joint_state.position[15] = msg->data[15] - 0.15;
    joint_state.position[16] = msg->data[16];
    joint_state.position[17] = msg->data[17] - 0.5;

    joint_state.position[18] = msg->data[18] - 0.3;
    joint_state.position[19] = msg->data[19] - 0.3;
    joint_state.position[20] = msg->data[20];
    joint_state.position[21] = msg->data[21] * (-1) + 0.1;

    pub_states.publish(joint_state);
}

void callback_tf(const asr_msgs::AsrObjectConstPtr& msg)
{
    if (msg->identifier == "tracker_right")
    {
        odom_trans.header.stamp = ros::Time::now();

        if(!msg->sampledPoses.size())
        {
            std::cerr << "Got a AsrObject without poses." << std::endl;
            std::exit(1);
        }

        geometry_msgs::Pose current_pose = msg->sampledPoses.front().pose;
        geometry_msgs::Point position = current_pose.position;

        odom_trans.transform.translation.x = position.x;
        odom_trans.transform.translation.y = position.y;
        odom_trans.transform.translation.z = position.z;
        odom_trans.transform.rotation = current_pose.orientation;

        pub_tf->sendTransform(odom_trans);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_glove_fob");
    ros::NodeHandle node;

    sub_states = node.subscribe("rightGloveData_radian", 1, callback);
    pub_states = node.advertise<sensor_msgs::JointState>("joint_states", 1);

    sub_tf = node.subscribe("fob_objects", 1, callback_tf);
    pub_tf = new tf::TransformBroadcaster();

    joint_state.name.resize(22);
    joint_state.position.resize(22);

    // the names are set accordingly to the names used in the xml model
    joint_state.name[0] ="wr_j0";
    joint_state.name[1] ="wr_j1";

    joint_state.name[2] ="th_j0";
    joint_state.name[3] ="th_j1";
    joint_state.name[4] ="th_j2";
    joint_state.name[5] ="th_j3";

    joint_state.name[6] ="in_j1";
    joint_state.name[7] ="in_j0";
    joint_state.name[8] ="in_j2";
    joint_state.name[9] ="in_j3";

    joint_state.name[10] ="mi_j1";
    joint_state.name[11] ="mi_j0";
    joint_state.name[12] ="mi_j2";
    joint_state.name[13] ="mi_j3";

    joint_state.name[14] ="ri_j1";
    joint_state.name[15] ="ri_j0";
    joint_state.name[16] ="ri_j2";
    joint_state.name[17] ="ri_j3";

    joint_state.name[18] ="pi_j1";
    joint_state.name[19] ="pi_j0";
    joint_state.name[20] ="pi_j2";
    joint_state.name[21] ="pi_j3";

    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "fob_sensor";

    ros::spin();

    return 0;
}