virtual_arm_node.cpp

Go to the documentation of this file.

#include <ros/ros.h>

#include <boost/thread.hpp>
#include <boost/smart_ptr.hpp>

#include "sr_hand/sr_subscriber.h"
#include "sr_hand/sr_publisher.h"
#include "sr_hand/sr_diagnosticer.h"
//#include "shadowhand/shadowhand_config_server.h"
#include "sr_hand/hand/virtual_arm.h"

using namespace std;
using namespace ros;
using namespace shadowrobot;
//using namespace shadowhand_config_server;

//           MAIN              //

void run_diagnotics(boost::shared_ptr<SRDiagnosticer> shadowhand_diag)
{
  while( ok() )
    shadowhand_diag->publish();
}

void run_publisher(boost::shared_ptr<SRPublisher> shadowhand_pub)
{
  while( ok() )
    shadowhand_pub->publish();
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "shadowarm");
  NodeHandle n;

  boost::shared_ptr<VirtualArm> virt_arm( new VirtualArm() );
  boost::shared_ptr<SRSubscriber> shadowhand_subscriber(new SRSubscriber(virt_arm));

  boost::shared_ptr<SRPublisher> shadowhand_pub( new SRPublisher(virt_arm));
  boost::shared_ptr<SRDiagnosticer> shadowhand_diag( new SRDiagnosticer(virt_arm, sr_arm_hardware));

  boost::thread thrd1( boost::bind( &run_diagnotics, shadowhand_diag ));
  boost::thread thrd2( boost::bind( &run_publisher, shadowhand_pub ));
  thrd1.join();
  thrd2.join();

  return 0;
}