tibi_dabo_laser_3d_driver_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_laser_3d_driver_node.h"

TibiDaboLaser3dDriverNode::TibiDaboLaser3dDriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<TibiDaboLaser3dDriver>(nh)
{
  //init class attributes if necessary
  this->loop_rate_ = 100;//in [Hz]

  // [init publishers]
  this->assembled_scans_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("assembled_scans", 10);
  this->joint_states_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("joint_states", 10);

  this->JointState_msg_.name.resize(1);
  this->JointState_msg_.name[0] = "servo_laser";
  this->JointState_msg_.position.resize(1);
  this->JointState_msg_.position[0]=0.0;
  this->JointState_msg_.velocity.resize(1);
  this->JointState_msg_.velocity[0]=0.0;
  this->JointState_msg_.effort.resize(1);
  this->JointState_msg_.effort[0]=0.0;
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  assemble_scans_client_ = this->public_node_handle_.serviceClient<laser_assembler::AssembleScans>("assemble_scans");
  
  // [init action servers]
  
  // [init action clients]
}

void TibiDaboLaser3dDriverNode::mainNodeThread(void)
{
  static ros::Time previous_time=ros::Time::now();
  static bool first=true;

  //lock access to driver if necessary
  this->driver_.lock();

  // [fill msg Header if necessary]
  //<publisher_name>.header.stamp = ros::Time::now();
  //<publisher_name>.header.frame_id = "<publisher_topic_name>";

  // [fill msg structures]
  //this->PointCloud_msg_.data = my_var;
  //this->JointState_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  //assemble_scans_srv_.request.data = my_var; 
  //ROS_INFO("TibiDaboLaser3dDriverNode:: Sending New Request!"); 
  //if (assemble_scans_client_.call(assemble_scans_srv_)) 
  //{ 
    //ROS_INFO("TibiDaboLaser3dDriverNode:: Response: %s", assemble_scans_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("TibiDaboLaser3dDriverNode:: Failed to Call Server on topic assemble_scans "); 
  //}
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  //this->assembled_scans_publisher_.publish(this->PointCloud_msg_);
  this->JointState_msg_.header.stamp=ros::Time::now();
  this->JointState_msg_.position[0]=-this->driver_.get_current_angle()*3.14159/180.0;
  this->JointState_msg_.velocity[0]=this->driver_.get_current_velocity()*3.14159/180.0;
  this->joint_states_publisher_.publish(this->JointState_msg_);
  if(this->driver_.is_scan_enabled())
  {
    if(this->driver_.is_scan_done())
    {
      assemble_scans_srv_.request.begin = previous_time;
      assemble_scans_srv_.request.end   = ros::Time::now();
      if (assemble_scans_client_.call(assemble_scans_srv_))
        this->assembled_scans_publisher_.publish(assemble_scans_srv_.response.cloud);
      else
        ROS_ERROR("TibiDaboLaser2dTo3dAlgNode:: Failed to Call Server on topic assemble_scans ");
      previous_time=ros::Time::now();
      this->driver_.start_scan();
    }
    if(!first)
      first=true;
  }
  else
  {
    if(first)
    {
      first=false;
      this->driver_.move_absolute_angle(this->driver_.get_fixed_angle());
    }    
  }

  //unlock access to driver if previously blocked
  this->driver_.unlock();
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void TibiDaboLaser3dDriverNode::postNodeOpenHook(void)
{
}

void TibiDaboLaser3dDriverNode::addNodeDiagnostics(void)
{
}

void TibiDaboLaser3dDriverNode::addNodeOpenedTests(void)
{
}

void TibiDaboLaser3dDriverNode::addNodeStoppedTests(void)
{
}

void TibiDaboLaser3dDriverNode::addNodeRunningTests(void)
{
}

void TibiDaboLaser3dDriverNode::reconfigureNodeHook(int level)
{
}

TibiDaboLaser3dDriverNode::~TibiDaboLaser3dDriverNode(void)
{
  // [free dynamic memory]
}

/* main function */
int main(int argc,char *argv[])
{
  return driver_base::main<TibiDaboLaser3dDriverNode>(argc, argv, "tibi_dabo_laser_3d_driver_node");
}