biped_serializer.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "std_msgs/UInt8MultiArray.h"
#include "std_srvs/Empty.h"
#include "visualization_msgs/MarkerArray.h"
#include "visualization_msgs/Marker.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "biped_robin_msgs/StepTarget3DService.h"
#include "biped_robin_msgs/SetModeService.h"
#include "biped_robin_msgs/SetParamService.h"
#include "biped_robin_msgs/MovePostureCartesianService.h"
#include "biped_robin_msgs/MovePostureJointService.h"
#include "biped_robin_msgs/MoveGripperService.h"

 /*
 * This node serializes the data to send to RTAI via mailbox
 */

#define DEFAULT_VARIABLE_NUMBER 150
#define DEFAULT_WORDLENGTH 40   /* multible of 4 and compatible with simulink receiving file */


struct DataPoint
{
        char name[DEFAULT_WORDLENGTH];
        long timestamp;
        double value;
};

struct DataPointArray
{
        struct DataPoint dataPoint[DEFAULT_VARIABLE_NUMBER];
        int length;
};

ros::Publisher rtai_data;
int old_left_right = 1;
unsigned int stepCounter = 0;


bool sendParameterToRobot (const char* name, double value) {
        struct DataPointArray data;

        ROS_INFO("Robot Parameter \"%s\" will be set to value: %f.", name, value);

        /* populate data */
        strcpy(data.dataPoint[0].name, name); data.dataPoint[0].value = value;
        /* set number of parameters */
        data.length = 1;
        /* set timestamps */
        for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;

        //copy DataPointArray to vector
        std_msgs::UInt8MultiArray msgOut;
        //TODO: header to be added!
        for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
                msgOut.data.push_back(((char*)&data)[i]);
        }
        rtai_data.publish(msgOut);

        return true;
}

bool footstepIncCallback(biped_robin_msgs::StepTarget3DService::Request &req, biped_robin_msgs::StepTarget3DService::Response &res) {
        ROS_INFO("Passing incremental step to robot");  

        struct DataPointArray data;
        double roll=0;
        double pitch=0;
        double yaw=0;   
        
        // btMatrix3x3(btQuaternion(req.step.pose.orientation.x, req.step.pose.orientation.y, req.step.pose.orientation.z, req.step.pose.orientation.w)).getEulerYPR(yaw, pitch, roll);
        tf::Quaternion quat;    
        tf::quaternionMsgToTF(req.step.pose.orientation, quat);
        tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

        if(req.step.leg == 0){
                strcpy(data.dataPoint[0].name, "srv_desiredStepInc_feet"); data.dataPoint[0].value = 2;
        } else {
                strcpy(data.dataPoint[0].name, "srv_desiredStepInc_feet"); data.dataPoint[0].value = 1;
        }       
        strcpy(data.dataPoint[1].name, "srv_desiredStepInc_x"); data.dataPoint[1].value = req.step.pose.position.x;
        strcpy(data.dataPoint[2].name, "srv_desiredStepInc_y"); data.dataPoint[2].value = req.step.pose.position.y;
        strcpy(data.dataPoint[3].name, "srv_desiredStepInc_z"); data.dataPoint[3].value = req.step.pose.position.z;
        strcpy(data.dataPoint[4].name, "srv_desiredStepInc_alpha"); data.dataPoint[4].value = roll;
        strcpy(data.dataPoint[5].name, "srv_desiredStepInc_beta"); data.dataPoint[5].value = pitch;
        strcpy(data.dataPoint[6].name, "srv_desiredStepInc_gamma"); data.dataPoint[6].value = yaw;
        strcpy(data.dataPoint[7].name, "srv_executeStep"); data.dataPoint[7].value = ++stepCounter;
        ROS_INFO("This was Step Number %d", stepCounter);

        data.length = 8;
        /*TODO:  set timestamps */
        for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
        //copy DataPointArray to vector
        std_msgs::UInt8MultiArray msgOut;
        //TODO: header to be added!
        for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
                msgOut.data.push_back(((char*)&data)[i]);
        }
        rtai_data.publish(msgOut);

        res.successful = true;

        return true;
}

bool movePostureCartesianCallback(biped_robin_msgs::MovePostureCartesianService::Request &req, biped_robin_msgs::MovePostureCartesianService::Response &res) {
        ROS_INFO("Passing cartesian posture to robot");
        struct DataPointArray data;
        double roll=0;
        double pitch=0;
        double yaw=0;

        
        tf::Quaternion quat;    
        tf::quaternionMsgToTF(req.upper_body_posture, quat);
        tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

        strcpy(data.dataPoint[0].name, "srv_cartMoveTime"); data.dataPoint[0].value = req.time_to_move;
        strcpy(data.dataPoint[1].name, "srv_cartMoveTCPPos_x"); data.dataPoint[1].value = req.right_tcp_position.x;
        strcpy(data.dataPoint[2].name, "srv_cartMoveTCPPos_y"); data.dataPoint[2].value = req.right_tcp_position.y;
        strcpy(data.dataPoint[3].name, "srv_cartMoveTCPPos_z"); data.dataPoint[3].value = req.right_tcp_position.z;
        strcpy(data.dataPoint[4].name, "srv_cartMoveRB_alpha"); data.dataPoint[4].value = roll;
        strcpy(data.dataPoint[5].name, "srv_cartMoveRB_beta"); data.dataPoint[5].value = pitch;
        strcpy(data.dataPoint[6].name, "srv_cartMoveRB_gamma"); data.dataPoint[6].value = yaw;
        strcpy(data.dataPoint[7].name, "srv_cartMoveLeftArm"); data.dataPoint[7].value = req.left_arm_angle;
        strcpy(data.dataPoint[8].name, "srv_cartMoveRightArm"); data.dataPoint[8].value = req.right_arm_angle;

        data.length = 9;
        /*TODO:  set timestamps */
        for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
        //copy DataPointArray to vector
        std_msgs::UInt8MultiArray msgOut;
        //TODO: header to be added!
        for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
                msgOut.data.push_back(((char*)&data)[i]);
        }
        rtai_data.publish(msgOut);

        res.successful = true;

        return true;
}

bool movePostureJointCallback(biped_robin_msgs::MovePostureJointService::Request &req, biped_robin_msgs::MovePostureJointService::Response &res) {
        ROS_INFO("Passing joint posture to robot");
        struct DataPointArray data;

        strcpy(data.dataPoint[0].name, "srv_jointMoveTime"); data.dataPoint[0].value = req.time_to_move;
        strcpy(data.dataPoint[1].name, "srv_joint_q1"); data.dataPoint[1].value = req.joint_positions[0];
        strcpy(data.dataPoint[2].name, "srv_joint_q2"); data.dataPoint[2].value = req.joint_positions[1];
        strcpy(data.dataPoint[3].name, "srv_joint_q3"); data.dataPoint[3].value = req.joint_positions[2];
        strcpy(data.dataPoint[4].name, "srv_joint_q4"); data.dataPoint[4].value = req.joint_positions[3];
        strcpy(data.dataPoint[5].name, "srv_joint_q5"); data.dataPoint[5].value = req.joint_positions[4];
        strcpy(data.dataPoint[6].name, "srv_joint_q6"); data.dataPoint[6].value = req.joint_positions[5];
        strcpy(data.dataPoint[7].name, "srv_joint_q7"); data.dataPoint[7].value = req.joint_positions[6];
        strcpy(data.dataPoint[8].name, "srv_joint_q8"); data.dataPoint[8].value = req.joint_positions[7];
        strcpy(data.dataPoint[9].name, "srv_joint_q9"); data.dataPoint[9].value = req.joint_positions[8];
        strcpy(data.dataPoint[10].name, "srv_joint_q10"); data.dataPoint[10].value = req.joint_positions[9];
        strcpy(data.dataPoint[11].name, "srv_joint_q11"); data.dataPoint[11].value = req.joint_positions[10];
        strcpy(data.dataPoint[12].name, "srv_joint_q12"); data.dataPoint[12].value = req.joint_positions[11];
        strcpy(data.dataPoint[13].name, "srv_joint_q13"); data.dataPoint[13].value = req.joint_positions[12];
        strcpy(data.dataPoint[14].name, "srv_joint_q14"); data.dataPoint[14].value = req.joint_positions[13];

        data.length = 15;
        /*TODO:  set timestamps */
        for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
        //copy DataPointArray to vector
        std_msgs::UInt8MultiArray msgOut;
        //TODO: header to be added!
        for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
                msgOut.data.push_back(((char*)&data)[i]);
        }
        rtai_data.publish(msgOut);

        res.successful = true;

        return true;
}

bool moveGripperCallback(biped_robin_msgs::MoveGripperService::Request &req, biped_robin_msgs::MoveGripperService::Response &res) {
        ROS_INFO("Passing gripper speed to robot");
        res.successful = true;
        return sendParameterToRobot("srv_moveGripperSpeed", req.speed);
}

bool acknowledgeCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
        ROS_INFO("Acknowledge Service called");
        return sendParameterToRobot("srv_acknowledgement", rand()%100);
}

bool referenceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
        ROS_INFO("Reference Service called");
        return sendParameterToRobot("srv_referenceDrives", rand()%100);
}

bool stopCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
        ROS_INFO("Stop Service called");
        return sendParameterToRobot("srv_emergencyStop", rand()%100);
}

bool resetCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
        ROS_INFO("Reset Internal State Service called");
        return sendParameterToRobot("srv_resetInternalState", rand()%100);
}

bool setModeCallback(biped_robin_msgs::SetModeService::Request &req, biped_robin_msgs::SetModeService::Response &res) {
        ROS_INFO("Set Mode Service called. Desired Mode is set to %d", req.desiredMode);
        res.successful = true;
        return sendParameterToRobot("srv_setDesiredMotionMode", req.desiredMode);
}

bool setParamCallback(biped_robin_msgs::SetParamService::Request &req, biped_robin_msgs::SetParamService::Response &res) {
        ROS_INFO("Set Parameter Service called.");
        res.successful = true;
        return sendParameterToRobot(req.parameterName.c_str(), req.parameterValue);
}


int main(int argc, char **argv)
{
  /* init ROS */

  ros::init(argc, argv, "biped_serializer");
  ros::NodeHandle n;
        ros::NodeHandle pn("~");

  rtai_data = n.advertise<std_msgs::UInt8MultiArray>("data_to_rtai", 1000);
  //rtai_data = pn.advertise<std_msgs::UInt8MultiArray>("unchangeableSteps", 10);

  ros::ServiceServer footstep3DInc_srv = n.advertiseService("footstep3DInc_srv", footstepIncCallback);
  ros::ServiceServer movePostureCartesian_srv = n.advertiseService("movePostureCartesian_srv", movePostureCartesianCallback);
  ros::ServiceServer movePostureJoint_srv = n.advertiseService("movePostureJoint_srv", movePostureJointCallback);
  ros::ServiceServer moveGripper_srv = n.advertiseService("moveGripper_srv", moveGripperCallback);
  ros::ServiceServer acknowledge_srv = n.advertiseService("acknowledge_srv", acknowledgeCallback);
  ros::ServiceServer reference_srv = n.advertiseService("reference_srv", referenceCallback);
  ros::ServiceServer stop_srv = n.advertiseService("stop_srv", stopCallback);
  ros::ServiceServer reset_srv = n.advertiseService("resetState_srv", resetCallback);
  ros::ServiceServer setMode_srv = n.advertiseService("setMode_srv", setModeCallback);
  ros::ServiceServer setParam_srv = n.advertiseService("setParam_srv", setParamCallback);

  ros::Duration(2.0).sleep();   
  ros::spinOnce();

  bool isReal;
  n.param("/par_isRealEnvironment", isReal, false);
  if (isReal) {
          ROS_INFO("The Robot is present in reality");
  } else {
          ROS_INFO("The Robot is just simulated");
  }
  sendParameterToRobot("par_isRealEnvironment", isReal);

  ros::spin();

  return 0;
}