rtai_ros_bridge.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "std_msgs/UInt8MultiArray.h"

#include <mbx.h>


#ifndef TASKNAME
        #define TASKNAME "ROSTSK" 
#endif
#ifndef MAILBOX_NAME_IN
        #define MAILBOX_NAME_IN "DRIMBX" //"ROS_IN"
#endif
#ifndef MAILBOX_NAME_OUT
        #define MAILBOX_NAME_OUT "DROMBX" //"ROSOUT"
#endif
#ifndef SAMPLETIME
        #define SAMPLETIME 500   /* must correspond to base time of Simulink/RTW target */
#endif                           /* sample rate is 500Hz for bipedRobin */

static RT_TASK *mytask;

MBX *mbxI, *mbxO;
int closingMbxFlagI = 0;
int closingMbxFlagO = 0;

int initializeRTMBX(void);
int terminateRTMBX(void);


void rtaiDataCallback(const std_msgs::UInt8MultiArray rtaiData) {
  int state=0;
  char dataIn[100000];
  ROS_INFO("Received MSG size to be sent: %d", rtaiData.data.size());
  copy(rtaiData.data.begin(), rtaiData.data.end(), (char*)&dataIn);
  if ((state = sendViaMbx(mbxI, (void*)&dataIn, rtaiData.data.size()))) {
    ROS_INFO("problem when sending via mbx %d", state);
  }
}

int main(int argc, char **argv)
{
  /* init ROS */
  ros::init(argc, argv, "ros_rtai_bridge_publisher");
  ros::NodeHandle n;
  ros::Publisher rtai_dataOut = n.advertise<std_msgs::UInt8MultiArray>("data_from_rtai", 1000, true);
  ros::Subscriber rtai_dataIn = n.subscribe<std_msgs::UInt8MultiArray>("data_to_rtai", 1000, rtaiDataCallback);
  ros::Rate loop_rate(SAMPLETIME); 

  /* init RTAI */
  ROS_INFO("Starting ROS RTAI bridge ...");
  if (initializeRTMBX() <= 0) {
    ROS_INFO("Error creating MBX Connection");
    ros::shutdown();
  }

  char dataOut[100000];
  ROS_INFO("Entering the loop ...");
  while (ros::ok()) { // SIGINT causes ros::ok() to be false 
    int count=0;
    count = receiveViaMbx(mbxO, (void*) &dataOut, sizeof(dataOut));

    if (count > 0) { //publish received data to ROS
      std_msgs::UInt8MultiArray msgOut;
      //TODO: fill out header of array!
      for (int i=0; i<count; i++) {
        msgOut.data.push_back(dataOut[i]);
      }
      rtai_dataOut.publish(msgOut);
    }

    ros::spinOnce();
    loop_rate.sleep();
  }

  ROS_INFO("Terminating ROS RTAI bridge ...");
  terminateRTMBX();

  return 0;
}

int initializeRTMBX(void) {
        rt_allow_nonroot_hrt();

        if (!(mytask = rt_task_init(nam2num(TASKNAME), 100, 0, 0))) {
                ROS_INFO("Error creating task TASK");
                return -1;
        }
        ROS_INFO("Initialized RTAI task...");

        mlockall(MCL_CURRENT|MCL_FUTURE);

        //opening MBX_IN
        if (openExistingMbx(&mbxI, MAILBOX_NAME_IN)) {
                ROS_INFO("Mailbox DRIMBX not opened yet, going to open mailbox ...");
                closingMbxFlagI = 1;
                if (openNewMbxBuf(&mbxI, MAILBOX_NAME_IN, 4804, 100)) {
                        ROS_INFO("Could not open mailbox DRIMBX");
                        return -1;
                } else {
                        ROS_INFO("Opened mailbox IN...");
                }
        } else {
                ROS_INFO("Mailbox IN was already opened ...");
        }

        //opening MBX_OUT
        if (openExistingMbx(&mbxO, MAILBOX_NAME_OUT)) {
                ROS_INFO("Mailbox DROMBX not opened yet, going to open mailbox ...");
                closingMbxFlagO = 1;
                if (openNewMbxBuf(&mbxO, MAILBOX_NAME_OUT, 4804, 100)) {
                        ROS_INFO("Could not open mailbox DROMBX");
                        return -1;
                } else {
                        ROS_INFO("Opened mailbox OUT...");
                }

        } else {
                ROS_INFO("Mailbox OUT was already opened ...");
        }

        return 1;
}

int terminateRTMBX(void) {
        if (closingMbxFlagI) {
                ROS_INFO("Closing MBX I ...");
                closeMbx(mbxI);
        }
        if (closingMbxFlagO) {
                ROS_INFO("Closing MBX O ...");
                closeMbx(mbxO);
        }
        munlockall();
        rt_task_delete(mytask);
        ROS_INFO("Deleted RTAI task...\n");
        return 1;
}