tibi_dabo_laser_2d_to_3d_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_laser_2d_to_3d_alg_node.h"

TibiDaboLaser2dTo3dAlgNode::TibiDaboLaser2dTo3dAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboLaser2dTo3dAlgorithm>(),
  servo_angle_client_("servo_angle", true)
{
  //init class attributes if necessary
  this->loop_rate_ = 20;//in [Hz]

  // [init publishers]
  this->vertical_laser_pan_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("vertical_laser_pan", 100);
  this->pointcloud_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("pointcloud", 100);
  
  this->JointState_msg_.name.resize(1);
  this->JointState_msg_.name[0] = "base_link2vertical_laser";
  this->JointState_msg_.position.resize(1); 
  
  // [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]
  
  this->public_node_handle_.getParam("scan_vel", scan_vel);
  this->public_node_handle_.getParam("min_angle", angle_min);
  this->public_node_handle_.getParam("max_angle", angle_max);
  
  angle=0.0;
  angle_margin=3.0;
  previous_time = ros::Time::now();
  minimum_time_between_scans = ros::Duration(1/900.0*scan_vel*scan_vel-7.0/60.0*scan_vel+3.5);
  
  
  this->servo_angle_goal_.trajectory.joint_names.resize(1);
  this->servo_angle_goal_.trajectory.points.resize(1);
  this->servo_angle_goal_.trajectory.points[0].positions.resize(1);
  this->servo_angle_goal_.trajectory.points[0].velocities.resize(1);
  
  this->servo_angle_goal_.trajectory.joint_names[0]="vertical_laser_servo";
  this->servo_angle_goal_.trajectory.points[0].positions[0]=angle_min-angle_margin;
  this->servo_angle_goal_.trajectory.points[0].velocities[0]=scan_vel;
  
  servo_angleMakeActionRequest();
  servo_going_right=false;
  
}

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

void TibiDaboLaser2dTo3dAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->JointState_msg_.data = my_var;
  //this->PointCloud_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  
  
  
  //When servo arrives at boundaries, publish assembled scans
  if((angle>angle_max || angle<angle_min) && (ros::Time::now()-previous_time>minimum_time_between_scans)){
    
    assemble_scans_srv_.request.begin = previous_time;
    assemble_scans_srv_.request.end   = ros::Time::now();
    previous_time = ros::Time::now();
    
    //ROS_INFO("TibiDaboLaser2dTo3dAlgNode:: Sending New Request!"); 
    if (assemble_scans_client_.call(assemble_scans_srv_)) 
    { 
      //ROS_INFO("TibiDaboLaser2dTo3dAlgNode:: Response: %s", assemble_scans_srv_.response.result);
      //ROS_INFO("Scan assembled and published");
      this->pointcloud_publisher_.publish(assemble_scans_srv_.response.cloud);
    } 
    else 
    { 
      ROS_ERROR("TibiDaboLaser2dTo3dAlgNode:: Failed to Call Server on topic assemble_scans "); 
    }
    
    //Move servo
    if(angle>angle_max){
      this->servo_angle_goal_.trajectory.points[0].positions[0]=angle_min-angle_margin;
      servo_going_right=false;
    }else if(angle<angle_min){
      this->servo_angle_goal_.trajectory.points[0].positions[0]=angle_max+angle_margin;
      servo_going_right=true;
    }
    
    this->servo_angle_goal_.trajectory.points[0].velocities[0]=scan_vel;
    //ROS_INFO("Moving servo");
    servo_angleMakeActionRequest();
  }else if(ros::Time::now()-previous_time>minimum_time_between_scans){}

  
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  
  
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */
void TibiDaboLaser2dTo3dAlgNode::servo_angleDone(const actionlib::SimpleClientGoalState& state,  const control_msgs::FollowJointTrajectoryResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiDaboLaser2dTo3dAlgNode::servo_angleDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiDaboLaser2dTo3dAlgNode::servo_angleDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
} 

void TibiDaboLaser2dTo3dAlgNode::servo_angleActive() 
{ 
  //ROS_INFO("TibiDaboLaser2dTo3dAlgNode::servo_angleActive: Goal just went active!"); 
} 

void TibiDaboLaser2dTo3dAlgNode::servo_angleFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiDaboLaser2dTo3dAlgNode::servo_angleFeedback: Got Feedback!"); 

  //bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var; 

  //if feedback is not what expected, cancel requested goal 
  //if( !feedback_is_ok ) 
  //{ 
    //servo_angle_client_.cancelGoal(); 
    //ROS_INFO("TibiDaboLaser2dTo3dAlgNode::servo_angleFeedback: Cancelling Action!"); 
  //}
  
  double correction=-scan_vel/10.0;
  
  angle=feedback->actual.positions[0];
  if(servo_going_right) angle=angle+correction;
  //ROS_INFO_STREAM("Servo at " << angle);
  
  //Refresh laser frame
  this->JointState_msg_.header.stamp = ros::Time::now();
  this->JointState_msg_.position[0]=angle*TO_RAD;
  this->vertical_laser_pan_publisher_.publish(this->JointState_msg_);
  
  if(servo_going_right) angle=angle-correction;
  
  
  //ROS_INFO_STREAM("Actual servo angle: " << angle);
}

/*  [action requests] */
void TibiDaboLaser2dTo3dAlgNode::servo_angleMakeActionRequest() 
{ 
  //ROS_INFO("TibiDaboLaser2dTo3dAlgNode::servo_angleMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  servo_angle_client_.waitForServer();  
  //ROS_INFO("TibiDaboLaser2dTo3dAlgNode::servo_angleMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //servo_angle_goal_.data = my_desired_goal; 
  servo_angle_client_.sendGoal(servo_angle_goal_, 
              boost::bind(&TibiDaboLaser2dTo3dAlgNode::servo_angleDone,     this, _1, _2), 
              boost::bind(&TibiDaboLaser2dTo3dAlgNode::servo_angleActive,   this), 
              boost::bind(&TibiDaboLaser2dTo3dAlgNode::servo_angleFeedback, this, _1)); 
  //ROS_INFO("TibiDaboLaser2dTo3dAlgNode::servo_angleMakeActionRequest: Goal Sent. Wait for Result!"); 
}

void TibiDaboLaser2dTo3dAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  
  scan_vel=config.scan_velocity;
  minimum_time_between_scans = ros::Duration(1/900.0*scan_vel*scan_vel-7.0/60.0*scan_vel+3.5);
  angle_min=config.min_angle;
  angle_max=config.max_angle;

  this->alg_.unlock();
}

void TibiDaboLaser2dTo3dAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<TibiDaboLaser2dTo3dAlgNode>(argc, argv, "tibi_dabo_laser_2d_to_3d_alg_node");
}