dsa_only.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


// ##################
// #### includes ####
// standard includes
#include <unistd.h>
#include <string>
#include <vector>

// ROS includes
#include <ros/ros.h>

// ROS message includes
#include <schunk_sdh/TactileSensor.h>
#include <schunk_sdh/TactileMatrix.h>

// ROS diagnostic msgs
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/KeyValue.h>

#include <schunk_sdh/dsa.h>

#include <boost/lexical_cast.hpp>
#include <boost/bind.hpp>

template<typename T>
  bool read_vector(ros::NodeHandle &n_, const std::string &key, std::vector<T> & res)
  {
    XmlRpc::XmlRpcValue namesXmlRpc;
    if (!n_.hasParam(key))
    {
      return false;
    }

    n_.getParam(key, namesXmlRpc);
    res.resize(namesXmlRpc.size());
    for (int i = 0; i < namesXmlRpc.size(); i++)
    {
      res[i] = (T)namesXmlRpc[i];
    }
    return true;
  }

class DsaNode
{
public:
  ros::NodeHandle nh_;
private:
  // declaration of topics to publish
  ros::Publisher topicPub_TactileSensor_;
  ros::Publisher topicPub_Diagnostics_;

  // topic subscribers

  // service servers

  // actionlib server

  // service clients
  // --

  // other variables
  SDH::cDSA *dsa_;
  SDH::UInt32 last_data_publish_;  // time stamp of last data publishing

  std::string dsadevicestring_;
  int dsadevicenum_;
  int maxerror_;  // maximum error count allowed

  bool isDSAInitialized_;
  int error_counter_;
  bool polling_;  // try to publish on each response
  bool auto_publish_;
  bool use_rle_;
  bool debug_;
  double frequency_;

  ros::Timer timer_dsa, timer_publish, timer_diag;

  std::vector<int> dsa_reorder_;
public:
  DsaNode() :
      nh_("~"), dsa_(0), last_data_publish_(0), isDSAInitialized_(false), error_counter_(0)
  {
    topicPub_Diagnostics_ = nh_.advertise < diagnostic_msgs::DiagnosticArray > ("/diagnostics", 1);
    topicPub_TactileSensor_ = nh_.advertise < schunk_sdh::TactileSensor > ("tactile_data", 1);
  }

  ~DsaNode()
  {
    if (isDSAInitialized_)
      dsa_->Close();
    if (dsa_)
      delete dsa_;
  }

  void shutdown()
  {
    timer_dsa.stop();
    timer_publish.stop();
    timer_diag.stop();
    nh_.shutdown();
  }

  bool init()
  {
    // implementation of topics to publish

    nh_.param("dsadevicestring", dsadevicestring_, std::string(""));
    if (dsadevicestring_.empty())
      return false;

    nh_.param("dsadevicenum", dsadevicenum_, 0);
    nh_.param("maxerror", maxerror_, 8);

    double publish_frequency, diag_frequency;

    nh_.param("debug", debug_, false);
    nh_.param("polling", polling_, false);
    nh_.param("use_rle", use_rle_, true);
    nh_.param("diag_frequency", diag_frequency, 5.0);
    frequency_ = 30.0;
    if (polling_)
      nh_.param("poll_frequency", frequency_, 5.0);
    nh_.param("publish_frequency", publish_frequency, 0.0);

    auto_publish_ = true;

    if (polling_)
    {
      timer_dsa = nh_.createTimer(ros::Rate(frequency_).expectedCycleTime(), boost::bind(&DsaNode::pollDsa, this));
    }
    else
    {
      timer_dsa = nh_.createTimer(ros::Rate(frequency_ * 2.0).expectedCycleTime(),
                                  boost::bind(&DsaNode::readDsaFrame, this));
      if (publish_frequency > 0.0)
      {
        auto_publish_ = false;
        timer_publish = nh_.createTimer(ros::Rate(publish_frequency).expectedCycleTime(),
                                        boost::bind(&DsaNode::publishTactileData, this));
      }
    }

    timer_diag = nh_.createTimer(ros::Rate(diag_frequency).expectedCycleTime(),
                                 boost::bind(&DsaNode::publishDiagnostics, this));

    if (!read_vector(nh_, "dsa_reorder", dsa_reorder_))
    {
      dsa_reorder_.resize(6);
      dsa_reorder_[0] = 2;  // t1
      dsa_reorder_[1] = 3;  // t2
      dsa_reorder_[2] = 4;  // f11
      dsa_reorder_[3] = 5;  // f12
      dsa_reorder_[4] = 0;  // f21
      dsa_reorder_[5] = 1;  // f22
    }

    return true;
  }
  bool stop()
  {
    if (dsa_)
    {
      if (isDSAInitialized_)
        dsa_->Close();
      delete dsa_;
    }
    dsa_ = 0;
    isDSAInitialized_ = false;
    return true;
  }

  bool start()
  {
    if (isDSAInitialized_ == false)
    {
      // Init tactile data
      if (!dsadevicestring_.empty())
      {
        try
        {
          dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str());
          if (!polling_)
            dsa_->SetFramerate(frequency_, use_rle_);
          else
            dsa_->SetFramerate(0, use_rle_);

          ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s", dsadevicestring_.c_str());
          // ROS_INFO("Set sensitivity to 1.0");
          // for(int i=0; i<6; i++)
          //  dsa_->SetMatrixSensitivity(i, 1.0);
          error_counter_ = 0;
          isDSAInitialized_ = true;
        }
        catch (SDH::cSDHLibraryException* e)
        {
          isDSAInitialized_ = false;
          ROS_ERROR("An exception was caught: %s", e->what());
          delete e;

          shutdown();
          return false;
        }
      }
    }

    return true;
  }

  void readDsaFrame()
  {
    if (debug_)
      ROS_DEBUG("readDsaFrame");

    if (isDSAInitialized_)
    {
      try
      {
        SDH::UInt32 last_time;
        last_time = dsa_->GetFrame().timestamp;
        dsa_->UpdateFrame();
        if (last_time != dsa_->GetFrame().timestamp)
        {
          // new data
          if (error_counter_ > 0)
            --error_counter_;
          if (auto_publish_)
            publishTactileData();
        }
      }
      catch (SDH::cSDHLibraryException* e)
      {
        ROS_ERROR("An exception was caught: %s", e->what());
        delete e;
        ++error_counter_;
      }
      if (error_counter_ > maxerror_)
        stop();
    }
    else
    {
      start();
    }
  }

  void pollDsa()
  {
    if (debug_)
      ROS_DEBUG("pollDsa");

    if (isDSAInitialized_)
    {
      try
      {
        dsa_->SetFramerate(0, use_rle_);
        readDsaFrame();
      }
      catch (SDH::cSDHLibraryException* e)
      {
        ROS_ERROR("An exception was caught: %s", e->what());
        delete e;
        ++error_counter_;
      }
      if (error_counter_ > maxerror_)
        stop();
    }
    else
    {
      start();
    }
  }

  void publishTactileData()
  {
    if (debug_)
      ROS_DEBUG("publishTactileData %ul %ul", dsa_->GetFrame().timestamp, last_data_publish_);
    if (!isDSAInitialized_ || dsa_->GetFrame().timestamp == last_data_publish_)
      return;  // no new frame available
    last_data_publish_ = dsa_->GetFrame().timestamp;

    schunk_sdh::TactileSensor msg;
    msg.header.stamp = ros::Time::now();
    int m, x, y;
    msg.tactile_matrix.resize(dsa_->GetSensorInfo().nb_matrices);
    ROS_ASSERT(dsa_->GetSensorInfo().nb_matrices == dsa_reorder_.size());
    for (unsigned int i = 0; i < dsa_reorder_.size(); i++)
    {
      m = dsa_reorder_[i];
      schunk_sdh::TactileMatrix &tm = msg.tactile_matrix[i];
      tm.matrix_id = i;
      tm.cells_x = dsa_->GetMatrixInfo(m).cells_x;
      tm.cells_y = dsa_->GetMatrixInfo(m).cells_y;
      tm.tactile_array.resize(tm.cells_x * tm.cells_y);
      for (y = 0; y < tm.cells_y; y++)
      {
        for (x = 0; x < tm.cells_x; x++)
          tm.tactile_array[tm.cells_x * y + x] = dsa_->GetTexel(m, x, y);
      }
    }
    // publish matrix
    topicPub_TactileSensor_.publish(msg);
  }
  void publishDiagnostics()
  {
    // publishing diagnotic messages
    diagnostic_msgs::DiagnosticArray diagnostics;
    diagnostics.status.resize(1);
    diagnostics.status[0].name = nh_.getNamespace();
    diagnostics.status[0].values.resize(1);
    diagnostics.status[0].values[0].key = "error_count";
    diagnostics.status[0].values[0].value = boost::lexical_cast < std::string > (error_counter_);

    // set data to diagnostics
    if (isDSAInitialized_)
    {
      diagnostics.status[0].level = 0;
      diagnostics.status[0].message = "DSA tactile sensing initialized and running";
    }
    else if (error_counter_ == 0)
    {
      diagnostics.status[0].level = 1;
      diagnostics.status[0].message = "DSA not initialized";
    }
    else
    {
      diagnostics.status[0].level = 2;
      diagnostics.status[0].message = "DSA exceeded eror count";
    }
    // publish diagnostic message
    topicPub_Diagnostics_.publish(diagnostics);
    if (debug_)
      ROS_DEBUG_STREAM("publishDiagnostics " << diagnostics);
  }
};
// DsaNode

int main(int argc, char** argv)
{
  // initialize ROS, spezify name of node
  ros::init(argc, argv, "schunk_dsa");

  DsaNode dsa_node;

  if (!dsa_node.init())
    return 0;

  dsa_node.start();

  ROS_INFO("...dsa node running...");

  ros::spin();

  ROS_INFO("...dsa node shut down...");
  return 0;
}