skin_dashboard: skin.cpp Source File

00001 /*
00002  * Copyright (c) 2010, Bosch LLC
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Bosch LLC nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 //\Author Joerg Wagner, Bosch LLC
00031 
00032 #include "skin_dashboard/skin.h"
00033 
00034 //wxwidgets stuff
00035 #include <wx/wx.h>
00036 
00037 Skin::Skin()
00038 {
00039         //initialize data
00040         initialize_sensor_data();//function to initialize the sensor data
00041         dtime_subscription_ = ros::Time::now();
00042         bsens_num_changed = false;
00043 
00044     //ROS Subscriber
00045     skin_sub_ = nh_.subscribe("skin_data", 100, &Skin::skin_Callback, this);
00046 }
00047 
00048 void Skin::initialize_sensor_data()//function to initialize the sensor data
00049 {
00050         //initialize data
00051         u8sensor_number_        = skind_def::max_sensor_number;
00052         u8readadr_hi_ = 1; //hardware defined values
00053         u8readadr_lo_ = 0;
00054         bstatus_go_ = 0;
00055         bstatus_err_ = 1;
00056     for(int i=0; i<skind_def::max_sensor_number; i++)
00057         {
00058         for(int j=0; j<skind_def::anz_byte_per_sensor; j++)
00059                 {
00060                 u8asensor_data_[i][j] = 0;
00061                 }
00062         }
00063 }
00064 
00065 void Skin::skin_Callback(const boost::shared_ptr<skin_driver::skin_meas const>& msg)
00066 {
00067         //check if sensor number changed
00068         if(u8sensor_number_ != msg->sensor_number_msg)
00069         {
00070                 bsens_num_changed = true;//sensor number was changed to a smaller value => GUI needs to be initialized (if the gui is not initialized => old data of the no longer existing sensors are still displayed)
00071         }
00072         u8sensor_number_ = msg->sensor_number_msg;
00073         u8readadr_hi_ = msg->readadr_hi_msg;
00074         u8readadr_lo_ = msg->readadr_lo_msg;
00075         bstatus_go_ = msg->status_go_msg;
00076         bstatus_err_ = msg->status_err_msg;
00077 
00078         //Converting a 1 dimensional uint64 array into a 2 dimensional uint8 array
00079         std::cout << "skin_Callback" << " \n";
00080 
00081         for(int i = 0; i < skind_def::max_sensor_number; i++)//convert received sensor data into 2 dimensional uint8_t array
00082         {
00083                 for(int j = 0; j < skind_def::anz_byte_per_sensor ; j++)
00084                 {
00085                         u8asensor_data_[i][j] = ( (255) & (msg->sensor_data_msg[i] >> (8 * j)) );
00086                 }
00087         }
00088 
00089         dtime_subscription_ = ros::Time::now();
00090 }