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 "bipedRobin_msgs/StepTarget3DService.h"
#include "bipedRobin_msgs/SetModeService.h"
#include "bipedRobin_msgs/SetParamService.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;
        //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 footstepCallback(bipedRobin_msgs::StepTarget3DService::Request &req, bipedRobin_msgs::StepTarget3DService::Response &res) {
        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);

        strcpy(data.dataPoint[0].name, "srv_desiredStep_flag"); data.dataPoint[0].value = 2;

        if(old_left_right%2 == req.step.leg){
                ROS_ERROR("Wrong foot received");
                return 0;
        }

        if(req.step.leg == 0){
                strcpy(data.dataPoint[1].name, "srv_desiredStep_feet"); data.dataPoint[1].value = 2;
                old_left_right = 2;
        } else {
                strcpy(data.dataPoint[1].name, "srv_desiredStep_feet"); data.dataPoint[1].value = 1;
                old_left_right = 1;
        }
        strcpy(data.dataPoint[2].name, "srv_desiredStep_x"); data.dataPoint[2].value = req.step.pose.position.x;
        strcpy(data.dataPoint[3].name, "srv_desiredStep_y"); data.dataPoint[3].value = req.step.pose.position.y;
  strcpy(data.dataPoint[4].name, "srv_desiredStep_z"); data.dataPoint[4].value = req.step.pose.position.z;
  strcpy(data.dataPoint[5].name, "srv_desiredStep_alpha"); data.dataPoint[5].value = roll;
  strcpy(data.dataPoint[6].name, "srv_desiredStep_beta"); data.dataPoint[6].value = pitch;
  strcpy(data.dataPoint[7].name, "srv_desiredStep_gamma"); data.dataPoint[7].value = yaw;

        data.length = 8;

        /* set timestamps */
        for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;

        //copy DataPointArray to vector
        std_msgs::UInt8MultiArray msgOut;
        //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(bipedRobin_msgs::StepTarget3DService::Request &req, bipedRobin_msgs::StepTarget3DService::Response &res) {
        ROS_INFO("Passing incremental step to robot");
        struct DataPointArray data;

        double roll;
        double pitch;
        double yaw;
        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);

        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;

        /* set timestamps */
        for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;

        //copy DataPointArray to vector
        std_msgs::UInt8MultiArray msgOut;
        //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 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(bipedRobin_msgs::SetModeService::Request &req, bipedRobin_msgs::SetModeService::Response &res) {
        ROS_INFO("Set Mode Service called. Desired Mode is set to %d", req.desiredMode);
        return sendParameterToRobot("srv_setDesiredMotionMode", req.desiredMode);
}

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

bool initialstep0Callback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
        ROS_INFO("Initialstep Service called");
        struct DataPointArray data;
        strcpy(data.dataPoint[0].name, "srv_desiredStep_flag"); data.dataPoint[0].value = 1;
        strcpy(data.dataPoint[1].name, "srv_desiredStep_feet"); data.dataPoint[1].value = 1;
        strcpy(data.dataPoint[2].name, "srv_desiredStep_x"); data.dataPoint[2].value = 0;
        strcpy(data.dataPoint[3].name, "srv_desiredStep_y"); data.dataPoint[3].value = 0;
        strcpy(data.dataPoint[4].name, "srv_desiredStep_z"); data.dataPoint[4].value = 0;
        strcpy(data.dataPoint[5].name, "srv_desiredStep_alpha"); data.dataPoint[5].value = 0;
        strcpy(data.dataPoint[6].name, "srv_desiredStep_beta"); data.dataPoint[6].value = 0;
        strcpy(data.dataPoint[7].name, "srv_desiredStep_gamma"); data.dataPoint[7].value = 0;
        data.length = 8;
        /* set timestamps */
        for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
        //copy DataPointArray to vector
        std_msgs::UInt8MultiArray msgOut;
        //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 initialstepCallback(bipedRobin_msgs::StepTarget3DService::Request &req, bipedRobin_msgs::StepTarget3DService::Response &res) {
        ROS_INFO("Initialstep Service called");
        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);
        strcpy(data.dataPoint[0].name, "srv_desiredStep_flag"); data.dataPoint[0].value = 1;
        strcpy(data.dataPoint[1].name, "srv_desiredStep_feet"); data.dataPoint[1].value = 1;
        strcpy(data.dataPoint[2].name, "srv_desiredStep_x"); data.dataPoint[2].value = req.step.pose.position.x;
        strcpy(data.dataPoint[3].name, "srv_desiredStep_y"); data.dataPoint[3].value = req.step.pose.position.y;
        strcpy(data.dataPoint[4].name, "srv_desiredStep_z"); data.dataPoint[4].value = req.step.pose.position.z;
  strcpy(data.dataPoint[5].name, "srv_desiredStep_alpha"); data.dataPoint[5].value = roll;
  strcpy(data.dataPoint[6].name, "srv_desiredStep_beta"); data.dataPoint[6].value = pitch;
  strcpy(data.dataPoint[7].name, "srv_desiredStep_gamma"); data.dataPoint[7].value = yaw;
        data.length = 8;
        for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
        std_msgs::UInt8MultiArray msgOut;
        for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
                msgOut.data.push_back(((char*)&data)[i]);
        }
        rtai_data.publish(msgOut);
        return true;
}


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 footstep3D_srv = n.advertiseService("footstep3D_srv", footstepCallback);
        ros::ServiceServer footstep3DInc_srv = n.advertiseService("footstep3DInc_srv", footstepIncCallback);
  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 initialstep_srv = n.advertiseService("initialstep0_srv", initialstep0Callback);
        ros::ServiceServer initialstep0_srv = n.advertiseService("initialstep_srv", initialstepCallback);
  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;
}