matlab_output.cpp
Go to the documentation of this file.
00001 
00015 /************************************************************************
00016  *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
00017  *      All rights reserved.                                                                                            *
00018  ************************************************************************
00019  *      Redistribution and use in source and binary forms, with or without      *
00020  *      modification, are permitted provided that the following conditions      *
00021  *      are met:                                                                                                                        *
00022  *                                                                                                                                              *
00023  *              1.      Redistributions of source code must retain the above            *
00024  *                      copyright notice, this list of conditions and the following *
00025  *                      disclaimer.                                                                                                     *
00026  *                                                                                                                                              *
00027  *              2.      Redistributions in binary form must reproduce the above         *
00028  *                      copyright notice, this list of conditions and the following *
00029  *                      disclaimer in the documentation and/or other materials          *
00030  *                      provided with the distribution.                                                         *
00031  *                                                                                                                                              *
00032  *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
00033  *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
00034  *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
00035  *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
00036  *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
00037  *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
00038  *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
00039  *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
00040  *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
00041  *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
00042  *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
00043  *      DAMAGE.                                                                                                                         *
00044  *                                                                                                                                              *
00045  *      The views and conclusions contained in the software and                         *
00046  *      documentation are those of the authors and should not be                        *
00047  *      interpreted as representing official policies, either expressed or      *
00048  *      implied, of TU/e.                                                                                                       *
00049  ************************************************************************/
00050 
00051 
00052 #include <ros/console.h> // for ROS_DEBUG and ROS_INFO messages, open rxconsole and click Levels > matlab_output > ros > debug > refresh to see them
00053 #include <ros/ros.h> 
00054 #include <nav_msgs/Odometry.h>
00055 #include <gazebo_msgs/ContactsState.h>
00056 #include "tulip_gazebo/xsens_state_message.h"
00057 #include "tulip_gazebo/footsens_state_message.h"
00058 #include "std_msgs/String.h" //for testing
00059 
00060 #define _USE_MATH_DEFINES
00061 #include <cmath>
00062 #include <tf/transform_broadcaster.h>
00063 
00064 // MUST READ:
00065 //http://ros-users.122217.n3.nabble.com/How-to-identify-the-subscriber-or-the-topic-name-related-to-a-callback-td2391327.html
00066 //http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
00067 
00068 // BACKGROUND:
00069 //http://ros-users.122217.n3.nabble.com/Subscriber-and-Publisher-node-at-once-in-cpp-td2651157.html
00070 //http://ros-users.122217.n3.nabble.com/Subscribing-and-publishing-Topics-td2763646.html
00071 //http://ros-users.122217.n3.nabble.com/question-about-callback-functions-td844156.html
00072 
00073 // SOLVING SEGMENTATION FAULTS
00074 //http://www.ros.org/wiki/roslaunch/Tutorials/Roslaunch%20Nodes%20in%20Valgrind%20or%20GDB
00075 
00076 class matlab_output_class 
00077 { 
00078   public: 
00079     
00080     std::vector<std::string> footsens_name;
00081     std::vector<ros::Subscriber> footsens_sub;
00082     ros::Subscriber xsens_sub;
00083     struct cnt_footsens_msg {
00084       int cnt; // count number of sensors collected
00085       tulip_gazebo::footsens_state_message msg; // actual message consisting of all sensor z-force elements
00086     };
00087     std::vector<cnt_footsens_msg> footsens_buffer;
00088     
00089     
00090     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
00091     { 
00092       footsens_buffer.reserve(30); // without this line you get a segmentation fault
00093       
00094       footsens_name.resize(8);
00095       footsens_name[0]  = "left_heel_inner";
00096       footsens_name[1]  = "left_heel_outer";
00097       footsens_name[2]  = "left_toe_inner";
00098       footsens_name[3]  = "left_toe_outer";
00099       footsens_name[4]  = "right_heel_inner";
00100       footsens_name[5]  = "right_heel_outer";
00101       footsens_name[6]  = "right_toe_inner";
00102       footsens_name[7]  = "right_toe_outer";
00103         
00104       footsens_sub.resize(8);
00105                   
00106       ros::NodeHandle n; 
00107       
00108       ros::Publisher     xsens_pub = n.advertise<tulip_gazebo::xsens_state_message>   ("matlab_xsens", 1000);
00109       ros::Publisher  footsens_pub = n.advertise<tulip_gazebo::footsens_state_message>("matlab_footsens", 1000);
00110       
00111       for (int i = 0; i < 8 ; i++){
00112         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));
00113       }
00114              
00115       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));
00116       
00117     } 
00118 
00119     void received_xsens_message_callback(const nav_msgs::OdometryConstPtr& inputmsg, const ros::Publisher& xsens_pub) 
00120     { 
00121       //ROS_INFO("received message from xsens");
00122       tulip_gazebo::xsens_state_message outputmsg;
00123   
00124       outputmsg.time   = inputmsg->header.stamp;
00125       outputmsg.pose   = inputmsg->pose.pose;
00126       outputmsg.twist  = inputmsg->twist.twist;
00127       
00128       xsens_pub.publish(outputmsg);
00129       ros::spinOnce();
00130      
00131     }  
00132     
00133     void received_footsens_message_callback(const gazebo_msgs::ContactsStateConstPtr& inputmsg, const ros::Publisher& footsens_pub, const int footsens_name_nr)  
00134     { 
00135       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. 
00136       
00137       for (unsigned int i=0; i<footsens_buffer.size(); i++) { // check all the filled elements, or if vector is empty then skip
00138         if (inputmsg->header.stamp == footsens_buffer[i].msg.time){ // if the time is already present in the vector
00139           buffer_write_i = i; // instead of the default writing position, remember to write at the position with the same time header
00140           //ROS_INFO("matched sensor %s with buffer position %d",footsens_name[footsens_name_nr].c_str(),i);
00141           break; // escape the for-loop
00142         }
00143       }
00144       
00145       if (buffer_write_i == footsens_buffer.size()) {
00146         footsens_buffer.resize(footsens_buffer.size()+1); // if necessary, create extra space at the end
00147         footsens_buffer[buffer_write_i].msg.time = inputmsg->header.stamp; // store the time at the writing position
00148         footsens_buffer[buffer_write_i].cnt = 0; //reset sensor counter
00149         //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());
00150       }
00151       
00152       double footsens_force_z;
00153       if (inputmsg->states.size()) { //THIS WAS THE SEGMENTATION FAULT!!!
00154         footsens_force_z = inputmsg->states[0].total_wrench.force.z; // access the z-force
00155       } else {
00156         footsens_force_z = 0.0;
00157         //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);
00158       }
00159        
00160         //Unlike the MATLAB switch statement, the C language switch falls through.
00161         //When a case statement is true, the other case statements are executed unless a break is used...
00162         switch (footsens_name_nr){ // find out in which sensor the force needs to be stored
00163           case 0: footsens_buffer[buffer_write_i].msg.left_heel_inner   = footsens_force_z; break;
00164           case 1: footsens_buffer[buffer_write_i].msg.left_heel_outer   = footsens_force_z; break;
00165           case 2: footsens_buffer[buffer_write_i].msg.left_toe_inner    = footsens_force_z; break;
00166           case 3: footsens_buffer[buffer_write_i].msg.left_toe_outer    = footsens_force_z; break;
00167           case 4: footsens_buffer[buffer_write_i].msg.right_heel_inner  = footsens_force_z; break;
00168           case 5: footsens_buffer[buffer_write_i].msg.right_heel_outer  = footsens_force_z; break;
00169           case 6: footsens_buffer[buffer_write_i].msg.right_toe_inner   = footsens_force_z; break;
00170           case 7: footsens_buffer[buffer_write_i].msg.right_toe_outer   = footsens_force_z; break;
00171         }
00172         /*
00173         std::cout << "sec = " << footsens_buffer[buffer_write_i].msg.time.sec
00174                   << "\nnsec = " << footsens_buffer[buffer_write_i].msg.time.nsec
00175                   << "\nlhi = " << footsens_buffer[buffer_write_i].msg.left_heel_inner
00176               << "\nlho = " << footsens_buffer[buffer_write_i].msg.left_heel_outer
00177                   << "\nlti = " << footsens_buffer[buffer_write_i].msg.left_toe_inner
00178               << "\nlto = " << footsens_buffer[buffer_write_i].msg.left_toe_outer
00179                   << "\nrhi = " << footsens_buffer[buffer_write_i].msg.right_heel_inner
00180                   << "\nrho = " << footsens_buffer[buffer_write_i].msg.right_heel_outer
00181                   << "\nrti = " << footsens_buffer[buffer_write_i].msg.right_toe_inner
00182                   << "\nrto = " << footsens_buffer[buffer_write_i].msg.right_toe_outer << std::endl;    */
00183         
00184       footsens_buffer[buffer_write_i].cnt++; //increment the number of sensors collected
00185       //ROS_INFO("Currently %d sensors collected at buffer position %d",footsens_buffer[buffer_write_i].cnt,buffer_write_i);
00186       if (footsens_buffer[buffer_write_i].cnt == 8) { // if all sensors are collected
00187         footsens_pub.publish(footsens_buffer[buffer_write_i].msg);
00188         ros::spinOnce();
00189         footsens_buffer.erase(footsens_buffer.begin()+buffer_write_i);
00190         //ROS_INFO("Published a set of sensor data on the matlab_footsens topic, new buffer size %d",footsens_buffer.size());
00191       }
00192       if (footsens_buffer.size()>4) {
00193         footsens_buffer.erase(footsens_buffer.begin());
00194         //ROS_INFO("Erased old buffer element of time %f sec, new buffer size %d",footsens_buffer[0].msg.time.toSec(),footsens_buffer.size());
00195       }
00196     }
00197 }; // end of class
00198 
00199 int main(int argc, char** argv) 
00200 { 
00201   ros::init(argc, argv, "matlab_output");
00202   matlab_output_class m; 
00203   ros::spin(); 
00204   return 0; 
00205 } 


tulip_gazebo
Author(s): Tim Assman, Pieter van Zutven
autogenerated on Sun Jan 26 2014 21:00:10