matlab_output.cpp

Go to the documentation of this file.

/************************************************************************
 *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
 *      All rights reserved.                                                                                            *
 ************************************************************************
 *      Redistribution and use in source and binary forms, with or without      *
 *      modification, are permitted provided that the following conditions      *
 *      are met:                                                                                                                        *
 *                                                                                                                                              *
 *              1.      Redistributions of source code must retain the above            *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer.                                                                                                     *
 *                                                                                                                                              *
 *              2.      Redistributions in binary form must reproduce the above         *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer in the documentation and/or other materials          *
 *                      provided with the distribution.                                                         *
 *                                                                                                                                              *
 *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
 *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
 *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
 *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
 *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
 *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
 *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
 *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
 *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
 *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
 *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
 *      DAMAGE.                                                                                                                         *
 *                                                                                                                                              *
 *      The views and conclusions contained in the software and                         *
 *      documentation are those of the authors and should not be                        *
 *      interpreted as representing official policies, either expressed or      *
 *      implied, of TU/e.                                                                                                       *
 ************************************************************************/


#include <ros/console.h> // for ROS_DEBUG and ROS_INFO messages, open rxconsole and click Levels > matlab_output > ros > debug > refresh to see them
#include <ros/ros.h> 
#include <nav_msgs/Odometry.h>
#include <gazebo_msgs/ContactsState.h>
#include "tulip_gazebo/xsens_state_message.h"
#include "tulip_gazebo/footsens_state_message.h"
#include "std_msgs/String.h" //for testing

#define _USE_MATH_DEFINES
#include <cmath>
#include <tf/transform_broadcaster.h>

// MUST READ:
//http://ros-users.122217.n3.nabble.com/How-to-identify-the-subscriber-or-the-topic-name-related-to-a-callback-td2391327.html
//http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

// BACKGROUND:
//http://ros-users.122217.n3.nabble.com/Subscriber-and-Publisher-node-at-once-in-cpp-td2651157.html
//http://ros-users.122217.n3.nabble.com/Subscribing-and-publishing-Topics-td2763646.html
//http://ros-users.122217.n3.nabble.com/question-about-callback-functions-td844156.html

// SOLVING SEGMENTATION FAULTS
//http://www.ros.org/wiki/roslaunch/Tutorials/Roslaunch%20Nodes%20in%20Valgrind%20or%20GDB

class matlab_output_class 
{ 
  public: 
    
    std::vector<std::string> footsens_name;
    std::vector<ros::Subscriber> footsens_sub;
    ros::Subscriber xsens_sub;
    struct cnt_footsens_msg {
      int cnt; // count number of sensors collected
      tulip_gazebo::footsens_state_message msg; // actual message consisting of all sensor z-force elements
    };
    std::vector<cnt_footsens_msg> footsens_buffer;
    
    
    matlab_output_class() //automatically called whenever a new object of this class is created. This constructor function must have the same name as the class
    { 
      footsens_buffer.reserve(30); // without this line you get a segmentation fault
      
      footsens_name.resize(8);
      footsens_name[0]  = "left_heel_inner";
      footsens_name[1]  = "left_heel_outer";
      footsens_name[2]  = "left_toe_inner";
      footsens_name[3]  = "left_toe_outer";
      footsens_name[4]  = "right_heel_inner";
      footsens_name[5]  = "right_heel_outer";
      footsens_name[6]  = "right_toe_inner";
      footsens_name[7]  = "right_toe_outer";
        
      footsens_sub.resize(8);
                  
      ros::NodeHandle n; 
      
      ros::Publisher     xsens_pub = n.advertise<tulip_gazebo::xsens_state_message>   ("matlab_xsens", 1000);
      ros::Publisher  footsens_pub = n.advertise<tulip_gazebo::footsens_state_message>("matlab_footsens", 1000);
      
      for (int i = 0; i < 8 ; i++){
        footsens_sub[i] = n.subscribe<gazebo_msgs::ContactsState>(footsens_name[i] + "_foot_sensor/state", 1000, boost::bind(&matlab_output_class::received_footsens_message_callback, this, _1, footsens_pub, i));
      }
             
      xsens_sub = n.subscribe<nav_msgs::Odometry>("xsens_pose_ground_truth", 1000, boost::bind(&matlab_output_class::received_xsens_message_callback, this, _1, xsens_pub));
      
    } 

    void received_xsens_message_callback(const nav_msgs::OdometryConstPtr& inputmsg, const ros::Publisher& xsens_pub) 
    { 
      //ROS_INFO("received message from xsens");
      tulip_gazebo::xsens_state_message outputmsg;
  
      outputmsg.time   = inputmsg->header.stamp;
      outputmsg.pose   = inputmsg->pose.pose;
      outputmsg.twist  = inputmsg->twist.twist;
      
      xsens_pub.publish(outputmsg);
      ros::spinOnce();
     
    }  
    
    void received_footsens_message_callback(const gazebo_msgs::ContactsStateConstPtr& inputmsg, const ros::Publisher& footsens_pub, const int footsens_name_nr)  
    { 
      unsigned int buffer_write_i = footsens_buffer.size(); // by default, add a new element at the end of the vector, after its current last element. 
      
      for (unsigned int i=0; i<footsens_buffer.size(); i++) { // check all the filled elements, or if vector is empty then skip
        if (inputmsg->header.stamp == footsens_buffer[i].msg.time){ // if the time is already present in the vector
          buffer_write_i = i; // instead of the default writing position, remember to write at the position with the same time header
          //ROS_INFO("matched sensor %s with buffer position %d",footsens_name[footsens_name_nr].c_str(),i);
          break; // escape the for-loop
        }
      }
      
      if (buffer_write_i == footsens_buffer.size()) {
        footsens_buffer.resize(footsens_buffer.size()+1); // if necessary, create extra space at the end
        footsens_buffer[buffer_write_i].msg.time = inputmsg->header.stamp; // store the time at the writing position
        footsens_buffer[buffer_write_i].cnt = 0; //reset sensor counter
        //ROS_INFO("created new element for %s at position %d in the footsens_buffer, capacity %d",footsens_name[footsens_name_nr].c_str(),buffer_write_i, footsens_buffer.capacity());
      }
      
      double footsens_force_z;
      if (inputmsg->states.size()) { //THIS WAS THE SEGMENTATION FAULT!!!
        footsens_force_z = inputmsg->states[0].total_wrench.force.z; // access the z-force
      } else {
        footsens_force_z = 0.0;
        //ROS_INFO("this footsens message from %s has no state data, stored force_z=0 at buffer position %d",footsens_name[footsens_name_nr].c_str(),buffer_write_i);
      }
       
        //Unlike the MATLAB switch statement, the C language switch falls through.
        //When a case statement is true, the other case statements are executed unless a break is used...
        switch (footsens_name_nr){ // find out in which sensor the force needs to be stored
          case 0: footsens_buffer[buffer_write_i].msg.left_heel_inner   = footsens_force_z; break;
          case 1: footsens_buffer[buffer_write_i].msg.left_heel_outer   = footsens_force_z; break;
          case 2: footsens_buffer[buffer_write_i].msg.left_toe_inner    = footsens_force_z; break;
          case 3: footsens_buffer[buffer_write_i].msg.left_toe_outer    = footsens_force_z; break;
          case 4: footsens_buffer[buffer_write_i].msg.right_heel_inner  = footsens_force_z; break;
          case 5: footsens_buffer[buffer_write_i].msg.right_heel_outer  = footsens_force_z; break;
          case 6: footsens_buffer[buffer_write_i].msg.right_toe_inner   = footsens_force_z; break;
          case 7: footsens_buffer[buffer_write_i].msg.right_toe_outer   = footsens_force_z; break;
        }
        /*
        std::cout << "sec = " << footsens_buffer[buffer_write_i].msg.time.sec
                  << "\nnsec = " << footsens_buffer[buffer_write_i].msg.time.nsec
                  << "\nlhi = " << footsens_buffer[buffer_write_i].msg.left_heel_inner
              << "\nlho = " << footsens_buffer[buffer_write_i].msg.left_heel_outer
                  << "\nlti = " << footsens_buffer[buffer_write_i].msg.left_toe_inner
              << "\nlto = " << footsens_buffer[buffer_write_i].msg.left_toe_outer
                  << "\nrhi = " << footsens_buffer[buffer_write_i].msg.right_heel_inner
                  << "\nrho = " << footsens_buffer[buffer_write_i].msg.right_heel_outer
                  << "\nrti = " << footsens_buffer[buffer_write_i].msg.right_toe_inner
                  << "\nrto = " << footsens_buffer[buffer_write_i].msg.right_toe_outer << std::endl;    */
        
      footsens_buffer[buffer_write_i].cnt++; //increment the number of sensors collected
      //ROS_INFO("Currently %d sensors collected at buffer position %d",footsens_buffer[buffer_write_i].cnt,buffer_write_i);
      if (footsens_buffer[buffer_write_i].cnt == 8) { // if all sensors are collected
        footsens_pub.publish(footsens_buffer[buffer_write_i].msg);
        ros::spinOnce();
        footsens_buffer.erase(footsens_buffer.begin()+buffer_write_i);
        //ROS_INFO("Published a set of sensor data on the matlab_footsens topic, new buffer size %d",footsens_buffer.size());
      }
      if (footsens_buffer.size()>4) {
        footsens_buffer.erase(footsens_buffer.begin());
        //ROS_INFO("Erased old buffer element of time %f sec, new buffer size %d",footsens_buffer[0].msg.time.toSec(),footsens_buffer.size());
      }
    }
}; // end of class

int main(int argc, char** argv) 
{ 
  ros::init(argc, argv, "matlab_output");
  matlab_output_class m; 
  ros::spin(); 
  return 0; 
}