/home/laradmin/lar/hmi/phantom_dual/src/phantom_B.cpp

Go to the documentation of this file.

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <cassert>
#include <cmath>

#include <HD/hd.h>
#include <HDU/hduVector.h>
#include <HDU/hduMatrix.h>
#include <HDU/hduError.h>

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
#include <vrep_common/JointSetStateData.h>
#include <vrep_common/ForceSensorData.h>
#include <vrep_common/ObjectGroupData.h>
#include <vrep_common/simRosGetObjectHandle.h>
#include <phantom_dual/conio.h>

using namespace std;

typedef struct
{
    HDdouble angle[3];
    HDdouble torque[3];
    
} omni_state;

omni_state omni;

vrep_common::JointSetStateData joint_target;
vrep_common::simRosGetObjectHandle object_handles;

ros::ServiceClient object_handles_srv;
ros::Publisher command_joints_position_pub;

int right_joint1Handle;
int right_joint2Handle; 
int right_joint3Handle; 
int right_joint4Handle;
int right_joint5Handle;
int right_joint6Handle;

int left_sensor1Handle;
int left_sensor2Handle;
int left_sensor3Handle;
int left_sensor4Handle;
int right_sensor1Handle;

HDSchedulerHandle gCallbackHandle = HD_INVALID_HANDLE;

HDdouble base_torque[3];


HDCallbackCode HDCALLBACK ServoSchedulerCallback(void *pUserData)
{
    HDErrorInfo error;
    
    hdBeginFrame(hdGetCurrentDevice());
   
    hdGetDoublev(HD_CURRENT_JOINT_ANGLES, omni.angle);
    hdSetDoublev(HD_CURRENT_JOINT_TORQUE, base_torque);
    
    hdEndFrame(hdGetCurrentDevice());
    
    //cout << omni.angle[0]*(180/M_PI) << "   " << omni.angle[1]*(180/M_PI) << "   " << -(omni.angle[2]-(M_PI/2))*(180/M_PI) << endl;    
    
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error during main scheduler callback\n");
        if (hduIsSchedulerError(&error))
            return HD_CALLBACK_DONE;
    }

    return HD_CALLBACK_CONTINUE;
}


void objectHandles()
{   
    object_handles.request.objectName = "right_joint1";
    if (object_handles_srv.call(object_handles))
        right_joint1Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");

    object_handles.request.objectName = "right_joint2";
    if (object_handles_srv.call(object_handles))
        right_joint2Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
    object_handles.request.objectName = "right_joint3";
    if (object_handles_srv.call(object_handles))
        right_joint3Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
    object_handles.request.objectName = "right_joint4";
    if (object_handles_srv.call(object_handles))
        right_joint4Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");

    object_handles.request.objectName = "right_joint5";
    if (object_handles_srv.call(object_handles))
        right_joint5Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
    object_handles.request.objectName = "right_joint6";
    if (object_handles_srv.call(object_handles))
        right_joint6Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
    
    object_handles.request.objectName = "left_sensor1";
    if (object_handles_srv.call(object_handles))
        left_sensor1Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
    object_handles.request.objectName = "left_sensor2";
    if (object_handles_srv.call(object_handles))
        left_sensor2Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
    object_handles.request.objectName = "left_sensor3";
    if (object_handles_srv.call(object_handles))
        left_sensor3Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
    object_handles.request.objectName = "left_sensor4";
    if (object_handles_srv.call(object_handles))
        left_sensor4Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
    object_handles.request.objectName = "right_sensor1";
    if (object_handles_srv.call(object_handles))
        right_sensor1Handle = object_handles.response.handle;
    else
        ROS_ERROR("Failed to call service get_joint_handles");
    
//     cout << left_sensor1Handle << endl;
//     cout << left_sensor2Handle << endl; 
//     cout << left_sensor3Handle << endl; 
//     cout << left_sensor4Handle << endl;
//     cout << right_sensor1Handle << endl;

}


void setJointValue()
{
    joint_target.handles.data.clear();
    joint_target.setModes.data.clear();
    joint_target.values.data.clear();
    
    joint_target.handles.data.push_back(right_joint2Handle);
    joint_target.setModes.data.push_back(1);
    joint_target.values.data.push_back(omni.angle[0]);
    
    joint_target.handles.data.push_back(right_joint6Handle);
    joint_target.setModes.data.push_back(1);
    joint_target.values.data.push_back(-omni.angle[0]);

//     joint_target.handles.data.push_back(right_joint4Handle);
//     joint_target.setModes.data.push_back(1);
//     joint_target.values.data.push_back((omni.angle[2]-(M_PI/2)));
// 
//     joint_target.handles.data.push_back(right_joint5Handle);
//     joint_target.setModes.data.push_back(1);
//     joint_target.values.data.push_back((omni.angle[2]-(M_PI/2)));
// 
//     joint_target.handles.data.push_back(right_joint4Handle);
//     joint_target.setModes.data.push_back(1);
//     joint_target.values.data.push_back(-(omni.angle[2]-(M_PI/2)));
    
    command_joints_position_pub.publish(joint_target);
}


void jointsCallback(const sensor_msgs::JointState& msg)
{
    int scale_cte = 2700;
    
    base_torque[0] = scale_cte*msg.position[7];
    
    cout << "Balance angle: " << -msg.position[1]*(180/M_PI) << "\n" << endl;
}


// void left_sensor1Callback(const vrep_common::ForceSensorData& msg)
// {
//     cout << "Sensor 1 - left foot: " << msg.force.z << endl;
//     
// }
// 
// 
// void right_sensor1Callback(const vrep_common::ForceSensorData& msg)
// {
//     cout << "Sensor 1 - right foot: " << msg.force.z << endl;
//     
// }
// 

void sensorsCallback(const vrep_common::ObjectGroupData& msg)
{
    cout << "Left sensor 1 value: " << msg.floatData.data[2] << endl;
    cout << "Left sensor 2 value: " << msg.floatData.data[8] << endl;
    cout << "Left sensor 3 value: " << msg.floatData.data[14] << endl;
    cout << "Left sensor 4 value: " << msg.floatData.data[20] << endl;
    
    cout << "Right sensor 1 value: " << msg.floatData.data[26] << endl;
    
    cout << "--------------------------------------" << endl;   
}


int main(int argc, char **argv)
{
    HDErrorInfo error;
    HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to initialize the device");
        
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        return -1;           
    }
    ROS_INFO("Found %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));
    
    gCallbackHandle = hdScheduleAsynchronous(ServoSchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to schedule servoloop callback");

        hdDisableDevice(hHD);
        
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        return -1;
    }
    
    hdEnable(HD_FORCE_OUTPUT);
    
    hdStartScheduler();
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to start the scheduler");
        
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        return -1;           
    }
    
    ros::init(argc, argv, "phantom_B");
    
    ros::NodeHandle n;
    
    object_handles_srv = n.serviceClient<vrep_common::simRosGetObjectHandle>("/vrep/simRosGetObjectHandle");

    command_joints_position_pub = n.advertise<vrep_common::JointSetStateData>("/vrep/command_right_joints_position",1000);
    
    ros::Subscriber notify_joints_position_sub = n.subscribe("/vrep/notify_joints_position", 1000, jointsCallback);
    
    //ros::Subscriber notify_right_sensor1_data_sub = n.subscribe("/vrep/notify_right_sensor1_data", 1000, right_sensor1Callback);
    
    //ros::Subscriber notify_left_sensor1_data_sub = n.subscribe("/vrep/notify_left_sensor1_data", 1000, left_sensor1Callback);
    
    ros::Subscriber notify_sensors_data_sub = n.subscribe("/vrep/notify_sensors_data", 1000, sensorsCallback);
    
    objectHandles();
    
    ros::Rate loop_rate(1000);
    
    while (ros::ok())
    {
        setJointValue();
        
        ros::spinOnce();
        loop_rate.sleep();
    }
    
    hdStopScheduler();
    hdDisableDevice(hHD);
    
    return 0;   
}

---

phantom_dual
Author(s): Barros
autogenerated on Wed Jul 23 04:34:39 2014